Commit d17f10d2 authored by Beat Küng's avatar Beat Küng

AirMap: refactor for multi-vehicle support & base class API

- add multi-vehicle support: each vehicle has now a per-vehicle AirMap
  object that takes care of flight creation, traffic & telemetry
- add a base class API layer so that the interface between QGC and the
  AirMap implementation is clear, and easier to change
- removes polygon upload to vehicle (needs a better solution)
parent 3f64f28b
...@@ -505,7 +505,9 @@ HEADERS += \ ...@@ -505,7 +505,9 @@ HEADERS += \
src/MG.h \ src/MG.h \
src/MissionManager/CameraCalc.h \ src/MissionManager/CameraCalc.h \
src/MissionManager/AirMapController.h \ src/MissionManager/AirMapController.h \
src/MissionManager/AirspaceController.h \
src/MissionManager/AirMapManager.h \ src/MissionManager/AirMapManager.h \
src/MissionManager/AirspaceManagement.h \
src/MissionManager/CameraSection.h \ src/MissionManager/CameraSection.h \
src/MissionManager/CameraSpec.h \ src/MissionManager/CameraSpec.h \
src/MissionManager/ComplexMissionItem.h \ src/MissionManager/ComplexMissionItem.h \
...@@ -712,7 +714,9 @@ SOURCES += \ ...@@ -712,7 +714,9 @@ SOURCES += \
src/LogCompressor.cc \ src/LogCompressor.cc \
src/MissionManager/CameraCalc.cc \ src/MissionManager/CameraCalc.cc \
src/MissionManager/AirMapController.cc \ src/MissionManager/AirMapController.cc \
src/MissionManager/AirspaceController.cc \
src/MissionManager/AirMapManager.cc \ src/MissionManager/AirMapManager.cc \
src/MissionManager/AirspaceManagement.cc \
src/MissionManager/CameraSection.cc \ src/MissionManager/CameraSection.cc \
src/MissionManager/CameraSpec.cc \ src/MissionManager/CameraSpec.cc \
src/MissionManager/ComplexMissionItem.cc \ src/MissionManager/ComplexMissionItem.cc \
......
...@@ -615,11 +615,11 @@ QGCView { ...@@ -615,11 +615,11 @@ QGCView {
} }
} }
//-- AirMap Indicator //-- Airspace Indicator
Rectangle { Rectangle {
id: airMapIndicator id: airspaceIndicator
width: airMapRow.width + (ScreenTools.defaultFontPixelWidth * 3) width: airspaceRow.width + (ScreenTools.defaultFontPixelWidth * 3)
height: airMapRow.height * 1.25 height: airspaceRow.height * 1.25
color: qgcPal.globalTheme === QGCPalette.Light ? Qt.rgba(1,1,1,0.95) : Qt.rgba(0,0,0,0.75) color: qgcPal.globalTheme === QGCPalette.Light ? Qt.rgba(1,1,1,0.95) : Qt.rgba(0,0,0,0.75)
visible: _mainIsMap && flightPermit && flightPermit !== AirspaceAuthorization.PermitUnknown && !messageArea.visible && !criticalMmessageArea.visible visible: _mainIsMap && flightPermit && flightPermit !== AirspaceAuthorization.PermitUnknown && !messageArea.visible && !criticalMmessageArea.visible
radius: 3 radius: 3
...@@ -629,27 +629,27 @@ QGCView { ...@@ -629,27 +629,27 @@ QGCView {
anchors.topMargin: ScreenTools.toolbarHeight + (ScreenTools.defaultFontPixelHeight * 0.25) anchors.topMargin: ScreenTools.toolbarHeight + (ScreenTools.defaultFontPixelHeight * 0.25)
anchors.horizontalCenter: parent.horizontalCenter anchors.horizontalCenter: parent.horizontalCenter
Row { Row {
id: airMapRow id: airspaceRow
spacing: ScreenTools.defaultFontPixelWidth spacing: ScreenTools.defaultFontPixelWidth
anchors.centerIn: parent anchors.centerIn: parent
QGCLabel { text: qsTr("AirMap:"); anchors.verticalCenter: parent.verticalCenter; } QGCLabel { text: airspaceIndicator.providerName+":"; anchors.verticalCenter: parent.verticalCenter; }
QGCLabel { QGCLabel {
text: { text: {
if(airMapIndicator.flightPermit) { if(airspaceIndicator.flightPermit) {
if(airMapIndicator.flightPermit === AirspaceAuthorization.PermitPending) if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitPending)
return qsTr("Approval Pending") return qsTr("Approval Pending")
if(airMapIndicator.flightPermit === AirspaceAuthorization.PermitAccepted) if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitAccepted)
return qsTr("Flight Approved") return qsTr("Flight Approved")
if(airMapIndicator.flightPermit === AirspaceAuthorization.PermitRejected) if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitRejected)
return qsTr("Flight Denied") return qsTr("Flight Rejected")
} }
return "" return ""
} }
color: { color: {
if(airMapIndicator.flightPermit) { if(airspaceIndicator.flightPermit) {
if(airMapIndicator.flightPermit === AirspaceAuthorization.PermitPending) if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitPending)
return qgcPal.colorOrange return qgcPal.colorOrange
if(airMapIndicator.flightPermit === AirspaceAuthorization.PermitAccepted) if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitAccepted)
return qgcPal.colorGreen return qgcPal.colorGreen
} }
return qgcPal.colorRed return qgcPal.colorRed
...@@ -657,7 +657,8 @@ QGCView { ...@@ -657,7 +657,8 @@ QGCView {
anchors.verticalCenter: parent.verticalCenter; anchors.verticalCenter: parent.verticalCenter;
} }
} }
property var flightPermit: _activeVehicle ? _activeVehicle.airMapController.flightPermitStatus : null property var flightPermit: _activeVehicle ? _activeVehicle.flightPermitStatus : null
property var providerName: _activeVehicle ? _activeVehicle.airspaceController.providerName : ""
} }
} }
...@@ -59,7 +59,7 @@ FlightMap { ...@@ -59,7 +59,7 @@ FlightMap {
onZoomLevelChanged: QGroundControl.flightMapZoom = zoomLevel onZoomLevelChanged: QGroundControl.flightMapZoom = zoomLevel
onCenterChanged: { onCenterChanged: {
if(_activeVehicle) { if(_activeVehicle) {
_activeVehicle.airMapController.setROI(center, 5000) _activeVehicle.airspaceController.setROI(center, 5000)
} }
QGroundControl.flightMapPosition = center QGroundControl.flightMapPosition = center
} }
...@@ -329,9 +329,9 @@ FlightMap { ...@@ -329,9 +329,9 @@ FlightMap {
] ]
} }
// AirMap overlap support // Airspace overlap support
MapItemView { MapItemView {
model: _activeVehicle ? _activeVehicle.airMapController.circles : [] model: _activeVehicle ? _activeVehicle.airspaceController.circles : []
delegate: MapCircle { delegate: MapCircle {
center: object.center center: object.center
radius: object.radius radius: object.radius
...@@ -342,7 +342,7 @@ FlightMap { ...@@ -342,7 +342,7 @@ FlightMap {
} }
MapItemView { MapItemView {
model: _activeVehicle ? _activeVehicle.airMapController.polygons : [] model: _activeVehicle ? _activeVehicle.airspaceController.polygons : []
delegate: MapPolygon { delegate: MapPolygon {
border.color: "white" border.color: "white"
color: "yellow" color: "yellow"
......
This diff is collapsed.
...@@ -15,6 +15,7 @@ ...@@ -15,6 +15,7 @@
#include "QmlObjectListModel.h" #include "QmlObjectListModel.h"
#include "MissionItem.h" #include "MissionItem.h"
#include "MultiVehicleManager.h" #include "MultiVehicleManager.h"
#include "AirspaceManagement.h"
#include <qmqtt.h> #include <qmqtt.h>
...@@ -32,44 +33,6 @@ ...@@ -32,44 +33,6 @@
Q_DECLARE_LOGGING_CATEGORY(AirMapManagerLog) Q_DECLARE_LOGGING_CATEGORY(AirMapManagerLog)
class AirspaceRestriction : public QObject
{
Q_OBJECT
public:
AirspaceRestriction(QObject* parent = NULL);
};
class PolygonAirspaceRestriction : public AirspaceRestriction
{
Q_OBJECT
public:
PolygonAirspaceRestriction(const QVariantList& polygon, QObject* parent = NULL);
Q_PROPERTY(QVariantList polygon MEMBER _polygon CONSTANT)
const QVariantList& getPolygon() const { return _polygon; }
private:
QVariantList _polygon;
};
class CircularAirspaceRestriction : public AirspaceRestriction
{
Q_OBJECT
public:
CircularAirspaceRestriction(const QGeoCoordinate& center, double radius, QObject* parent = NULL);
Q_PROPERTY(QGeoCoordinate center MEMBER _center CONSTANT)
Q_PROPERTY(double radius MEMBER _radius CONSTANT)
private:
QGeoCoordinate _center;
double _radius;
};
class AirMapLogin : public QObject class AirMapLogin : public QObject
{ {
...@@ -165,6 +128,8 @@ public: ...@@ -165,6 +128,8 @@ public:
*/ */
void abort(); void abort();
bool hasAPIKey() const { return _networkingData.airmapAPIKey != ""; }
signals: signals:
/// signal when the request finished (get or post). All requests are assumed to return JSON. /// signal when the request finished (get or post). All requests are assumed to return JSON.
void finished(QJsonParseError parseError, QJsonDocument document); void finished(QJsonParseError parseError, QJsonDocument document);
...@@ -196,16 +161,13 @@ private: ...@@ -196,16 +161,13 @@ private:
/// class to download polygons from AirMap /// class to download polygons from AirMap
class AirspaceRestrictionManager : public QObject class AirMapRestrictionManager : public AirspaceRestrictionProvider
{ {
Q_OBJECT Q_OBJECT
public: public:
AirspaceRestrictionManager(AirMapNetworking::SharedData& sharedData); AirMapRestrictionManager(AirMapNetworking::SharedData& sharedData);
void updateROI(const QGeoCoordinate& center, double radiusMeters);
QmlObjectListModel* polygonRestrictions(void) { return &_polygonList; } void setROI(const QGeoCoordinate& center, double radiusMeters) override;
QmlObjectListModel* circularRestrictions(void) { return &_circleList; }
signals: signals:
void networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage); void networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage);
...@@ -223,26 +185,9 @@ private: ...@@ -223,26 +185,9 @@ private:
State _state = State::Idle; State _state = State::Idle;
int _numAwaitingItems = 0; int _numAwaitingItems = 0;
AirMapNetworking _networking; AirMapNetworking _networking;
QmlObjectListModel _polygonList;
QmlObjectListModel _circleList;
QList<PolygonAirspaceRestriction*> _nextPolygonList;
QList<CircularAirspaceRestriction*> _nextcircleList;
}; };
class AirspaceAuthorization : public QObject {
Q_OBJECT
public:
enum PermitStatus {
PermitUnknown = 0,
PermitPending,
PermitAccepted,
PermitRejected,
};
Q_ENUMS(PermitStatus);
};
/// class to upload a flight /// class to upload a flight
class AirMapFlightManager : public QObject class AirMapFlightManager : public QObject
{ {
...@@ -344,6 +289,8 @@ public: ...@@ -344,6 +289,8 @@ public:
void stopTelemetryStream(); void stopTelemetryStream();
bool isTelemetryStreaming() const;
signals: signals:
void networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage); void networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage);
...@@ -360,8 +307,6 @@ private: ...@@ -360,8 +307,6 @@ private:
void _handleGlobalPositionInt(const mavlink_message_t& message); void _handleGlobalPositionInt(const mavlink_message_t& message);
void _handleGPSRawInt(const mavlink_message_t& message); void _handleGPSRawInt(const mavlink_message_t& message);
bool _isTelemetryStreaming() const;
enum class State { enum class State {
Idle, Idle,
StartCommunication, StartCommunication,
...@@ -418,68 +363,74 @@ private: ...@@ -418,68 +363,74 @@ private:
/// AirMap per vehicle management class.
/// AirMap server communication support. class AirMapManagerPerVehicle : public AirspaceManagerPerVehicle
class AirMapManager : public QGCTool
{ {
Q_OBJECT Q_OBJECT
public: public:
AirMapManager(QGCApplication* app, QGCToolbox* toolbox); AirMapManagerPerVehicle(AirMapNetworking::SharedData& sharedData, const Vehicle& vehicle, QGCToolbox& toolbox);
~AirMapManager(); virtual ~AirMapManagerPerVehicle() = default;
/// Set the ROI for airspace information
/// @param center Center coordinate for ROI
/// @param radiusMeters Radius in meters around center which is of interest
void setROI(QGeoCoordinate& center, double radiusMeters);
/// Send flight path to AirMap void createFlight(const QList<MissionItem*>& missionItems) override;
void createFlight(const QList<MissionItem*>& missionItems);
AirspaceAuthorization::PermitStatus flightPermitStatus() const override;
QmlObjectListModel* polygonRestrictions(void) { return _airspaceRestrictionManager.polygonRestrictions(); } void startTelemetryStream() override;
QmlObjectListModel* circularRestrictions(void) { return _airspaceRestrictionManager.circularRestrictions(); }
void setToolbox(QGCToolbox* toolbox) override; void stopTelemetryStream() override;
AirspaceAuthorization::PermitStatus flightPermitStatus() const { return _flightManager.flightPermitStatus(); } bool isTelemetryStreaming() const override;
signals: signals:
void flightPermitStatusChanged(); void networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage);
void trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading);
private slots: public slots:
void _updateToROI(void); void endFlight() override;
void _networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage);
void _activeVehicleChanged(Vehicle* activeVehicle); void settingsChanged();
void _vehicleArmedChanged(bool armed);
protected slots:
virtual void vehicleMavlinkMessageReceived(const mavlink_message_t& message) override;
private slots:
void _flightPermitStatusChanged(); void _flightPermitStatusChanged();
void _settingsChanged();
private: private:
bool _hasAPIKey() const { return _networkingData.airmapAPIKey != ""; } AirMapNetworking _networking;
/**
* A new vehicle got connected, listen to its state
*/
void _connectVehicle(Vehicle* vehicle);
AirMapNetworking::SharedData _networkingData;
AirspaceRestrictionManager _airspaceRestrictionManager;
AirMapFlightManager _flightManager; AirMapFlightManager _flightManager;
AirMapTelemetry _telemetry; AirMapTelemetry _telemetry;
AirMapTrafficAlertClient _trafficAlerts; AirMapTrafficAlertClient _trafficAlerts;
QGeoCoordinate _roiCenter; QGCToolbox& _toolbox;
double _roiRadius; };
QTimer _updateTimer; class AirMapManager : public AirspaceManager
{
Q_OBJECT
public:
AirMapManager(QGCApplication* app, QGCToolbox* toolbox);
virtual ~AirMapManager() = default;
void setToolbox(QGCToolbox* toolbox) override;
AirspaceManagerPerVehicle* instantiateVehicle(const Vehicle& vehicle) override;
AirspaceRestrictionProvider* instantiateRestrictionProvider() override;
QString name() const override { return "AirMap"; }
Vehicle* _vehicle = nullptr; ///< current vehicle signals:
bool _vehicleWasInMissionMode = false; ///< true if the vehicle was in mission mode when arming void settingsChanged();
private slots:
void _networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage);
void _settingsChanged();
private:
AirMapNetworking::SharedData _networkingData;
}; };
#endif #endif
...@@ -7,21 +7,14 @@ ...@@ -7,21 +7,14 @@
* *
****************************************************************************/ ****************************************************************************/
#include "AirMapController.h" #include "AirspaceController.h"
#include "AirMapManager.h" #include "AirspaceManagement.h"
#include "QGCApplication.h" #include "QGCApplication.h"
#include "QGCQGeoCoordinate.h" #include "QGCQGeoCoordinate.h"
QGC_LOGGING_CATEGORY(AirMapControllerLog, "AirMapControllerLog") AirspaceController::AirspaceController(QObject* parent)
AirMapController::AirMapController(QObject* parent)
: QObject(parent) : QObject(parent)
, _manager(qgcApp()->toolbox()->airMapManager()) , _manager(qgcApp()->toolbox()->airspaceManager())
{ {
connect(_manager, &AirMapManager::flightPermitStatusChanged, this, &AirMapController::flightPermitStatusChanged);
} }
AirMapController::~AirMapController()
{
}
...@@ -7,44 +7,30 @@ ...@@ -7,44 +7,30 @@
* *
****************************************************************************/ ****************************************************************************/
#ifndef AirMapController_H #pragma once
#define AirMapController_H
#include "AirMapManager.h" #include "AirspaceManagement.h"
#include "QGCMapPolygon.h" #include "QGCMapPolygon.h"
#include "QGCLoggingCategory.h"
Q_DECLARE_LOGGING_CATEGORY(AirMapControllerLog) class AirspaceController : public QObject
class AirMapManager;
class AirMapController : public QObject
{ {
Q_OBJECT Q_OBJECT
public: public:
AirMapController(QObject* parent = NULL); AirspaceController(QObject* parent = NULL);
~AirMapController(); ~AirspaceController() = default;
Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT) ///< List of PolygonAirspaceRestriction objects Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT) ///< List of PolygonAirspaceRestriction objects
Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT) ///< List of CircularAirspaceRestriction objects Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT) ///< List of CircularAirspaceRestriction objects
Q_PROPERTY(AirspaceAuthorization::PermitStatus flightPermitStatus READ flightPermitStatus NOTIFY flightPermitStatusChanged) ///< state of flight permission
Q_INVOKABLE void setROI(QGeoCoordinate center, double radius) { _manager->setROI(center, radius); } Q_INVOKABLE void setROI(QGeoCoordinate center, double radius) { _manager->setROI(center, radius); }
QmlObjectListModel* polygons(void) { return _manager->polygonRestrictions(); } QmlObjectListModel* polygons() { return _manager->polygonRestrictions(); }
QmlObjectListModel* circles(void) { return _manager->circularRestrictions(); } QmlObjectListModel* circles() { return _manager->circularRestrictions(); }
AirspaceAuthorization::PermitStatus flightPermitStatus() const { return _manager->flightPermitStatus(); } Q_PROPERTY(QString providerName READ providerName CONSTANT)
signals:
void flightPermitStatusChanged();
QString providerName() { return _manager->name(); }
private: private:
AirMapManager* _manager; AirspaceManager* _manager;
QmlObjectListModel _polygonList;
QmlObjectListModel _circleList;
}; };
#endif
/****************************************************************************
*
* (c) 2017 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "AirspaceManagement.h"
#include <Vehicle.h>
QGC_LOGGING_CATEGORY(AirspaceManagementLog, "AirspaceManagementLog")
AirspaceRestriction::AirspaceRestriction(QObject* parent)
: QObject(parent)
{
}
PolygonAirspaceRestriction::PolygonAirspaceRestriction(const QVariantList& polygon, QObject* parent)
: AirspaceRestriction(parent)
, _polygon(polygon)
{
}
CircularAirspaceRestriction::CircularAirspaceRestriction(const QGeoCoordinate& center, double radius, QObject* parent)
: AirspaceRestriction(parent)
, _center(center)
, _radius(radius)
{
}
AirspaceManager::AirspaceManager(QGCApplication* app, QGCToolbox* toolbox)
: QGCTool(app, toolbox)
{
_roiUpdateTimer.setInterval(2000);
_roiUpdateTimer.setSingleShot(true);
connect(&_roiUpdateTimer, &QTimer::timeout, this, &AirspaceManager::_updateToROI);
qmlRegisterUncreatableType<AirspaceAuthorization>("QGroundControl", 1, 0, "AirspaceAuthorization", "Reference only");
}
AirspaceManager::~AirspaceManager()
{
if (_restrictionsProvider) {
delete _restrictionsProvider;
}
_polygonRestrictions.clearAndDeleteContents();
_circleRestrictions.clearAndDeleteContents();
}
void AirspaceManager::setToolbox(QGCToolbox* toolbox)
{
QGCTool::setToolbox(toolbox);
// we should not call virtual methods in the constructor, so we instantiate the restriction provider here
_restrictionsProvider = instantiateRestrictionProvider();
if (_restrictionsProvider) {
connect(_restrictionsProvider, &AirspaceRestrictionProvider::requestDone, this,
&AirspaceManager::_restrictionsUpdated);
}
}
void AirspaceManager::setROI(const QGeoCoordinate& center, double radiusMeters)
{
_roiCenter = center;
_roiRadius = radiusMeters;
_roiUpdateTimer.start();
}
void AirspaceManager::_updateToROI()
{
if (_restrictionsProvider) {
_restrictionsProvider->setROI(_roiCenter, _roiRadius);
}
}
void AirspaceManager::_restrictionsUpdated(bool success)
{
_polygonRestrictions.clearAndDeleteContents();
_circleRestrictions.clearAndDeleteContents();
if (success) {
for (const auto& circle : _restrictionsProvider->circles()) {
_circleRestrictions.append(circle);
}
for (const auto& polygon : _restrictionsProvider->polygons()) {
_polygonRestrictions.append(polygon);
}
} else {
// TODO: show error?
}
}
AirspaceManagerPerVehicle::AirspaceManagerPerVehicle(const Vehicle& vehicle)
: _vehicle(vehicle)
{
connect(&_vehicle, &Vehicle::armedChanged, this, &AirspaceManagerPerVehicle::_vehicleArmedChanged);
connect(&_vehicle, &Vehicle::mavlinkMessageReceived, this, &AirspaceManagerPerVehicle::vehicleMavlinkMessageReceived);
}
void AirspaceManagerPerVehicle::_vehicleArmedChanged(bool armed)
{
if (armed) {
startTelemetryStream();
_vehicleWasInMissionMode = _vehicle.flightMode() == _vehicle.missionFlightMode();
} else {
stopTelemetryStream();
// end the flight if we were in mission mode during arming or disarming
// TODO: needs to be improved. for instance if we do RTL and then want to continue the mission...
if (_vehicleWasInMissionMode || _vehicle.flightMode() == _vehicle.missionFlightMode()) {
endFlight();
}
}
}
/****************************************************************************
*
* (c) 2017 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once
/**
* @file AirspaceManagement.h
* This file contains the interface definitions used by an airspace management implementation (AirMap).
* There are 3 base classes that must be subclassed:
* - AirspaceManager
* main manager that contains the restrictions for display. It acts as a factory to create instances of the other
* classes.
* - AirspaceManagerPerVehicle
* this provides the multi-vehicle support - each vehicle has an instance
* - AirspaceRestrictionProvider
* provides airspace restrictions. Currently only used by AirspaceManager, but
* each vehicle could have its own restrictions.
*/
#include "QGCToolbox.h"
#include "MissionItem.h"
#include <QObject>
#include <QString>
#include <QList>
#include <QGCLoggingCategory.h>
Q_DECLARE_LOGGING_CATEGORY(AirspaceManagementLog)
/**
* Contains the status of the Airspace authorization
*/
class AirspaceAuthorization : public QObject {
Q_OBJECT
public:
enum PermitStatus {
PermitUnknown = 0,
PermitPending,
PermitAccepted,
PermitRejected,
};
Q_ENUMS(PermitStatus);
};
/**
* Base class for an airspace restriction
*/
class AirspaceRestriction : public QObject
{
Q_OBJECT
public:
AirspaceRestriction(QObject* parent = NULL);
};
class PolygonAirspaceRestriction : public AirspaceRestriction
{
Q_OBJECT
public:
PolygonAirspaceRestriction(const QVariantList& polygon, QObject* parent = NULL);
Q_PROPERTY(QVariantList polygon MEMBER _polygon CONSTANT)
const QVariantList& getPolygon() const { return _polygon; }
private:
QVariantList _polygon;
};
class CircularAirspaceRestriction : public AirspaceRestriction
{
Q_OBJECT
public:
CircularAirspaceRestriction(const QGeoCoordinate& center, double radius, QObject* parent = NULL);
Q_PROPERTY(QGeoCoordinate center MEMBER _center CONSTANT)
Q_PROPERTY(double radius MEMBER _radius CONSTANT)
private:
QGeoCoordinate _center;
double _radius;
};
/**
* @class AirspaceRestrictionProvider
* Base class that queries for airspace restrictions
*/
class AirspaceRestrictionProvider : public QObject {
Q_OBJECT
public:
AirspaceRestrictionProvider() = default;
~AirspaceRestrictionProvider() = default;
/**
* Set region of interest that should be queried. When finished, the requestDone() signal will be emmited.
* @param center Center coordinate for ROI
* @param radiusMeters Radius in meters around center which is of interest
*/
virtual void setROI(const QGeoCoordinate& center, double radiusMeters) = 0;
const QList<PolygonAirspaceRestriction*>& polygons() const { return _polygonList; }
const QList<CircularAirspaceRestriction*>& circles() const { return _circleList; }
signals:
void requestDone(bool success);
protected:
QList<PolygonAirspaceRestriction*> _polygonList;
QList<CircularAirspaceRestriction*> _circleList;
};
class AirspaceManagerPerVehicle;
class Vehicle;
/**
* @class AirspaceManager
* Base class for airspace management. There is one (global) instantiation of this
*/
class AirspaceManager : public QGCTool {
Q_OBJECT
public:
AirspaceManager(QGCApplication* app, QGCToolbox* toolbox);
virtual ~AirspaceManager();
/**
* Factory method to create an AirspaceManagerPerVehicle object
*/
virtual AirspaceManagerPerVehicle* instantiateVehicle(const Vehicle& vehicle) = 0;
/**
*
* Factory method to create an AirspaceRestrictionProvider object
*/
virtual AirspaceRestrictionProvider* instantiateRestrictionProvider() = 0;
/**
* Set the ROI for airspace information (restrictions shown in UI)
* @param center Center coordinate for ROI
* @param radiusMeters Radius in meters around center which is of interest
*/
void setROI(const QGeoCoordinate& center, double radiusMeters);
QmlObjectListModel* polygonRestrictions(void) { return &_polygonRestrictions; }
QmlObjectListModel* circularRestrictions(void) { return &_circleRestrictions; }
void setToolbox(QGCToolbox* toolbox) override;
/**
* Name of the airspace management provider (used in the UI)
*/
virtual QString name() const = 0;
private slots:
void _restrictionsUpdated(bool success);
private:
void _updateToROI();
AirspaceRestrictionProvider* _restrictionsProvider = nullptr; ///< restrictions that are shown in the UI
QmlObjectListModel _polygonRestrictions; ///< current polygon restrictions
QmlObjectListModel _circleRestrictions; ///< current circle restrictions
QTimer _roiUpdateTimer;
QGeoCoordinate _roiCenter;
double _roiRadius;
};
/**
* @class AirspaceManagerPerVehicle
* Base class for per-vehicle management (each vehicle has one (or zero) of these)
*/
class AirspaceManagerPerVehicle : public QObject {
Q_OBJECT
public:
AirspaceManagerPerVehicle(const Vehicle& vehicle);
virtual ~AirspaceManagerPerVehicle() = default;
/**
* create/upload a flight from a mission. This should update the flight permit status.
* There can only be one active flight for each vehicle.
*/
virtual void createFlight(const QList<MissionItem*>& missionItems) = 0;
/**
* get the current flight permit status
*/
virtual AirspaceAuthorization::PermitStatus flightPermitStatus() const = 0;
/**
* Setup the connection and start sending telemetry
*/
virtual void startTelemetryStream() = 0;
virtual void stopTelemetryStream() = 0;
virtual bool isTelemetryStreaming() const = 0;
public slots:
virtual void endFlight() = 0;
signals:
void trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading);
void flightPermitStatusChanged();
protected slots:
virtual void vehicleMavlinkMessageReceived(const mavlink_message_t& message) = 0;
protected:
const Vehicle& _vehicle;
private slots:
void _vehicleArmedChanged(bool armed);
private:
bool _vehicleWasInMissionMode = false; ///< true if the vehicle was in mission mode when arming
};
...@@ -21,7 +21,7 @@ GeoFenceManager::GeoFenceManager(Vehicle* vehicle) ...@@ -21,7 +21,7 @@ GeoFenceManager::GeoFenceManager(Vehicle* vehicle)
: _vehicle (vehicle) : _vehicle (vehicle)
, _planManager (vehicle, MAV_MISSION_TYPE_FENCE) , _planManager (vehicle, MAV_MISSION_TYPE_FENCE)
, _firstParamLoadComplete (false) , _firstParamLoadComplete (false)
, _airmapManager (qgcApp()->toolbox()->airMapManager()) , _airspaceManager (qgcApp()->toolbox()->airspaceManager())
{ {
connect(&_planManager, &PlanManager::inProgressChanged, this, &GeoFenceManager::inProgressChanged); connect(&_planManager, &PlanManager::inProgressChanged, this, &GeoFenceManager::inProgressChanged);
connect(&_planManager, &PlanManager::error, this, &GeoFenceManager::error); connect(&_planManager, &PlanManager::error, this, &GeoFenceManager::error);
...@@ -101,31 +101,6 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, ...@@ -101,31 +101,6 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn,
fenceItems.append(item); fenceItems.append(item);
} }
// send AirMap polygons
const QmlObjectListModel& airmapPolygons = *_airmapManager->polygonRestrictions();
for (int i = 0; i < airmapPolygons.count(); ++i) {
PolygonAirspaceRestriction *polygon = (PolygonAirspaceRestriction*)airmapPolygons[i];
int polygonCount = polygon->getPolygon().count() - 1; // last vertex is equal to the first
for (int j = 0; j < polygonCount; ++j) {
const QGeoCoordinate& vertex = polygon->getPolygon()[j].value<QGeoCoordinate>();
MissionItem* item = new MissionItem(0,
MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
MAV_FRAME_GLOBAL,
polygonCount, // vertex count
0, 0, 0, // param 2-4 unused
vertex.latitude(),
vertex.longitude(),
0, // param 7 unused
false, // autocontinue
false, // isCurrentItem
this); // parent
fenceItems.append(item);
}
}
// TODO: circles too
_planManager.writeMissionItems(fenceItems); _planManager.writeMissionItems(fenceItems);
for (int i=0; i<fenceItems.count(); i++) { for (int i=0; i<fenceItems.count(); i++) {
......
...@@ -13,7 +13,7 @@ ...@@ -13,7 +13,7 @@
#include <QObject> #include <QObject>
#include <QGeoCoordinate> #include <QGeoCoordinate>
#include "AirMapManager.h" #include "AirspaceManagement.h"
#include "QGCLoggingCategory.h" #include "QGCLoggingCategory.h"
#include "FactSystem.h" #include "FactSystem.h"
#include "PlanManager.h" #include "PlanManager.h"
...@@ -97,7 +97,7 @@ private: ...@@ -97,7 +97,7 @@ private:
bool _firstParamLoadComplete; bool _firstParamLoadComplete;
QList<QGCFencePolygon> _sendPolygons; QList<QGCFencePolygon> _sendPolygons;
QList<QGCFenceCircle> _sendCircles; QList<QGCFenceCircle> _sendCircles;
AirMapManager* _airmapManager; AirspaceManager* _airspaceManager;
}; };
#endif #endif
...@@ -18,7 +18,6 @@ ...@@ -18,7 +18,6 @@
#include "QGCApplication.h" #include "QGCApplication.h"
#include "MissionCommandTree.h" #include "MissionCommandTree.h"
#include "MissionCommandUIInfo.h" #include "MissionCommandUIInfo.h"
#include "AirMapManager.h"
QGC_LOGGING_CATEGORY(PlanManagerLog, "PlanManagerLog") QGC_LOGGING_CATEGORY(PlanManagerLog, "PlanManagerLog")
...@@ -80,9 +79,10 @@ void PlanManager::writeMissionItems(const QList<MissionItem*>& missionItems) ...@@ -80,9 +79,10 @@ void PlanManager::writeMissionItems(const QList<MissionItem*>& missionItems)
} }
if (_planType == MAV_MISSION_TYPE_MISSION) { if (_planType == MAV_MISSION_TYPE_MISSION) {
AirMapManager *airmapManager = qgcApp()->toolbox()->airMapManager(); // upload the flight to the airspace management backend
if(airmapManager) { AirspaceManagerPerVehicle* airspaceManager = _vehicle->airspaceManager();
airmapManager->createFlight(missionItems); if (airspaceManager) {
airspaceManager->createFlight(missionItems);
} }
} }
......
...@@ -84,6 +84,7 @@ ...@@ -84,6 +84,7 @@
#include "CameraCalc.h" #include "CameraCalc.h"
#include "VisualMissionItem.h" #include "VisualMissionItem.h"
#include "AirMapController.h" #include "AirMapController.h"
#include "AirspaceController.h"
#ifndef NO_SERIAL_LINK #ifndef NO_SERIAL_LINK
#include "SerialLink.h" #include "SerialLink.h"
...@@ -362,6 +363,7 @@ void QGCApplication::_initCommon(void) ...@@ -362,6 +363,7 @@ void QGCApplication::_initCommon(void)
qmlRegisterUncreatableType<QGCCameraManager> ("QGroundControl.Vehicle", 1, 0, "QGCCameraManager", "Reference only"); qmlRegisterUncreatableType<QGCCameraManager> ("QGroundControl.Vehicle", 1, 0, "QGCCameraManager", "Reference only");
qmlRegisterUncreatableType<QGCCameraControl> ("QGroundControl.Vehicle", 1, 0, "QGCCameraControl", "Reference only"); qmlRegisterUncreatableType<QGCCameraControl> ("QGroundControl.Vehicle", 1, 0, "QGCCameraControl", "Reference only");
qmlRegisterUncreatableType<AirMapController> ("QGroundControl.Vehicle", 1, 0, "AirMapController", "Reference only"); qmlRegisterUncreatableType<AirMapController> ("QGroundControl.Vehicle", 1, 0, "AirMapController", "Reference only");
qmlRegisterUncreatableType<AirspaceController> ("QGroundControl.Vehicle", 1, 0, "AirspaceController", "Reference only");
qmlRegisterUncreatableType<JoystickManager> ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only"); qmlRegisterUncreatableType<JoystickManager> ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only");
qmlRegisterUncreatableType<Joystick> ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only"); qmlRegisterUncreatableType<Joystick> ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only");
qmlRegisterUncreatableType<QGCPositionManager> ("QGroundControl.QGCPositionManager", 1, 0, "QGCPositionManager", "Reference only"); qmlRegisterUncreatableType<QGCPositionManager> ("QGroundControl.QGCPositionManager", 1, 0, "QGCPositionManager", "Reference only");
......
...@@ -57,7 +57,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app) ...@@ -57,7 +57,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app)
, _mavlinkLogManager (NULL) , _mavlinkLogManager (NULL)
, _corePlugin (NULL) , _corePlugin (NULL)
, _settingsManager (NULL) , _settingsManager (NULL)
, _airMapManager (NULL) , _airspaceManager (NULL)
{ {
// SettingsManager must be first so settings are available to any subsequent tools // SettingsManager must be first so settings are available to any subsequent tools
_settingsManager = new SettingsManager(app, this); _settingsManager = new SettingsManager(app, this);
...@@ -82,7 +82,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app) ...@@ -82,7 +82,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app)
_followMe = new FollowMe (app, this); _followMe = new FollowMe (app, this);
_videoManager = new VideoManager (app, this); _videoManager = new VideoManager (app, this);
_mavlinkLogManager = new MAVLinkLogManager (app, this); _mavlinkLogManager = new MAVLinkLogManager (app, this);
_airMapManager = new AirMapManager (app, this); _airspaceManager = new AirMapManager (app, this);
} }
void QGCToolbox::setChildToolboxes(void) void QGCToolbox::setChildToolboxes(void)
...@@ -109,7 +109,7 @@ void QGCToolbox::setChildToolboxes(void) ...@@ -109,7 +109,7 @@ void QGCToolbox::setChildToolboxes(void)
_qgcPositionManager->setToolbox(this); _qgcPositionManager->setToolbox(this);
_videoManager->setToolbox(this); _videoManager->setToolbox(this);
_mavlinkLogManager->setToolbox(this); _mavlinkLogManager->setToolbox(this);
_airMapManager->setToolbox(this); _airspaceManager->setToolbox(this);
} }
void QGCToolbox::_scanAndLoadPlugins(QGCApplication* app) void QGCToolbox::_scanAndLoadPlugins(QGCApplication* app)
......
...@@ -32,7 +32,7 @@ class VideoManager; ...@@ -32,7 +32,7 @@ class VideoManager;
class MAVLinkLogManager; class MAVLinkLogManager;
class QGCCorePlugin; class QGCCorePlugin;
class SettingsManager; class SettingsManager;
class AirMapManager; class AirspaceManager;
/// This is used to manage all of our top level services/tools /// This is used to manage all of our top level services/tools
class QGCToolbox : public QObject { class QGCToolbox : public QObject {
...@@ -57,7 +57,7 @@ public: ...@@ -57,7 +57,7 @@ public:
MAVLinkLogManager* mavlinkLogManager(void) { return _mavlinkLogManager; } MAVLinkLogManager* mavlinkLogManager(void) { return _mavlinkLogManager; }
QGCCorePlugin* corePlugin(void) { return _corePlugin; } QGCCorePlugin* corePlugin(void) { return _corePlugin; }
SettingsManager* settingsManager(void) { return _settingsManager; } SettingsManager* settingsManager(void) { return _settingsManager; }
AirMapManager* airMapManager(void) { return _airMapManager; } AirspaceManager* airspaceManager(void) { return _airspaceManager; }
#ifndef __mobile__ #ifndef __mobile__
GPSManager* gpsManager(void) { return _gpsManager; } GPSManager* gpsManager(void) { return _gpsManager; }
...@@ -88,7 +88,7 @@ private: ...@@ -88,7 +88,7 @@ private:
MAVLinkLogManager* _mavlinkLogManager; MAVLinkLogManager* _mavlinkLogManager;
QGCCorePlugin* _corePlugin; QGCCorePlugin* _corePlugin;
SettingsManager* _settingsManager; SettingsManager* _settingsManager;
AirMapManager* _airMapManager; AirspaceManager* _airspaceManager;
friend class QGCApplication; friend class QGCApplication;
}; };
......
...@@ -34,6 +34,7 @@ ...@@ -34,6 +34,7 @@
#include "ADSBVehicle.h" #include "ADSBVehicle.h"
#include "QGCCameraManager.h" #include "QGCCameraManager.h"
#include "AirMapController.h" #include "AirMapController.h"
#include "AirspaceController.h"
QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")
...@@ -132,7 +133,8 @@ Vehicle::Vehicle(LinkInterface* link, ...@@ -132,7 +133,8 @@ Vehicle::Vehicle(LinkInterface* link,
, _rallyPointManager(NULL) , _rallyPointManager(NULL)
, _rallyPointManagerInitialRequestSent(false) , _rallyPointManagerInitialRequestSent(false)
, _parameterManager(NULL) , _parameterManager(NULL)
, _airMapController(NULL) , _airspaceController(NULL)
, _airspaceManagerPerVehicle(NULL)
, _armed(false) , _armed(false)
, _base_mode(0) , _base_mode(0)
, _custom_mode(0) , _custom_mode(0)
...@@ -255,8 +257,17 @@ Vehicle::Vehicle(LinkInterface* link, ...@@ -255,8 +257,17 @@ Vehicle::Vehicle(LinkInterface* link,
_adsbTimer.setSingleShot(false); _adsbTimer.setSingleShot(false);
_adsbTimer.start(1000); _adsbTimer.start(1000);
_airMapController = new AirMapController(this); _airspaceController = new AirspaceController(this);
connect(qgcApp()->toolbox()->airMapManager(), &AirMapManager::trafficUpdate, this, &Vehicle::_trafficUpdate);
AirspaceManager* airspaceManager = _toolbox->airspaceManager();
if (airspaceManager) {
_airspaceManagerPerVehicle = airspaceManager->instantiateVehicle(*this);
if (_airspaceManagerPerVehicle) {
connect(_airspaceManagerPerVehicle, &AirspaceManagerPerVehicle::trafficUpdate, this, &Vehicle::_trafficUpdate);
connect(_airspaceManagerPerVehicle, &AirspaceManagerPerVehicle::flightPermitStatusChanged, this, &Vehicle::flightPermitStatusChanged);
}
}
} }
// Disconnected Vehicle for offline editing // Disconnected Vehicle for offline editing
...@@ -315,7 +326,8 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, ...@@ -315,7 +326,8 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _rallyPointManager(NULL) , _rallyPointManager(NULL)
, _rallyPointManagerInitialRequestSent(false) , _rallyPointManagerInitialRequestSent(false)
, _parameterManager(NULL) , _parameterManager(NULL)
, _airMapController(NULL) , _airspaceController(NULL)
, _airspaceManagerPerVehicle(NULL)
, _armed(false) , _armed(false)
, _base_mode(0) , _base_mode(0)
, _custom_mode(0) , _custom_mode(0)
...@@ -445,6 +457,9 @@ Vehicle::~Vehicle() ...@@ -445,6 +457,9 @@ Vehicle::~Vehicle()
delete _mav; delete _mav;
_mav = NULL; _mav = NULL;
if (_airspaceManagerPerVehicle) {
delete _airspaceManagerPerVehicle;
}
} }
void Vehicle::prepareDelete() void Vehicle::prepareDelete()
......
...@@ -20,6 +20,7 @@ ...@@ -20,6 +20,7 @@
#include "MAVLinkProtocol.h" #include "MAVLinkProtocol.h"
#include "UASMessageHandler.h" #include "UASMessageHandler.h"
#include "SettingsFact.h" #include "SettingsFact.h"
#include <AirspaceManagement.h>
class UAS; class UAS;
class UASInterface; class UASInterface;
...@@ -36,6 +37,7 @@ class SettingsManager; ...@@ -36,6 +37,7 @@ class SettingsManager;
class ADSBVehicle; class ADSBVehicle;
class QGCCameraManager; class QGCCameraManager;
class AirMapController; class AirMapController;
class AirspaceController;
Q_DECLARE_LOGGING_CATEGORY(VehicleLog) Q_DECLARE_LOGGING_CATEGORY(VehicleLog)
...@@ -322,6 +324,8 @@ public: ...@@ -322,6 +324,8 @@ public:
Q_PROPERTY(QGCCameraManager* dynamicCameras READ dynamicCameras NOTIFY dynamicCamerasChanged) Q_PROPERTY(QGCCameraManager* dynamicCameras READ dynamicCameras NOTIFY dynamicCamerasChanged)
Q_PROPERTY(QString hobbsMeter READ hobbsMeter NOTIFY hobbsMeterChanged) Q_PROPERTY(QString hobbsMeter READ hobbsMeter NOTIFY hobbsMeterChanged)
Q_PROPERTY(AirMapController* airMapController READ airMapController CONSTANT) Q_PROPERTY(AirMapController* airMapController READ airMapController CONSTANT)
Q_PROPERTY(AirspaceAuthorization::PermitStatus flightPermitStatus READ flightPermitStatus NOTIFY flightPermitStatusChanged) ///< state of flight permission
Q_PROPERTY(AirspaceController* airspaceController READ airspaceController CONSTANT)
// Vehicle state used for guided control // Vehicle state used for guided control
Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying
...@@ -537,7 +541,7 @@ public: ...@@ -537,7 +541,7 @@ public:
QmlObjectListModel* cameraTriggerPoints(void) { return &_cameraTriggerPoints; } QmlObjectListModel* cameraTriggerPoints(void) { return &_cameraTriggerPoints; }
QmlObjectListModel* adsbVehicles(void) { return &_adsbVehicles; } QmlObjectListModel* adsbVehicles(void) { return &_adsbVehicles; }
AirMapController* airMapController() { return _airMapController; } AirspaceController* airspaceController() { return _airspaceController; }
int flowImageIndex() { return _flowImageIndex; } int flowImageIndex() { return _flowImageIndex; }
...@@ -718,6 +722,11 @@ public: ...@@ -718,6 +722,11 @@ public:
/// Vehicle is about to be deleted /// Vehicle is about to be deleted
void prepareDelete(); void prepareDelete();
AirspaceAuthorization::PermitStatus flightPermitStatus() const
{ return _airspaceManagerPerVehicle ? _airspaceManagerPerVehicle->flightPermitStatus() : AirspaceAuthorization::PermitUnknown; }
AirspaceManagerPerVehicle* airspaceManager() const { return _airspaceManagerPerVehicle; }
signals: signals:
void allLinksInactive(Vehicle* vehicle); void allLinksInactive(Vehicle* vehicle);
void coordinateChanged(QGeoCoordinate coordinate); void coordinateChanged(QGeoCoordinate coordinate);
...@@ -749,6 +758,8 @@ signals: ...@@ -749,6 +758,8 @@ signals:
void capabilitiesKnownChanged(bool capabilitiesKnown); void capabilitiesKnownChanged(bool capabilitiesKnown);
void initialPlanRequestCompleteChanged(bool initialPlanRequestComplete); void initialPlanRequestCompleteChanged(bool initialPlanRequestComplete);
void capabilityBitsChanged(uint64_t capabilityBits); void capabilityBitsChanged(uint64_t capabilityBits);
void flightPermitStatusChanged();
void messagesReceivedChanged (); void messagesReceivedChanged ();
void messagesSentChanged (); void messagesSentChanged ();
...@@ -997,7 +1008,8 @@ private: ...@@ -997,7 +1008,8 @@ private:
ParameterManager* _parameterManager; ParameterManager* _parameterManager;
AirMapController* _airMapController; AirspaceController* _airspaceController;
AirspaceManagerPerVehicle* _airspaceManagerPerVehicle;
bool _armed; ///< true: vehicle is armed bool _armed; ///< true: vehicle is armed
uint8_t _base_mode; ///< base_mode from HEARTBEAT uint8_t _base_mode; ///< base_mode from HEARTBEAT
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment