From d17f10d2a504641485706cf84c71ec162fd52ef8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Oct 2017 10:52:28 +0200 Subject: [PATCH] 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) --- qgroundcontrol.pro | 4 + src/FlightDisplay/FlightDisplayView.qml | 31 +- src/FlightDisplay/FlightDisplayViewMap.qml | 8 +- src/MissionManager/AirMapController.h | 50 --- src/MissionManager/AirMapManager.cc | 291 +++++++++--------- src/MissionManager/AirMapManager.h | 155 ++++------ ...MapController.cc => AirspaceController.cc} | 15 +- src/MissionManager/AirspaceController.h | 36 +++ src/MissionManager/AirspaceManagement.cc | 120 ++++++++ src/MissionManager/AirspaceManagement.h | 232 ++++++++++++++ src/MissionManager/GeoFenceManager.cc | 27 +- src/MissionManager/GeoFenceManager.h | 4 +- src/MissionManager/PlanManager.cc | 8 +- src/QGCApplication.cc | 2 + src/QGCToolbox.cc | 6 +- src/QGCToolbox.h | 6 +- src/Vehicle/Vehicle.cc | 23 +- src/Vehicle/Vehicle.h | 16 +- 18 files changed, 671 insertions(+), 363 deletions(-) delete mode 100644 src/MissionManager/AirMapController.h rename src/MissionManager/{AirMapController.cc => AirspaceController.cc} (53%) create mode 100644 src/MissionManager/AirspaceController.h create mode 100644 src/MissionManager/AirspaceManagement.cc create mode 100644 src/MissionManager/AirspaceManagement.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 3d8ebdc5f..bb62b9023 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -505,7 +505,9 @@ HEADERS += \ src/MG.h \ src/MissionManager/CameraCalc.h \ src/MissionManager/AirMapController.h \ + src/MissionManager/AirspaceController.h \ src/MissionManager/AirMapManager.h \ + src/MissionManager/AirspaceManagement.h \ src/MissionManager/CameraSection.h \ src/MissionManager/CameraSpec.h \ src/MissionManager/ComplexMissionItem.h \ @@ -712,7 +714,9 @@ SOURCES += \ src/LogCompressor.cc \ src/MissionManager/CameraCalc.cc \ src/MissionManager/AirMapController.cc \ + src/MissionManager/AirspaceController.cc \ src/MissionManager/AirMapManager.cc \ + src/MissionManager/AirspaceManagement.cc \ src/MissionManager/CameraSection.cc \ src/MissionManager/CameraSpec.cc \ src/MissionManager/ComplexMissionItem.cc \ diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index b00669416..b445a3a44 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -615,11 +615,11 @@ QGCView { } } - //-- AirMap Indicator + //-- Airspace Indicator Rectangle { - id: airMapIndicator - width: airMapRow.width + (ScreenTools.defaultFontPixelWidth * 3) - height: airMapRow.height * 1.25 + id: airspaceIndicator + width: airspaceRow.width + (ScreenTools.defaultFontPixelWidth * 3) + height: airspaceRow.height * 1.25 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 radius: 3 @@ -629,27 +629,27 @@ QGCView { anchors.topMargin: ScreenTools.toolbarHeight + (ScreenTools.defaultFontPixelHeight * 0.25) anchors.horizontalCenter: parent.horizontalCenter Row { - id: airMapRow + id: airspaceRow spacing: ScreenTools.defaultFontPixelWidth anchors.centerIn: parent - QGCLabel { text: qsTr("AirMap:"); anchors.verticalCenter: parent.verticalCenter; } + QGCLabel { text: airspaceIndicator.providerName+":"; anchors.verticalCenter: parent.verticalCenter; } QGCLabel { text: { - if(airMapIndicator.flightPermit) { - if(airMapIndicator.flightPermit === AirspaceAuthorization.PermitPending) + if(airspaceIndicator.flightPermit) { + if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitPending) return qsTr("Approval Pending") - if(airMapIndicator.flightPermit === AirspaceAuthorization.PermitAccepted) + if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitAccepted) return qsTr("Flight Approved") - if(airMapIndicator.flightPermit === AirspaceAuthorization.PermitRejected) - return qsTr("Flight Denied") + if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitRejected) + return qsTr("Flight Rejected") } return "" } color: { - if(airMapIndicator.flightPermit) { - if(airMapIndicator.flightPermit === AirspaceAuthorization.PermitPending) + if(airspaceIndicator.flightPermit) { + if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitPending) return qgcPal.colorOrange - if(airMapIndicator.flightPermit === AirspaceAuthorization.PermitAccepted) + if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitAccepted) return qgcPal.colorGreen } return qgcPal.colorRed @@ -657,7 +657,8 @@ QGCView { 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 : "" } } diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index f28964809..281d78967 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -59,7 +59,7 @@ FlightMap { onZoomLevelChanged: QGroundControl.flightMapZoom = zoomLevel onCenterChanged: { if(_activeVehicle) { - _activeVehicle.airMapController.setROI(center, 5000) + _activeVehicle.airspaceController.setROI(center, 5000) } QGroundControl.flightMapPosition = center } @@ -329,9 +329,9 @@ FlightMap { ] } - // AirMap overlap support + // Airspace overlap support MapItemView { - model: _activeVehicle ? _activeVehicle.airMapController.circles : [] + model: _activeVehicle ? _activeVehicle.airspaceController.circles : [] delegate: MapCircle { center: object.center radius: object.radius @@ -342,7 +342,7 @@ FlightMap { } MapItemView { - model: _activeVehicle ? _activeVehicle.airMapController.polygons : [] + model: _activeVehicle ? _activeVehicle.airspaceController.polygons : [] delegate: MapPolygon { border.color: "white" color: "yellow" diff --git a/src/MissionManager/AirMapController.h b/src/MissionManager/AirMapController.h deleted file mode 100644 index 53266726b..000000000 --- a/src/MissionManager/AirMapController.h +++ /dev/null @@ -1,50 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#ifndef AirMapController_H -#define AirMapController_H - -#include "AirMapManager.h" -#include "QGCMapPolygon.h" -#include "QGCLoggingCategory.h" - -Q_DECLARE_LOGGING_CATEGORY(AirMapControllerLog) - -class AirMapManager; - -class AirMapController : public QObject -{ - Q_OBJECT - -public: - AirMapController(QObject* parent = NULL); - ~AirMapController(); - - Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT) ///< List of PolygonAirspaceRestriction 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); } - - QmlObjectListModel* polygons(void) { return _manager->polygonRestrictions(); } - QmlObjectListModel* circles(void) { return _manager->circularRestrictions(); } - - AirspaceAuthorization::PermitStatus flightPermitStatus() const { return _manager->flightPermitStatus(); } - -signals: - void flightPermitStatusChanged(); - -private: - AirMapManager* _manager; - QmlObjectListModel _polygonList; - QmlObjectListModel _circleList; -}; - -#endif diff --git a/src/MissionManager/AirMapManager.cc b/src/MissionManager/AirMapManager.cc index c23fd589b..502268361 100644 --- a/src/MissionManager/AirMapManager.cc +++ b/src/MissionManager/AirMapManager.cc @@ -290,16 +290,22 @@ void AirMapNetworking::_requestFinished(void) -AirspaceRestrictionManager::AirspaceRestrictionManager(AirMapNetworking::SharedData& sharedData) +AirMapRestrictionManager::AirMapRestrictionManager(AirMapNetworking::SharedData& sharedData) : _networking(sharedData) { - connect(&_networking, &AirMapNetworking::finished, this, &AirspaceRestrictionManager::_parseAirspaceJson); - connect(&_networking, &AirMapNetworking::error, this, &AirspaceRestrictionManager::_error); + connect(&_networking, &AirMapNetworking::finished, this, &AirMapRestrictionManager::_parseAirspaceJson); + connect(&_networking, &AirMapNetworking::error, this, &AirMapRestrictionManager::_error); } -void AirspaceRestrictionManager::updateROI(const QGeoCoordinate& center, double radiusMeters) +void AirMapRestrictionManager::setROI(const QGeoCoordinate& center, double radiusMeters) { + if (!_networking.hasAPIKey()) { + qCDebug(AirMapManagerLog) << "API key not set. Not updating Airspace"; + return; + } + if (_state != State::Idle) { + qCWarning(AirMapManagerLog) << "AirMapRestrictionManager::updateROI: state not idle"; return; } @@ -340,6 +346,9 @@ void AirspaceRestrictionManager::updateROI(const QGeoCoordinate& center, double // polygonJson["coordinates"] = rgPolygon; // QJsonDocument polygonJsonDoc(polygonJson); + _polygonList.clear(); + _circleList.clear(); + // Build up the http query QUrlQuery airspaceQuery; @@ -356,7 +365,7 @@ void AirspaceRestrictionManager::updateROI(const QGeoCoordinate& center, double _networking.get(airMapAirspaceUrl); } -void AirspaceRestrictionManager::_parseAirspaceJson(QJsonParseError parseError, QJsonDocument airspaceDoc) +void AirMapRestrictionManager::_parseAirspaceJson(QJsonParseError parseError, QJsonDocument airspaceDoc) { Q_UNUSED(parseError); QJsonObject rootObject = airspaceDoc.object(); @@ -378,7 +387,11 @@ void AirspaceRestrictionManager::_parseAirspaceJson(QJsonParseError parseError, _networking.get(url); } _numAwaitingItems = advisorySet.size(); - _state = State::RetrieveItems; + if (_numAwaitingItems > 0) { + _state = State::RetrieveItems; + } else { + _state = State::Idle; + } } break; @@ -392,8 +405,42 @@ void AirspaceRestrictionManager::_parseAirspaceJson(QJsonParseError parseError, QString airspaceName(airspaceObject["name"].toString()); const QJsonObject& airspaceGeometry(airspaceObject["geometry"].toObject()); QString geometryType(airspaceGeometry["type"].toString()); + + const QJsonObject& airspaceProperties(airspaceObject["properties"].toObject()); + bool enabled = airspaceProperties["enabled"].toBool(true); + if (!enabled) { + continue; + } + qCDebug(AirMapManagerLog) << "Airspace ID:" << airspaceId << "name:" << airspaceName << "type:" << airspaceType << "geometry:" << geometryType; + // filter by start and end time + const QJsonValue& effectiveStart(airspaceProperties["effective_start"]); + QDateTime now = QDateTime::currentDateTimeUtc(); + if (!effectiveStart.isNull()) { + QDateTime d = QDateTime::fromString(effectiveStart.toString(), Qt::ISODate); + if (d > now.addSecs(3600)) { + qCDebug(AirMapManagerLog) << "effective start filter"; + continue; + } + } + const QJsonValue& effectiveEnd(airspaceProperties["effective_end"]); + if (!effectiveEnd.isNull()) { + QDateTime d = QDateTime::fromString(effectiveEnd.toString(), Qt::ISODate); + if (d < now.addSecs(-3600)) { + qCDebug(AirMapManagerLog) << "effective end filter"; + continue; + } + } + + int authorizationLevel = (int)(airspaceProperties["authorization_level"].toDouble(3.)+0.5); + // 1 == green + if (authorizationLevel <= 1 || authorizationLevel > 3) { + qCDebug(AirMapManagerLog) << "auth level filter"; + continue; + } + + if (geometryType == "Polygon") { const QJsonArray& airspaceCoordinates(airspaceGeometry["coordinates"].toArray()[0].toArray()); @@ -405,12 +452,29 @@ void AirspaceRestrictionManager::_parseAirspaceJson(QJsonParseError parseError, polygon.append(QVariant::fromValue(((QGCQGeoCoordinate*)list[i])->coordinate())); } list.clearAndDeleteContents(); - _nextPolygonList.append(new PolygonAirspaceRestriction(polygon)); + _polygonList.append(new PolygonAirspaceRestriction(polygon)); } else { //TODO qWarning() << errorString; } + } else if (geometryType == "MultiPolygon") { + // TODO: it's possible (?) that polygons contain holes. These need to be rendered properly + const QJsonArray& polygonArray = airspaceGeometry["coordinates"].toArray(); + for (int polygonIdx = 0; polygonIdx < polygonArray.count(); polygonIdx++) { + const QJsonArray& airspaceCoordinates(polygonArray[polygonIdx].toArray()[0].toArray()); + QString errorString; + QmlObjectListModel list; + if (JsonHelper::loadPolygon(airspaceCoordinates, list, this, errorString)) { + QVariantList polygon; + for (int i = 0; i < list.count(); ++i) { + polygon.append(QVariant::fromValue(((QGCQGeoCoordinate*)list[i])->coordinate())); + } + list.clearAndDeleteContents(); + _polygonList.append(new PolygonAirspaceRestriction(polygon)); + } + } + } else { // TODO: are there any circles? qWarning() << "Unknown geometry type:" << geometryType; @@ -418,18 +482,8 @@ void AirspaceRestrictionManager::_parseAirspaceJson(QJsonParseError parseError, } if (--_numAwaitingItems == 0) { - _polygonList.clearAndDeleteContents(); - _circleList.clearAndDeleteContents(); - for (const auto& circle : _nextcircleList) { - _circleList.append(circle); - } - _nextcircleList.clear(); - for (const auto& polygon : _nextPolygonList) { - _polygonList.append(polygon); - } - _nextPolygonList.clear(); - _state = State::Idle; + emit requestDone(true); } } break; @@ -453,15 +507,15 @@ void AirspaceRestrictionManager::_parseAirspaceJson(QJsonParseError parseError, // } } -void AirspaceRestrictionManager::_error(QNetworkReply::NetworkError code, const QString& errorString, +void AirMapRestrictionManager::_error(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage) { - qCWarning(AirMapManagerLog) << "AirspaceRestrictionManager::_error" << code << serverErrorMessage; + qCWarning(AirMapManagerLog) << "AirMapRestrictionManager::_error" << code << serverErrorMessage; if (_state == State::RetrieveItems) { if (--_numAwaitingItems == 0) { _state = State::Idle; - // TODO: handle properly: update _polygonList... + emit requestDone(false); } } else { _state = State::Idle; @@ -775,7 +829,7 @@ void AirMapFlightManager::_parseJson(QJsonParseError parseError, QJsonDocument d } if (authorizationsArray.size() == 0) { - // FIXME: AirMap did not finish the integration yet, thus the array is just empty + // if we don't get any authorizations, we assume it's accepted accepted = true; } @@ -884,14 +938,14 @@ void AirMapTelemetry::vehicleMavlinkMessageReceived(const mavlink_message_t& mes } -bool AirMapTelemetry::_isTelemetryStreaming() const +bool AirMapTelemetry::isTelemetryStreaming() const { return _state == State::Streaming && !_udpHost.isNull(); } void AirMapTelemetry::_handleGPSRawInt(const mavlink_message_t& message) { - if (!_isTelemetryStreaming()) { + if (!isTelemetryStreaming()) { return; } @@ -907,7 +961,7 @@ void AirMapTelemetry::_handleGPSRawInt(const mavlink_message_t& message) void AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message) { - if (!_isTelemetryStreaming()) { + if (!isTelemetryStreaming()) { return; } @@ -916,7 +970,7 @@ void AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message) qDebug() << "Got position from vehicle:" << globalPosition.lat << "," << globalPosition.lon; - // documentation: https://developers.airmap.com/docs/telemetry-2 + // documentation: https://developers.airmap.com/docs/sending-telemetry uint8_t output[512]; // assume the whole packet is not longer than this uint8_t payload[512]; @@ -1128,102 +1182,101 @@ void AirMapTrafficAlertClient::onReceived(const QMQTT::Message& message) } -AirMapManager::AirMapManager(QGCApplication* app, QGCToolbox* toolbox) - : QGCTool(app, toolbox), _airspaceRestrictionManager(_networkingData), _flightManager(_networkingData), - _telemetry(_networkingData), _trafficAlerts("mqtt-prod.airmap.io", 8883) +AirMapManagerPerVehicle::AirMapManagerPerVehicle(AirMapNetworking::SharedData& sharedData, const Vehicle& vehicle, + QGCToolbox& toolbox) + : AirspaceManagerPerVehicle(vehicle), _networking(sharedData), + _flightManager(sharedData), _telemetry(sharedData), _trafficAlerts("mqtt-prod.airmap.io", 8883), + _toolbox(toolbox) { - _updateTimer.setInterval(2000); - _updateTimer.setSingleShot(true); - connect(&_updateTimer, &QTimer::timeout, this, &AirMapManager::_updateToROI); - - connect(&_airspaceRestrictionManager, &AirspaceRestrictionManager::networkError, this, &AirMapManager::_networkError); - connect(&_flightManager, &AirMapFlightManager::networkError, this, &AirMapManager::_networkError); - connect(&_telemetry, &AirMapTelemetry::networkError, this, &AirMapManager::_networkError); - - connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged, this, &AirMapManager::_flightPermitStatusChanged); - - connect(&_trafficAlerts, &AirMapTrafficAlertClient::trafficUpdate, this, &AirMapManager::trafficUpdate); - - qmlRegisterUncreatableType("QGroundControl", 1, 0, "AirspaceAuthorization", "Reference only"); - - MultiVehicleManager* multiVehicleManager = toolbox->multiVehicleManager(); - _connectVehicle(multiVehicleManager->activeVehicle()); - connect(multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &AirMapManager::_activeVehicleChanged); - - GOOGLE_PROTOBUF_VERIFY_VERSION; + connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged, + this, &AirMapManagerPerVehicle::flightPermitStatusChanged); + connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged, + this, &AirMapManagerPerVehicle::_flightPermitStatusChanged); + connect(&_flightManager, &AirMapFlightManager::networkError, this, &AirMapManagerPerVehicle::networkError); + connect(&_telemetry, &AirMapTelemetry::networkError, this, &AirMapManagerPerVehicle::networkError); + connect(&_trafficAlerts, &AirMapTrafficAlertClient::trafficUpdate, this, &AirspaceManagerPerVehicle::trafficUpdate); + settingsChanged(); } -void AirMapManager::_flightPermitStatusChanged() +void AirMapManagerPerVehicle::createFlight(const QList& missionItems) { - // activate traffic alerts - if (_flightManager.flightPermitStatus() == AirspaceAuthorization::PermitAccepted) { - qCDebug(AirMapManagerLog) << "Subscribing to Traffic Alerts"; - // since we already created the flight, we know that we have a valid login token - _trafficAlerts.startConnection(_flightManager.flightID(), _networkingData.login.JWTToken()); + if (!_networking.hasAPIKey()) { + qCDebug(AirMapManagerLog) << "API key not set. Will not create a flight"; + return; } + _flightManager.createFlight(missionItems); +} - emit flightPermitStatusChanged(); +AirspaceAuthorization::PermitStatus AirMapManagerPerVehicle::flightPermitStatus() const +{ + return _flightManager.flightPermitStatus(); } -AirMapManager::~AirMapManager() +void AirMapManagerPerVehicle::startTelemetryStream() { + if (_flightManager.flightID() != "") { + _telemetry.startTelemetryStream(_flightManager.flightID()); + } +} +void AirMapManagerPerVehicle::stopTelemetryStream() +{ + _telemetry.stopTelemetryStream(); } -void AirMapManager::_activeVehicleChanged(Vehicle* activeVehicle) +bool AirMapManagerPerVehicle::isTelemetryStreaming() const { - _connectVehicle(activeVehicle); + return _telemetry.isTelemetryStreaming(); } -void AirMapManager::_connectVehicle(Vehicle* vehicle) +void AirMapManagerPerVehicle::endFlight() { - if (vehicle == _vehicle) { - return; - } - if (_vehicle) { - disconnect(_vehicle, &Vehicle::mavlinkMessageReceived, &_telemetry, &AirMapTelemetry::vehicleMavlinkMessageReceived); - disconnect(_vehicle, &Vehicle::armedChanged, this, &AirMapManager::_vehicleArmedChanged); + _flightManager.endFlight(); + _trafficAlerts.disconnectFromHost(); +} - _telemetry.stopTelemetryStream(); - } +void AirMapManagerPerVehicle::settingsChanged() +{ + AirMapSettings* ap = _toolbox.settingsManager()->airMapSettings(); + _flightManager.setSitaPilotRegistrationId(ap->sitaUserReg()->rawValueString()); + _flightManager.setSitaUavRegistrationId(ap->sitaUavReg()->rawValueString()); - _vehicle = vehicle; - if (vehicle) { - connect(vehicle, &Vehicle::mavlinkMessageReceived, &_telemetry, &AirMapTelemetry::vehicleMavlinkMessageReceived); - connect(vehicle, &Vehicle::armedChanged, this, &AirMapManager::_vehicleArmedChanged); + // reset the states + _flightManager.abort(); + _flightManager.endFlight(); + _telemetry.stopTelemetryStream(); + _trafficAlerts.disconnectFromHost(); +} - _vehicleArmedChanged(vehicle->armed()); +void AirMapManagerPerVehicle::vehicleMavlinkMessageReceived(const mavlink_message_t& message) +{ + if (isTelemetryStreaming()) { + _telemetry.vehicleMavlinkMessageReceived(message); } } -void AirMapManager::_vehicleArmedChanged(bool armed) +void AirMapManagerPerVehicle::_flightPermitStatusChanged() { - if (_flightManager.flightID().length() == 0) { - qCDebug(AirMapManagerLog) << "No Flight ID. Will not update telemetry state"; - return; - } - if (armed) { - _telemetry.startTelemetryStream(_flightManager.flightID()); - _vehicleWasInMissionMode = _vehicle->flightMode() == _vehicle->missionFlightMode(); - } else { - _telemetry.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()) { - _trafficAlerts.disconnectFromHost(); - _flightManager.endFlight(); - } + // activate traffic alerts + if (_flightManager.flightPermitStatus() == AirspaceAuthorization::PermitAccepted) { + qCDebug(AirMapManagerLog) << "Subscribing to Traffic Alerts"; + // since we already created the flight, we know that we have a valid login token + _trafficAlerts.startConnection(_flightManager.flightID(), _networking.JWTLoginToken()); } } +AirMapManager::AirMapManager(QGCApplication* app, QGCToolbox* toolbox) + : AirspaceManager(app, toolbox) +{ + GOOGLE_PROTOBUF_VERIFY_VERSION; +} + void AirMapManager::setToolbox(QGCToolbox* toolbox) { - QGCTool::setToolbox(toolbox); + AirspaceManager::setToolbox(toolbox); AirMapSettings* ap = toolbox->settingsManager()->airMapSettings(); _networkingData.airmapAPIKey = ap->apiKey()->rawValueString(); _networkingData.login.setCredentials(ap->clientID()->rawValueString(), ap->userName()->rawValueString(), ap->password()->rawValueString()); - _flightManager.setSitaPilotRegistrationId(ap->sitaUserReg()->rawValueString()); - _flightManager.setSitaUavRegistrationId(ap->sitaUavReg()->rawValueString()); connect(ap->apiKey(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); connect(ap->clientID(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); @@ -1238,33 +1291,11 @@ void AirMapManager::_settingsChanged() { qCDebug(AirMapManagerLog) << "AirMap settings changed"; - // reset the states - _flightManager.abort(); - _flightManager.endFlight(); - _telemetry.stopTelemetryStream(); - _trafficAlerts.disconnectFromHost(); - AirMapSettings* ap = _toolbox->settingsManager()->airMapSettings(); _networkingData.airmapAPIKey = ap->apiKey()->rawValueString(); _networkingData.login.setCredentials(ap->clientID()->rawValueString(), ap->userName()->rawValueString(), ap->password()->rawValueString()); - _flightManager.setSitaPilotRegistrationId(ap->sitaUserReg()->rawValueString()); - _flightManager.setSitaUavRegistrationId(ap->sitaUavReg()->rawValueString()); -} -void AirMapManager::setROI(QGeoCoordinate& center, double radiusMeters) -{ - _roiCenter = center; - _roiRadius = radiusMeters; - _updateTimer.start(); -} - -void AirMapManager::_updateToROI(void) -{ - if (!_hasAPIKey()) { - qCDebug(AirMapManagerLog) << "API key not set. Not updating Airspace"; - return; - } - _airspaceRestrictionManager.updateROI(_roiCenter, _roiRadius); + emit settingsChanged(); } void AirMapManager::_networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage) @@ -1283,32 +1314,18 @@ void AirMapManager::_networkError(QNetworkReply::NetworkError code, const QStrin } } -void AirMapManager::createFlight(const QList& missionItems) +AirspaceManagerPerVehicle* AirMapManager::instantiateVehicle(const Vehicle& vehicle) { - if (!_hasAPIKey()) { - qCDebug(AirMapManagerLog) << "API key not set. Will not create a flight"; - return; - } - _flightManager.createFlight(missionItems); + AirMapManagerPerVehicle* manager = new AirMapManagerPerVehicle(_networkingData, vehicle, *_toolbox); + connect(manager, &AirMapManagerPerVehicle::networkError, this, &AirMapManager::_networkError); + connect(this, &AirMapManager::settingsChanged, manager, &AirMapManagerPerVehicle::settingsChanged); + return manager; } -AirspaceRestriction::AirspaceRestriction(QObject* parent) - : QObject(parent) +AirspaceRestrictionProvider* AirMapManager::instantiateRestrictionProvider() { - + AirMapRestrictionManager* restrictionManager = new AirMapRestrictionManager(_networkingData); + connect(restrictionManager, &AirMapRestrictionManager::networkError, this, &AirMapManager::_networkError); + return restrictionManager; } -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) -{ - -} diff --git a/src/MissionManager/AirMapManager.h b/src/MissionManager/AirMapManager.h index 5bd77fae6..7eb5f4ec6 100644 --- a/src/MissionManager/AirMapManager.h +++ b/src/MissionManager/AirMapManager.h @@ -15,6 +15,7 @@ #include "QmlObjectListModel.h" #include "MissionItem.h" #include "MultiVehicleManager.h" +#include "AirspaceManagement.h" #include @@ -32,44 +33,6 @@ 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 { @@ -165,6 +128,8 @@ public: */ void abort(); + bool hasAPIKey() const { return _networkingData.airmapAPIKey != ""; } + signals: /// signal when the request finished (get or post). All requests are assumed to return JSON. void finished(QJsonParseError parseError, QJsonDocument document); @@ -196,16 +161,13 @@ private: /// class to download polygons from AirMap -class AirspaceRestrictionManager : public QObject +class AirMapRestrictionManager : public AirspaceRestrictionProvider { Q_OBJECT public: - AirspaceRestrictionManager(AirMapNetworking::SharedData& sharedData); - - void updateROI(const QGeoCoordinate& center, double radiusMeters); + AirMapRestrictionManager(AirMapNetworking::SharedData& sharedData); - QmlObjectListModel* polygonRestrictions(void) { return &_polygonList; } - QmlObjectListModel* circularRestrictions(void) { return &_circleList; } + void setROI(const QGeoCoordinate& center, double radiusMeters) override; signals: void networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage); @@ -223,26 +185,9 @@ private: State _state = State::Idle; int _numAwaitingItems = 0; AirMapNetworking _networking; - - QmlObjectListModel _polygonList; - QmlObjectListModel _circleList; - QList _nextPolygonList; - QList _nextcircleList; }; -class AirspaceAuthorization : public QObject { - Q_OBJECT -public: - enum PermitStatus { - PermitUnknown = 0, - PermitPending, - PermitAccepted, - PermitRejected, - }; - Q_ENUMS(PermitStatus); -}; - /// class to upload a flight class AirMapFlightManager : public QObject { @@ -344,6 +289,8 @@ public: void stopTelemetryStream(); + bool isTelemetryStreaming() const; + signals: void networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage); @@ -360,8 +307,6 @@ private: void _handleGlobalPositionInt(const mavlink_message_t& message); void _handleGPSRawInt(const mavlink_message_t& message); - bool _isTelemetryStreaming() const; - enum class State { Idle, StartCommunication, @@ -418,68 +363,74 @@ private: - -/// AirMap server communication support. -class AirMapManager : public QGCTool +/// AirMap per vehicle management class. +class AirMapManagerPerVehicle : public AirspaceManagerPerVehicle { Q_OBJECT - public: - AirMapManager(QGCApplication* app, QGCToolbox* toolbox); - ~AirMapManager(); + AirMapManagerPerVehicle(AirMapNetworking::SharedData& sharedData, const Vehicle& vehicle, QGCToolbox& toolbox); + 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& missionItems); + void createFlight(const QList& missionItems) override; + AirspaceAuthorization::PermitStatus flightPermitStatus() const override; - QmlObjectListModel* polygonRestrictions(void) { return _airspaceRestrictionManager.polygonRestrictions(); } - QmlObjectListModel* circularRestrictions(void) { return _airspaceRestrictionManager.circularRestrictions(); } + void startTelemetryStream() override; - void setToolbox(QGCToolbox* toolbox) override; + void stopTelemetryStream() override; - AirspaceAuthorization::PermitStatus flightPermitStatus() const { return _flightManager.flightPermitStatus(); } + bool isTelemetryStreaming() const override; signals: - void flightPermitStatusChanged(); - - void trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading); + void networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage); -private slots: - void _updateToROI(void); - void _networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage); +public slots: + void endFlight() override; - void _activeVehicleChanged(Vehicle* activeVehicle); - void _vehicleArmedChanged(bool armed); + void settingsChanged(); +protected slots: + virtual void vehicleMavlinkMessageReceived(const mavlink_message_t& message) override; +private slots: void _flightPermitStatusChanged(); - - void _settingsChanged(); 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; AirMapTelemetry _telemetry; AirMapTrafficAlertClient _trafficAlerts; - QGeoCoordinate _roiCenter; - double _roiRadius; + QGCToolbox& _toolbox; +}; + - 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 - bool _vehicleWasInMissionMode = false; ///< true if the vehicle was in mission mode when arming +signals: + void settingsChanged(); + +private slots: + void _networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage); + + void _settingsChanged(); +private: + + AirMapNetworking::SharedData _networkingData; }; #endif diff --git a/src/MissionManager/AirMapController.cc b/src/MissionManager/AirspaceController.cc similarity index 53% rename from src/MissionManager/AirMapController.cc rename to src/MissionManager/AirspaceController.cc index e0ae896f4..d81d76301 100644 --- a/src/MissionManager/AirMapController.cc +++ b/src/MissionManager/AirspaceController.cc @@ -7,21 +7,14 @@ * ****************************************************************************/ -#include "AirMapController.h" -#include "AirMapManager.h" +#include "AirspaceController.h" +#include "AirspaceManagement.h" #include "QGCApplication.h" #include "QGCQGeoCoordinate.h" -QGC_LOGGING_CATEGORY(AirMapControllerLog, "AirMapControllerLog") - -AirMapController::AirMapController(QObject* parent) +AirspaceController::AirspaceController(QObject* parent) : QObject(parent) - , _manager(qgcApp()->toolbox()->airMapManager()) + , _manager(qgcApp()->toolbox()->airspaceManager()) { - connect(_manager, &AirMapManager::flightPermitStatusChanged, this, &AirMapController::flightPermitStatusChanged); } -AirMapController::~AirMapController() -{ - -} diff --git a/src/MissionManager/AirspaceController.h b/src/MissionManager/AirspaceController.h new file mode 100644 index 000000000..b97c6a86a --- /dev/null +++ b/src/MissionManager/AirspaceController.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "AirspaceManagement.h" +#include "QGCMapPolygon.h" + +class AirspaceController : public QObject +{ + Q_OBJECT + +public: + AirspaceController(QObject* parent = NULL); + ~AirspaceController() = default; + + Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT) ///< List of PolygonAirspaceRestriction objects + Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT) ///< List of CircularAirspaceRestriction objects + + Q_INVOKABLE void setROI(QGeoCoordinate center, double radius) { _manager->setROI(center, radius); } + + QmlObjectListModel* polygons() { return _manager->polygonRestrictions(); } + QmlObjectListModel* circles() { return _manager->circularRestrictions(); } + + Q_PROPERTY(QString providerName READ providerName CONSTANT) + + QString providerName() { return _manager->name(); } +private: + AirspaceManager* _manager; +}; diff --git a/src/MissionManager/AirspaceManagement.cc b/src/MissionManager/AirspaceManagement.cc new file mode 100644 index 000000000..ab43a785e --- /dev/null +++ b/src/MissionManager/AirspaceManagement.cc @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * (c) 2017 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +#include "AirspaceManagement.h" +#include + +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("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(); + } + } +} + diff --git a/src/MissionManager/AirspaceManagement.h b/src/MissionManager/AirspaceManagement.h new file mode 100644 index 000000000..9c3eb2b4e --- /dev/null +++ b/src/MissionManager/AirspaceManagement.h @@ -0,0 +1,232 @@ +/**************************************************************************** + * + * (c) 2017 QGROUNDCONTROL PROJECT + * + * 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 +#include +#include + +#include + +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& polygons() const { return _polygonList; } + const QList& circles() const { return _circleList; } + +signals: + void requestDone(bool success); + +protected: + QList _polygonList; + QList _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& 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 +}; diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc index 085198152..5619f41a5 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -21,7 +21,7 @@ GeoFenceManager::GeoFenceManager(Vehicle* vehicle) : _vehicle (vehicle) , _planManager (vehicle, MAV_MISSION_TYPE_FENCE) , _firstParamLoadComplete (false) - , _airmapManager (qgcApp()->toolbox()->airMapManager()) + , _airspaceManager (qgcApp()->toolbox()->airspaceManager()) { connect(&_planManager, &PlanManager::inProgressChanged, this, &GeoFenceManager::inProgressChanged); connect(&_planManager, &PlanManager::error, this, &GeoFenceManager::error); @@ -101,31 +101,6 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, 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(); - - 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); for (int i=0; i #include -#include "AirMapManager.h" +#include "AirspaceManagement.h" #include "QGCLoggingCategory.h" #include "FactSystem.h" #include "PlanManager.h" @@ -97,7 +97,7 @@ private: bool _firstParamLoadComplete; QList _sendPolygons; QList _sendCircles; - AirMapManager* _airmapManager; + AirspaceManager* _airspaceManager; }; #endif diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index 320c565eb..eb4267b02 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -18,7 +18,6 @@ #include "QGCApplication.h" #include "MissionCommandTree.h" #include "MissionCommandUIInfo.h" -#include "AirMapManager.h" QGC_LOGGING_CATEGORY(PlanManagerLog, "PlanManagerLog") @@ -80,9 +79,10 @@ void PlanManager::writeMissionItems(const QList& missionItems) } if (_planType == MAV_MISSION_TYPE_MISSION) { - AirMapManager *airmapManager = qgcApp()->toolbox()->airMapManager(); - if(airmapManager) { - airmapManager->createFlight(missionItems); + // upload the flight to the airspace management backend + AirspaceManagerPerVehicle* airspaceManager = _vehicle->airspaceManager(); + if (airspaceManager) { + airspaceManager->createFlight(missionItems); } } diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 9f31e124e..2eeabadd4 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -84,6 +84,7 @@ #include "CameraCalc.h" #include "VisualMissionItem.h" #include "AirMapController.h" +#include "AirspaceController.h" #ifndef NO_SERIAL_LINK #include "SerialLink.h" @@ -362,6 +363,7 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "QGCCameraManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "QGCCameraControl", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "AirMapController", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "AirspaceController", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.QGCPositionManager", 1, 0, "QGCPositionManager", "Reference only"); diff --git a/src/QGCToolbox.cc b/src/QGCToolbox.cc index e7ef87c80..94d5795a3 100644 --- a/src/QGCToolbox.cc +++ b/src/QGCToolbox.cc @@ -57,7 +57,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app) , _mavlinkLogManager (NULL) , _corePlugin (NULL) , _settingsManager (NULL) - , _airMapManager (NULL) + , _airspaceManager (NULL) { // SettingsManager must be first so settings are available to any subsequent tools _settingsManager = new SettingsManager(app, this); @@ -82,7 +82,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app) _followMe = new FollowMe (app, this); _videoManager = new VideoManager (app, this); _mavlinkLogManager = new MAVLinkLogManager (app, this); - _airMapManager = new AirMapManager (app, this); + _airspaceManager = new AirMapManager (app, this); } void QGCToolbox::setChildToolboxes(void) @@ -109,7 +109,7 @@ void QGCToolbox::setChildToolboxes(void) _qgcPositionManager->setToolbox(this); _videoManager->setToolbox(this); _mavlinkLogManager->setToolbox(this); - _airMapManager->setToolbox(this); + _airspaceManager->setToolbox(this); } void QGCToolbox::_scanAndLoadPlugins(QGCApplication* app) diff --git a/src/QGCToolbox.h b/src/QGCToolbox.h index fa1168b86..5ece9befe 100644 --- a/src/QGCToolbox.h +++ b/src/QGCToolbox.h @@ -32,7 +32,7 @@ class VideoManager; class MAVLinkLogManager; class QGCCorePlugin; class SettingsManager; -class AirMapManager; +class AirspaceManager; /// This is used to manage all of our top level services/tools class QGCToolbox : public QObject { @@ -57,7 +57,7 @@ public: MAVLinkLogManager* mavlinkLogManager(void) { return _mavlinkLogManager; } QGCCorePlugin* corePlugin(void) { return _corePlugin; } SettingsManager* settingsManager(void) { return _settingsManager; } - AirMapManager* airMapManager(void) { return _airMapManager; } + AirspaceManager* airspaceManager(void) { return _airspaceManager; } #ifndef __mobile__ GPSManager* gpsManager(void) { return _gpsManager; } @@ -88,7 +88,7 @@ private: MAVLinkLogManager* _mavlinkLogManager; QGCCorePlugin* _corePlugin; SettingsManager* _settingsManager; - AirMapManager* _airMapManager; + AirspaceManager* _airspaceManager; friend class QGCApplication; }; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index dd8e3bdf0..549c768e8 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -34,6 +34,7 @@ #include "ADSBVehicle.h" #include "QGCCameraManager.h" #include "AirMapController.h" +#include "AirspaceController.h" QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") @@ -132,7 +133,8 @@ Vehicle::Vehicle(LinkInterface* link, , _rallyPointManager(NULL) , _rallyPointManagerInitialRequestSent(false) , _parameterManager(NULL) - , _airMapController(NULL) + , _airspaceController(NULL) + , _airspaceManagerPerVehicle(NULL) , _armed(false) , _base_mode(0) , _custom_mode(0) @@ -255,8 +257,17 @@ Vehicle::Vehicle(LinkInterface* link, _adsbTimer.setSingleShot(false); _adsbTimer.start(1000); - _airMapController = new AirMapController(this); - connect(qgcApp()->toolbox()->airMapManager(), &AirMapManager::trafficUpdate, this, &Vehicle::_trafficUpdate); + _airspaceController = new AirspaceController(this); + + 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 @@ -315,7 +326,8 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _rallyPointManager(NULL) , _rallyPointManagerInitialRequestSent(false) , _parameterManager(NULL) - , _airMapController(NULL) + , _airspaceController(NULL) + , _airspaceManagerPerVehicle(NULL) , _armed(false) , _base_mode(0) , _custom_mode(0) @@ -445,6 +457,9 @@ Vehicle::~Vehicle() delete _mav; _mav = NULL; + if (_airspaceManagerPerVehicle) { + delete _airspaceManagerPerVehicle; + } } void Vehicle::prepareDelete() diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index acbce800b..a2bad6bc1 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -20,6 +20,7 @@ #include "MAVLinkProtocol.h" #include "UASMessageHandler.h" #include "SettingsFact.h" +#include class UAS; class UASInterface; @@ -36,6 +37,7 @@ class SettingsManager; class ADSBVehicle; class QGCCameraManager; class AirMapController; +class AirspaceController; Q_DECLARE_LOGGING_CATEGORY(VehicleLog) @@ -322,6 +324,8 @@ public: Q_PROPERTY(QGCCameraManager* dynamicCameras READ dynamicCameras NOTIFY dynamicCamerasChanged) Q_PROPERTY(QString hobbsMeter READ hobbsMeter NOTIFY hobbsMeterChanged) 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 Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying @@ -537,7 +541,7 @@ public: QmlObjectListModel* cameraTriggerPoints(void) { return &_cameraTriggerPoints; } QmlObjectListModel* adsbVehicles(void) { return &_adsbVehicles; } - AirMapController* airMapController() { return _airMapController; } + AirspaceController* airspaceController() { return _airspaceController; } int flowImageIndex() { return _flowImageIndex; } @@ -718,6 +722,11 @@ public: /// Vehicle is about to be deleted void prepareDelete(); + AirspaceAuthorization::PermitStatus flightPermitStatus() const + { return _airspaceManagerPerVehicle ? _airspaceManagerPerVehicle->flightPermitStatus() : AirspaceAuthorization::PermitUnknown; } + + AirspaceManagerPerVehicle* airspaceManager() const { return _airspaceManagerPerVehicle; } + signals: void allLinksInactive(Vehicle* vehicle); void coordinateChanged(QGeoCoordinate coordinate); @@ -749,6 +758,8 @@ signals: void capabilitiesKnownChanged(bool capabilitiesKnown); void initialPlanRequestCompleteChanged(bool initialPlanRequestComplete); void capabilityBitsChanged(uint64_t capabilityBits); + void flightPermitStatusChanged(); + void messagesReceivedChanged (); void messagesSentChanged (); @@ -997,7 +1008,8 @@ private: ParameterManager* _parameterManager; - AirMapController* _airMapController; + AirspaceController* _airspaceController; + AirspaceManagerPerVehicle* _airspaceManagerPerVehicle; bool _armed; ///< true: vehicle is armed uint8_t _base_mode; ///< base_mode from HEARTBEAT -- 2.22.0