From f55279d99c2fc25f04ee8b8363b47d0ccc60c183 Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Sat, 20 Jan 2018 23:27:11 -0500 Subject: [PATCH] WIP Making it possible to have a build without Airmap Move all airmap related stuff to its own directory --- QGCExternalLibs.pri | 2 + qgcresources.qrc | 7 - qgroundcontrol.pro | 32 +- qgroundcontrol.qrc | 5 - src/Airmap/AirMap.SettingsGroup.json | 26 + src/Airmap/AirMapManager.cc | 981 +++++++++++++++++++++++++++++ src/Airmap/AirMapManager.h | 367 +++++++++++ src/Airmap/AirMapSettings.cc | 30 + src/Airmap/AirMapSettings.h | 28 + src/Airmap/AirspaceController.cc | 20 + src/Airmap/AirspaceController.h | 36 ++ src/Airmap/AirspaceManagement.cc | 121 ++++ src/Airmap/AirspaceManagement.h | 255 ++++++++ src/Airmap/airmap.qrc | 17 + src/FlightDisplay/FlightDisplayView.qml | 6 +- src/FlightDisplay/FlightDisplayViewMap.qml | 6 +- src/MissionManager/AirMapManager.cc | 981 ----------------------------- src/MissionManager/AirMapManager.h | 367 ----------- src/MissionManager/AirspaceController.cc | 20 - src/MissionManager/AirspaceController.h | 36 -- src/MissionManager/AirspaceManagement.cc | 121 ---- src/MissionManager/AirspaceManagement.h | 255 -------- src/MissionManager/GeoFenceManager.cc | 2 + src/MissionManager/GeoFenceManager.h | 5 + src/MissionManager/PlanManager.cc | 2 + src/QGCApplication.cc | 7 +- src/QGCToolbox.cc | 8 + src/QGCToolbox.h | 8 +- src/QmlControls/QGroundControlQmlGlobal.h | 6 + src/Settings/AirMap.SettingsGroup.json | 26 - src/Settings/AirMapSettings.cc | 30 - src/Settings/AirMapSettings.h | 28 - src/Settings/SettingsManager.cc | 4 + src/Settings/SettingsManager.h | 9 +- src/Vehicle/Vehicle.cc | 13 +- src/Vehicle/Vehicle.h | 17 +- 36 files changed, 1983 insertions(+), 1901 deletions(-) create mode 100644 src/Airmap/AirMap.SettingsGroup.json create mode 100644 src/Airmap/AirMapManager.cc create mode 100644 src/Airmap/AirMapManager.h create mode 100644 src/Airmap/AirMapSettings.cc create mode 100644 src/Airmap/AirMapSettings.h create mode 100644 src/Airmap/AirspaceController.cc create mode 100644 src/Airmap/AirspaceController.h create mode 100644 src/Airmap/AirspaceManagement.cc create mode 100644 src/Airmap/AirspaceManagement.h create mode 100644 src/Airmap/airmap.qrc delete mode 100644 src/MissionManager/AirMapManager.cc delete mode 100644 src/MissionManager/AirMapManager.h delete mode 100644 src/MissionManager/AirspaceController.cc delete mode 100644 src/MissionManager/AirspaceController.h delete mode 100644 src/MissionManager/AirspaceManagement.cc delete mode 100644 src/MissionManager/AirspaceManagement.h delete mode 100644 src/Settings/AirMap.SettingsGroup.json delete mode 100644 src/Settings/AirMapSettings.cc delete mode 100644 src/Settings/AirMapSettings.h diff --git a/QGCExternalLibs.pri b/QGCExternalLibs.pri index 7d47a98..651e40d 100644 --- a/QGCExternalLibs.pri +++ b/QGCExternalLibs.pri @@ -130,9 +130,11 @@ contains (DEFINES, DISABLE_ZEROCONF) { # contains (DEFINES, DISABLE_AIRMAP) { message("Skipping support for AirMap (manual override from command line)") + DEFINES -= QGC_AIRMAP_ENABLED # Otherwise the user can still disable this feature in the user_config.pri file. } else:exists(user_config.pri):infile(user_config.pri, DEFINES, DISABLE_AIRMAP) { message("Skipping support for AirMap (manual override from user_config.pri)") + DEFINES -= QGC_AIRMAP_ENABLED } else { AIRMAPD_PATH = $$PWD/libs/airmapd INCLUDEPATH += \ diff --git a/qgcresources.qrc b/qgcresources.qrc index 019dfa2..496b4e3 100644 --- a/qgcresources.qrc +++ b/qgcresources.qrc @@ -172,13 +172,6 @@ src/FirmwarePlugin/APM/APMBrandImageSub.png src/FirmwarePlugin/PX4/PX4BrandImage.png src/FlightMap/Images/sub.png - src/Airmap/images/advisory-icon.svg - - - src/Airmap/images/advisory-icon.svg - src/Airmap/images/colapse.svg - src/Airmap/images/expand.svg - src/Airmap/images/pencil.svg resources/action.svg diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 7c9f0fe..e856f8c 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -505,9 +505,6 @@ HEADERS += \ src/LogCompressor.h \ src/MG.h \ src/MissionManager/CameraCalc.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 \ @@ -567,7 +564,6 @@ HEADERS += \ src/QmlControls/RCChannelMonitorController.h \ src/QmlControls/ScreenToolsController.h \ src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h \ - src/Settings/AirMapSettings.h \ src/Settings/AppSettings.h \ src/Settings/AutoConnectSettings.h \ src/Settings/BrandImageSettings.h \ @@ -594,6 +590,7 @@ HEADERS += \ src/uas/UASMessageHandler.h \ src/UTM.h \ + AndroidBuild { HEADERS += \ src/Joystick/JoystickAndroid.h \ @@ -701,9 +698,6 @@ SOURCES += \ src/JsonHelper.cc \ src/LogCompressor.cc \ src/MissionManager/CameraCalc.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 \ @@ -760,7 +754,6 @@ SOURCES += \ src/QmlControls/RCChannelMonitorController.cc \ src/QmlControls/ScreenToolsController.cc \ src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc \ - src/Settings/AirMapSettings.cc \ src/Settings/AppSettings.cc \ src/Settings/AutoConnectSettings.cc \ src/Settings/BrandImageSettings.cc \ @@ -1062,6 +1055,29 @@ SOURCES += \ src/FactSystem/SettingsFact.cc \ #------------------------------------------------------------------------------------- +# AirMap + +contains (DEFINES, QGC_AIRMAP_ENABLED) { + RESOURCES += \ + src/Airmap/airmap.qrc + + INCLUDEPATH += \ + src/Airmap + + HEADERS += \ + src/Airmap/AirspaceController.h \ + src/Airmap/AirMapManager.h \ + src/Airmap/AirspaceManagement.h \ + src/Airmap/AirMapSettings.h + + SOURCES += \ + src/Airmap/AirMapManager.cc \ + src/Airmap/AirspaceManagement.cc \ + src/Airmap/AirspaceController.cc \ + src/Airmap/AirMapSettings.cc +} + +#------------------------------------------------------------------------------------- # Video Streaming INCLUDEPATH += \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 9d399d6..3668172 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -15,10 +15,6 @@ src/ui/toolbar/JoystickIndicator.qml - src/Airmap/QGroundControl.Airmap.qmldir - src/Airmap/AirspaceControl.qml - src/Airmap/AirspaceRegulation.qml - src/Airmap/AirmapSettings.qml src/AnalyzeView/AnalyzeView.qml src/ui/AppSettings.qml src/ui/preferences/BluetoothSettings.qml @@ -199,7 +195,6 @@ src/MissionManager/MavCmdInfoRover.json src/MissionManager/MavCmdInfoSub.json src/MissionManager/MavCmdInfoVTOL.json - src/Settings/AirMap.SettingsGroup.json src/Settings/App.SettingsGroup.json src/Settings/AutoConnect.SettingsGroup.json src/Settings/FlightMap.SettingsGroup.json diff --git a/src/Airmap/AirMap.SettingsGroup.json b/src/Airmap/AirMap.SettingsGroup.json new file mode 100644 index 0000000..5806903 --- /dev/null +++ b/src/Airmap/AirMap.SettingsGroup.json @@ -0,0 +1,26 @@ +[ +{ + "name": "apiKey", + "shortDescription": "AirMap API Key", + "type": "string", + "defaultValue": "" +}, +{ + "name": "clientID", + "shortDescription": "AirMap Client ID", + "type": "string", + "defaultValue": "" +}, +{ + "name": "userName", + "shortDescription": "AirMap User Name", + "type": "string", + "defaultValue": "" +}, +{ + "name": "password", + "shortDescription": "AirMap Password", + "type": "string", + "defaultValue": "" +} +] diff --git a/src/Airmap/AirMapManager.cc b/src/Airmap/AirMapManager.cc new file mode 100644 index 0000000..d75772a --- /dev/null +++ b/src/Airmap/AirMapManager.cc @@ -0,0 +1,981 @@ +/**************************************************************************** + * + * (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 "AirMapManager.h" +#include "Vehicle.h" +#include "QmlObjectListModel.h" +#include "JsonHelper.h" +#include "SettingsManager.h" +#include "AppSettings.h" +#include "AirMapSettings.h" +#include "QGCQGeoCoordinate.h" +#include "QGCApplication.h" + +#include +#include +#include +#include +#include +#include +#include + +using namespace airmap; + +QGC_LOGGING_CATEGORY(AirMapManagerLog, "AirMapManagerLog") + + +void AirMapSharedState::setSettings(const Settings& settings) +{ + logout(); + _settings = settings; +} + +void AirMapSharedState::doRequestWithLogin(const Callback& callback) +{ + if (isLoggedIn()) { + callback(_loginToken); + } else { + login(); + _pendingRequests.enqueue(callback); + } +} + +void AirMapSharedState::login() +{ + if (isLoggedIn() || _isLoginInProgress) { + return; + } + _isLoginInProgress = true; + + if (_settings.userName == "") { //use anonymous login + + Authenticator::AuthenticateAnonymously::Params params; + params.id = ""; + _client->authenticator().authenticate_anonymously(params, + [this](const Authenticator::AuthenticateAnonymously::Result& result) { + if (!_isLoginInProgress) { // was logout() called in the meanwhile? + return; + } + if (result) { + qCDebug(AirMapManagerLog)<<"Successfully authenticated with AirMap: id="<< result.value().id.c_str(); + _loginToken = QString::fromStdString(result.value().id); + _processPendingRequests(); + } else { + _pendingRequests.clear(); + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + emit error("Failed to authenticate with AirMap", + QString::fromStdString(result.error().message()), description); + } + }); + + } else { + + Authenticator::AuthenticateWithPassword::Params params; + params.oauth.username = _settings.userName.toStdString(); + params.oauth.password = _settings.password.toStdString(); + params.oauth.client_id = _settings.clientID.toStdString(); + params.oauth.device_id = "QGroundControl"; + _client->authenticator().authenticate_with_password(params, + [this](const Authenticator::AuthenticateWithPassword::Result& result) { + if (!_isLoginInProgress) { // was logout() called in the meanwhile? + return; + } + if (result) { + qCDebug(AirMapManagerLog)<<"Successfully authenticated with AirMap: id="<< result.value().id.c_str()<<", access=" + < isAlive(_instance); + _shared.client()->airspaces().search(params, + [this, isAlive](const Airspaces::Search::Result& result) { + + if (!isAlive.lock()) return; + if (_state != State::RetrieveItems) return; + + if (result) { + const std::vector& airspaces = result.value(); + qCDebug(AirMapManagerLog)<<"Successful search. Items:" << airspaces.size(); + for (const auto& airspace : airspaces) { + + const Geometry& geometry = airspace.geometry(); + switch(geometry.type()) { + case Geometry::Type::polygon: { + const Geometry::Polygon& polygon = geometry.details_for_polygon(); + _addPolygonToList(polygon, _polygonList); + } + break; + case Geometry::Type::multi_polygon: { + const Geometry::MultiPolygon& multiPolygon = geometry.details_for_multi_polygon(); + for (const auto& polygon : multiPolygon) { + _addPolygonToList(polygon, _polygonList); + } + } + break; + case Geometry::Type::point: { + const Geometry::Point& point = geometry.details_for_point(); + _circleList.append(new CircularAirspaceRestriction(QGeoCoordinate(point.latitude, point.longitude), 0.)); + // TODO: radius??? + } + break; + default: + qWarning() << "unsupported geometry type: "<<(int)geometry.type(); + break; + } + + } + + } else { + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + emit error("Failed to retrieve Geofences", + QString::fromStdString(result.error().message()), description); + } + emit requestDone(true); + _state = State::Idle; + }); +} + +void AirMapRestrictionManager::_addPolygonToList(const airmap::Geometry::Polygon& polygon, QList& list) +{ + QVariantList polygonArray; + for (const auto& vertex : polygon.outer_ring.coordinates) { + QGeoCoordinate coord; + if (vertex.altitude) { + coord = QGeoCoordinate(vertex.latitude, vertex.longitude, vertex.altitude.get()); + } else { + coord = QGeoCoordinate(vertex.latitude, vertex.longitude); + } + polygonArray.append(QVariant::fromValue(coord)); + } + list.append(new PolygonAirspaceRestriction(polygonArray)); + + if (polygon.inner_rings.size() > 0) { + // no need to support those (they are rare, and in most cases, there's a more restrictive polygon filling the hole) + qCDebug(AirMapManagerLog) << "Polygon with holes. Size: "<& missionItems) +{ + if (!_shared.client()) { + qCDebug(AirMapManagerLog) << "No AirMap client instance. Will not create a flight"; + return; + } + + if (_state != State::Idle) { + qCWarning(AirMapManagerLog) << "AirMapFlightManager::createFlight: State not idle"; + return; + } + + _flight.reset(); + + // get the flight trajectory + for(const auto &item : missionItems) { + switch(item->command()) { + case MAV_CMD_NAV_WAYPOINT: + case MAV_CMD_NAV_LAND: + case MAV_CMD_NAV_TAKEOFF: + // TODO: others too? + { + // TODO: handle different coordinate frames? + double lat = item->param5(); + double lon = item->param6(); + double alt = item->param7(); + _flight.coords.append(QGeoCoordinate(lat, lon, alt)); + if (alt > _flight.maxAltitude) { + _flight.maxAltitude = alt; + } + if (item->command() == MAV_CMD_NAV_TAKEOFF) { + _flight.takeoffCoord = _flight.coords.last(); + } + } + break; + default: + break; + } + } + if (_flight.coords.empty()) { + return; + } + + _flight.maxAltitude += 5; // add a safety buffer + + + if (_pilotID == "") { + // need to get the pilot id before uploading the flight + qCDebug(AirMapManagerLog) << "Getting pilot ID"; + _state = State::GetPilotID; + std::weak_ptr isAlive(_instance); + _shared.doRequestWithLogin([this, isAlive](const QString& login_token) { + if (!isAlive.lock()) return; + + Pilots::Authenticated::Parameters params; + params.authorization = login_token.toStdString(); + _shared.client()->pilots().authenticated(params, [this, isAlive](const Pilots::Authenticated::Result& result) { + + if (!isAlive.lock()) return; + if (_state != State::GetPilotID) return; + + if (result) { + _pilotID = QString::fromStdString(result.value().id); + qCDebug(AirMapManagerLog) << "Got Pilot ID:"<<_pilotID; + _uploadFlight(); + } else { + _flightPermitStatus = AirspaceAuthorization::PermitUnknown; + emit flightPermitStatusChanged(); + _state = State::Idle; + + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + emit error("Failed to create Flight Plan", + QString::fromStdString(result.error().message()), description); + } + }); + }); + } else { + _uploadFlight(); + } + + _flightPermitStatus = AirspaceAuthorization::PermitPending; + emit flightPermitStatusChanged(); +} + +void AirMapFlightManager::_endFirstFlight() +{ + // it could be that AirMap still has an open pending flight, but we don't know the flight ID. + // As there can only be one, we query the flights that end in the future, and close it if there's one. + + _state = State::EndFirstFlight; + + Flights::Search::Parameters params; + params.pilot_id = _pilotID.toStdString(); + params.end_after = Clock::universal_time() - Hours{1}; + + std::weak_ptr isAlive(_instance); + _shared.client()->flights().search(params, [this, isAlive](const Flights::Search::Result& result) { + if (!isAlive.lock()) return; + if (_state != State::EndFirstFlight) return; + + if (result && result.value().flights.size() > 0) { + + Q_ASSERT(_shared.loginToken() != ""); // at this point we know the user is logged in (we queried the pilot id) + + Flights::EndFlight::Parameters params; + params.authorization = _shared.loginToken().toStdString(); + params.id = result.value().flights[0].id; // pick the first flight (TODO: match the vehicle id) + _shared.client()->flights().end_flight(params, [this, isAlive](const Flights::EndFlight::Result& result) { + if (!isAlive.lock()) return; + if (_state != State::EndFirstFlight) return; + + if (!result) { + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + emit error("Failed to end first Flight", + QString::fromStdString(result.error().message()), description); + } + _state = State::Idle; + _uploadFlight(); + }); + + } else { + _state = State::Idle; + _uploadFlight(); + } + }); +} + +void AirMapFlightManager::_uploadFlight() +{ + if (_pendingFlightId != "") { + // we need to end an existing flight first + _endFlight(_pendingFlightId); + return; + } + + if (_noFlightCreatedYet) { + _endFirstFlight(); + _noFlightCreatedYet = false; + return; + } + + qCDebug(AirMapManagerLog) << "uploading flight"; + _state = State::FlightUpload; + + std::weak_ptr isAlive(_instance); + _shared.doRequestWithLogin([this, isAlive](const QString& login_token) { + + if (!isAlive.lock()) return; + if (_state != State::FlightUpload) return; + + FlightPlans::Create::Parameters params; + params.max_altitude = _flight.maxAltitude; + params.buffer = 2.f; + params.latitude = _flight.takeoffCoord.latitude(); + params.longitude = _flight.takeoffCoord.longitude(); + params.pilot.id = _pilotID.toStdString(); + params.start_time = Clock::universal_time() + Minutes{5}; + params.end_time = Clock::universal_time() + Hours{2}; // TODO: user-configurable? + params.rulesets = { // TODO: which ones to use? + "che_drone_rules", + "che_notam", + "che_airmap_rules", + "che_nature_preserve" + }; + + // geometry: LineString + Geometry::LineString lineString; + for (const auto& qcoord : _flight.coords) { + Geometry::Coordinate coord; + coord.latitude = qcoord.latitude(); + coord.longitude = qcoord.longitude(); + lineString.coordinates.push_back(coord); + } + + params.geometry = Geometry(lineString); + params.authorization = login_token.toStdString(); + _flight.coords.clear(); + + _shared.client()->flight_plans().create_by_polygon(params, [this, isAlive](const FlightPlans::Create::Result& result) { + if (!isAlive.lock()) return; + if (_state != State::FlightUpload) return; + + if (result) { + _pendingFlightPlan = QString::fromStdString(result.value().id); + qCDebug(AirMapManagerLog) << "Flight Plan created:"<<_pendingFlightPlan; + + _checkForValidBriefing(); + + } else { + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + emit error("Flight Plan creation failed", + QString::fromStdString(result.error().message()), description); + } + + }); + + }); +} + +void AirMapFlightManager::_checkForValidBriefing() +{ + _state = State::FlightBrief; + FlightPlans::RenderBriefing::Parameters params; + params.authorization = _shared.loginToken().toStdString(); + params.id = _pendingFlightPlan.toStdString(); + std::weak_ptr isAlive(_instance); + _shared.client()->flight_plans().render_briefing(params, [this, isAlive](const FlightPlans::RenderBriefing::Result& result) { + if (!isAlive.lock()) return; + if (_state != State::FlightBrief) return; + + if (result) { + bool allValid = true; + for (const auto& validation : result.value().evaluation.validations) { + if (validation.status != Evaluation::Validation::Status::valid) { + emit error(QString("%1 registration identifier is invalid: %2").arg( + QString::fromStdString(validation.authority.name)).arg(QString::fromStdString(validation.message)), "", ""); + allValid = false; + } + } + if (allValid) { + _submitPendingFlightPlan(); + } else { + _flightPermitStatus = AirspaceAuthorization::PermitRejected; + emit flightPermitStatusChanged(); + _state = State::Idle; + } + + } else { + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + emit error("Brief Request failed", + QString::fromStdString(result.error().message()), description); + _state = State::Idle; + } + }); +} + +void AirMapFlightManager::_submitPendingFlightPlan() +{ + _state = State::FlightSubmit; + FlightPlans::Submit::Parameters params; + params.authorization = _shared.loginToken().toStdString(); + params.id = _pendingFlightPlan.toStdString(); + std::weak_ptr isAlive(_instance); + _shared.client()->flight_plans().submit(params, [this, isAlive](const FlightPlans::Submit::Result& result) { + if (!isAlive.lock()) return; + if (_state != State::FlightSubmit) return; + + if (result) { + _pendingFlightId = QString::fromStdString(result.value().flight_id.get()); + _state = State::FlightPolling; + _pollBriefing(); + + } else { + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + emit error("Failed to submit Flight Plan", + QString::fromStdString(result.error().message()), description); + _state = State::Idle; + } + }); +} + +void AirMapFlightManager::_pollBriefing() +{ + if (_state != State::FlightPolling) { + qCWarning(AirMapManagerLog) << "AirMapFlightManager::_pollBriefing: not in polling state"; + return; + } + + FlightPlans::RenderBriefing::Parameters params; + params.authorization = _shared.loginToken().toStdString(); + params.id = _pendingFlightPlan.toStdString(); + std::weak_ptr isAlive(_instance); + _shared.client()->flight_plans().render_briefing(params, [this, isAlive](const FlightPlans::RenderBriefing::Result& result) { + if (!isAlive.lock()) return; + if (_state != State::FlightPolling) return; + + if (result) { + const FlightPlan::Briefing& briefing = result.value(); + qCDebug(AirMapManagerLog) << "flight polling/briefing response"; + bool rejected = false; + bool accepted = false; + bool pending = false; + for (const auto& authorization : briefing.evaluation.authorizations) { + switch (authorization.status) { + case Evaluation::Authorization::Status::accepted: + case Evaluation::Authorization::Status::accepted_upon_submission: + accepted = true; + break; + case Evaluation::Authorization::Status::rejected: + case Evaluation::Authorization::Status::rejected_upon_submission: + rejected = true; + break; + case Evaluation::Authorization::Status::pending: + pending = true; + break; + } + } + + if (briefing.evaluation.authorizations.size() == 0) { + // if we don't get any authorizations, we assume it's accepted + accepted = true; + } + + qCDebug(AirMapManagerLog) << "flight approval: accepted=" << accepted << "rejected" << rejected << "pending" << pending; + + if ((rejected || accepted) && !pending) { + if (rejected) { // rejected has priority + _flightPermitStatus = AirspaceAuthorization::PermitRejected; + } else { + _flightPermitStatus = AirspaceAuthorization::PermitAccepted; + } + _currentFlightId = _pendingFlightId; + _pendingFlightPlan = ""; + emit flightPermitStatusChanged(); + _state = State::Idle; + } else { + // wait until we send the next polling request + _pollTimer.setSingleShot(true); + _pollTimer.start(2000); + } + } else { + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + emit error("Brief Request failed", + QString::fromStdString(result.error().message()), description); + _state = State::Idle; + } + }); +} + +void AirMapFlightManager::endFlight() +{ + if (_currentFlightId.length() == 0) { + return; + } + if (_state != State::Idle) { + qCWarning(AirMapManagerLog) << "AirMapFlightManager::endFlight: State not idle"; + return; + } + _endFlight(_currentFlightId); + + _flightPermitStatus = AirspaceAuthorization::PermitUnknown; + emit flightPermitStatusChanged(); +} + +void AirMapFlightManager::_endFlight(const QString& flightID) +{ + qCDebug(AirMapManagerLog) << "ending flight" << flightID; + + _state = State::FlightEnd; + + Q_ASSERT(_shared.loginToken() != ""); // Since we have a flight ID, we need to be logged in + + Flights::EndFlight::Parameters params; + params.authorization = _shared.loginToken().toStdString(); + params.id = flightID.toStdString(); + std::weak_ptr isAlive(_instance); + _shared.client()->flights().end_flight(params, [this, isAlive](const Flights::EndFlight::Result& result) { + if (!isAlive.lock()) return; + if (_state != State::FlightEnd) return; + + _state = State::Idle; + _pendingFlightId = ""; + _pendingFlightPlan = ""; + _currentFlightId = ""; + if (result) { + if (!_flight.coords.empty()) { + _uploadFlight(); + } + } else { + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + emit error("Failed to end Flight", + QString::fromStdString(result.error().message()), description); + } + }); +} + + +AirMapTelemetry::AirMapTelemetry(AirMapSharedState& shared) +: _shared(shared) +{ +} + +void AirMapTelemetry::vehicleMavlinkMessageReceived(const mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + _handleGlobalPositionInt(message); + break; + case MAVLINK_MSG_ID_GPS_RAW_INT: + _handleGPSRawInt(message); + break; + } + +} + +bool AirMapTelemetry::isTelemetryStreaming() const +{ + return _state == State::Streaming; +} + +void AirMapTelemetry::_handleGPSRawInt(const mavlink_message_t& message) +{ + if (!isTelemetryStreaming()) { + return; + } + + mavlink_gps_raw_int_t gps_raw; + mavlink_msg_gps_raw_int_decode(&message, &gps_raw); + + if (gps_raw.eph == UINT16_MAX) { + _lastHdop = 1.f; + } else { + _lastHdop = gps_raw.eph / 100.f; + } +} + +void AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message) +{ + if (!isTelemetryStreaming()) { + return; + } + + mavlink_global_position_int_t globalPosition; + mavlink_msg_global_position_int_decode(&message, &globalPosition); + + Telemetry::Position position{ + milliseconds_since_epoch(Clock::universal_time()), + (double) globalPosition.lat / 1e7, + (double) globalPosition.lon / 1e7, + (float) globalPosition.alt / 1000.f, + (float) globalPosition.relative_alt / 1000.f, + _lastHdop + }; + Telemetry::Speed speed{ + milliseconds_since_epoch(Clock::universal_time()), + globalPosition.vx / 100.f, + globalPosition.vy / 100.f, + globalPosition.vz / 100.f + }; + + Flight flight; + flight.id = _flightID.toStdString(); + _shared.client()->telemetry().submit_updates(flight, _key, + {Telemetry::Update{position}, Telemetry::Update{speed}}); + +} + +void AirMapTelemetry::startTelemetryStream(const QString& flightID) +{ + if (_state != State::Idle) { + qCWarning(AirMapManagerLog) << "Not starting telemetry: not in idle state:" << (int)_state; + return; + } + + qCInfo(AirMapManagerLog) << "Starting Telemetry stream with flightID" << flightID; + + _state = State::StartCommunication; + _flightID = flightID; + + Flights::StartFlightCommunications::Parameters params; + params.authorization = _shared.loginToken().toStdString(); + params.id = _flightID.toStdString(); + std::weak_ptr isAlive(_instance); + _shared.client()->flights().start_flight_communications(params, [this, isAlive](const Flights::StartFlightCommunications::Result& result) { + if (!isAlive.lock()) return; + if (_state != State::StartCommunication) return; + + if (result) { + _key = result.value().key; + _state = State::Streaming; + } else { + _state = State::Idle; + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + emit error("Failed to start telemetry streaming", + QString::fromStdString(result.error().message()), description); + } + }); +} + +void AirMapTelemetry::stopTelemetryStream() +{ + if (_state == State::Idle) { + return; + } + qCInfo(AirMapManagerLog) << "Stopping Telemetry stream with flightID" << _flightID; + + _state = State::EndCommunication; + Flights::EndFlightCommunications::Parameters params; + params.authorization = _shared.loginToken().toStdString(); + params.id = _flightID.toStdString(); + std::weak_ptr isAlive(_instance); + _shared.client()->flights().end_flight_communications(params, [this, isAlive](const Flights::EndFlightCommunications::Result& result) { + Q_UNUSED(result); + if (!isAlive.lock()) return; + if (_state != State::EndCommunication) return; + + _key = ""; + _state = State::Idle; + }); +} + +AirMapTrafficMonitor::~AirMapTrafficMonitor() +{ + stop(); +} + +void AirMapTrafficMonitor::startConnection(const QString& flightID) +{ + _flightID = flightID; + std::weak_ptr isAlive(_instance); + auto handler = [this, isAlive](const Traffic::Monitor::Result& result) { + if (!isAlive.lock()) return; + if (result) { + _monitor = result.value(); + _subscriber = std::make_shared( + std::bind(&AirMapTrafficMonitor::_update, this, std::placeholders::_1, std::placeholders::_2)); + _monitor->subscribe(_subscriber); + } else { + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + emit error("Failed to start Traffic Monitoring", + QString::fromStdString(result.error().message()), description); + } + }; + + Traffic::Monitor::Params params{flightID.toStdString(), _shared.loginToken().toStdString()}; + _shared.client()->traffic().monitor(params, handler); +} + +void AirMapTrafficMonitor::_update(Traffic::Update::Type type, const std::vector& update) +{ + qCDebug(AirMapManagerLog) << "Traffic update with" << update.size() << "elements"; + + if (type != Traffic::Update::Type::situational_awareness) + return; // currently we're only interested in situational awareness + + for (const auto& traffic : update) { + QString traffic_id = QString::fromStdString(traffic.id); + QString vehicle_id = QString::fromStdString(traffic.aircraft_id); + emit trafficUpdate(traffic_id, vehicle_id, QGeoCoordinate(traffic.latitude, traffic.longitude, traffic.altitude), + traffic.heading); + } +} + +void AirMapTrafficMonitor::stop() +{ + if (_monitor) { + _monitor->unsubscribe(_subscriber); + _subscriber.reset(); + _monitor.reset(); + } +} + +AirMapManagerPerVehicle::AirMapManagerPerVehicle(AirMapSharedState& shared, const Vehicle& vehicle, + QGCToolbox& toolbox) + : AirspaceManagerPerVehicle(vehicle), _shared(shared), + _flightManager(shared), _telemetry(shared), _trafficMonitor(shared), + _toolbox(toolbox) +{ + connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged, + this, &AirMapManagerPerVehicle::flightPermitStatusChanged); + connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged, + this, &AirMapManagerPerVehicle::_flightPermitStatusChanged); + connect(&_flightManager, &AirMapFlightManager::error, this, &AirMapManagerPerVehicle::error); + connect(&_telemetry, &AirMapTelemetry::error, this, &AirMapManagerPerVehicle::error); + connect(&_trafficMonitor, &AirMapTrafficMonitor::error, this, &AirMapManagerPerVehicle::error); + connect(&_trafficMonitor, &AirMapTrafficMonitor::trafficUpdate, this, &AirspaceManagerPerVehicle::trafficUpdate); +} + +void AirMapManagerPerVehicle::createFlight(const QList& missionItems) +{ + if (!_shared.client()) { + qCDebug(AirMapManagerLog) << "No AirMap client instance. Will not create a flight"; + return; + } + _flightManager.createFlight(missionItems); +} + +AirspaceAuthorization::PermitStatus AirMapManagerPerVehicle::flightPermitStatus() const +{ + return _flightManager.flightPermitStatus(); +} + +void AirMapManagerPerVehicle::startTelemetryStream() +{ + if (_flightManager.flightID() != "") { + _telemetry.startTelemetryStream(_flightManager.flightID()); + } +} + +void AirMapManagerPerVehicle::stopTelemetryStream() +{ + _telemetry.stopTelemetryStream(); +} + +bool AirMapManagerPerVehicle::isTelemetryStreaming() const +{ + return _telemetry.isTelemetryStreaming(); +} + +void AirMapManagerPerVehicle::endFlight() +{ + _flightManager.endFlight(); + _trafficMonitor.stop(); +} + +void AirMapManagerPerVehicle::vehicleMavlinkMessageReceived(const mavlink_message_t& message) +{ + if (isTelemetryStreaming()) { + _telemetry.vehicleMavlinkMessageReceived(message); + } +} + +void AirMapManagerPerVehicle::_flightPermitStatusChanged() +{ + // 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 + _trafficMonitor.startConnection(_flightManager.flightID()); + } +} + +AirMapManager::AirMapManager(QGCApplication* app, QGCToolbox* toolbox) + : AirspaceManager(app, toolbox) +{ + _logger = std::make_shared(); + qt::register_types(); // TODO: still needed? + _logger->logging_category().setEnabled(QtDebugMsg, true); + _logger->logging_category().setEnabled(QtInfoMsg, true); + _logger->logging_category().setEnabled(QtWarningMsg, true); + _dispatchingLogger = std::make_shared(_logger); + + connect(&_shared, &AirMapSharedState::error, this, &AirMapManager::_error); +} + +AirMapManager::~AirMapManager() +{ + if (_shared.client()) { + delete _shared.client(); + } +} + +void AirMapManager::setToolbox(QGCToolbox* toolbox) +{ + AirspaceManager::setToolbox(toolbox); + AirMapSettings* ap = toolbox->settingsManager()->airMapSettings(); + + connect(ap->apiKey(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); + connect(ap->clientID(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); + connect(ap->userName(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); + connect(ap->password(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); + + _settingsChanged(); + +} + +void AirMapManager::_error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails) +{ + qCDebug(AirMapManagerLog) << "Error: "<showMessage(QString("AirMap Error: %1. %2").arg(what).arg(airmapdMessage)); +} + +void AirMapManager::requestWeatherUpdate(const QGeoCoordinate& coordinate) +{ + if (!_shared.client()) { + qCDebug(AirMapManagerLog) << "No AirMap client instance. Not updating Weather information"; + emit weatherUpdate(false, QGeoCoordinate{}, WeatherInformation{}); + return; + } + + Status::GetStatus::Parameters params; + params.longitude = coordinate.longitude(); + params.latitude = coordinate.latitude(); + params.weather = true; + + _shared.client()->status().get_status_by_point(params, [this, coordinate](const Status::GetStatus::Result& result) { + + if (result) { + + const Status::Weather& weather = result.value().weather; + WeatherInformation weatherUpdateInfo; + + weatherUpdateInfo.condition = QString::fromStdString(weather.condition); + weatherUpdateInfo.icon = QString::fromStdString(weather.icon); + weatherUpdateInfo.windHeading = weather.wind.heading; + weatherUpdateInfo.windSpeed = weather.wind.speed; + weatherUpdateInfo.windGusting = weather.wind.gusting; + weatherUpdateInfo.temperature = weather.temperature; + weatherUpdateInfo.humidity = weather.humidity; + weatherUpdateInfo.visibility = weather.visibility; + weatherUpdateInfo.precipitation = weather.precipitation; + emit weatherUpdate(true, coordinate, weatherUpdateInfo); + + } else { + emit weatherUpdate(false, coordinate, WeatherInformation{}); + } + }); + +} + +void AirMapManager::_settingsChanged() +{ + qCDebug(AirMapManagerLog) << "AirMap settings changed"; + + AirMapSettings* ap = _toolbox->settingsManager()->airMapSettings(); + + AirMapSharedState::Settings settings; + settings.apiKey = ap->apiKey()->rawValueString(); + bool apiKeyChanged = settings.apiKey != _shared.settings().apiKey; + settings.clientID = ap->clientID()->rawValueString(); + settings.userName = ap->userName()->rawValueString(); + settings.password = ap->password()->rawValueString(); + _shared.setSettings(settings); + + // need to re-create the client if the API key changed + if (_shared.client() && apiKeyChanged) { + delete _shared.client(); + _shared.setClient(nullptr); + } + + + if (!_shared.client() && settings.apiKey != "") { + qCDebug(AirMapManagerLog) << "Creating AirMap client"; + + auto credentials = Credentials{}; + credentials.api_key = _shared.settings().apiKey.toStdString(); + auto configuration = Client::default_staging_configuration(credentials); + qt::Client::create(configuration, _dispatchingLogger, this, [this, ap](const qt::Client::CreateResult& result) { + if (result) { + qCDebug(AirMapManagerLog) << "Successfully created airmap::qt::Client instance"; + _shared.setClient(result.value()); + + } else { + qWarning("Failed to create airmap::qt::Client instance"); + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + _error("Failed to create airmap::qt::Client instance", + QString::fromStdString(result.error().message()), description); + } + }); + } +} + +AirspaceManagerPerVehicle* AirMapManager::instantiateVehicle(const Vehicle& vehicle) +{ + AirMapManagerPerVehicle* manager = new AirMapManagerPerVehicle(_shared, vehicle, *_toolbox); + connect(manager, &AirMapManagerPerVehicle::error, this, &AirMapManager::_error); + return manager; +} + +AirspaceRestrictionProvider* AirMapManager::instantiateRestrictionProvider() +{ + AirMapRestrictionManager* restrictionManager = new AirMapRestrictionManager(_shared); + connect(restrictionManager, &AirMapRestrictionManager::error, this, &AirMapManager::_error); + return restrictionManager; +} + diff --git a/src/Airmap/AirMapManager.h b/src/Airmap/AirMapManager.h new file mode 100644 index 0000000..99f85ee --- /dev/null +++ b/src/Airmap/AirMapManager.h @@ -0,0 +1,367 @@ +/**************************************************************************** + * + * (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 + +#include "QGCToolbox.h" +#include "QGCLoggingCategory.h" +#include "QmlObjectListModel.h" +#include "MissionItem.h" +#include "MultiVehicleManager.h" +#include "AirspaceManagement.h" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +Q_DECLARE_LOGGING_CATEGORY(AirMapManagerLog) + + +/** + * @class LifetimeChecker + * Base class which helps to check if an object instance still exists. + * A subclass can take a weak pointer from _instance and then check if the object was deleted. + * This is used in callbacks that access 'this', but the instance might already be deleted (e.g. vehicle disconnect). + */ +class LifetimeChecker +{ +public: + LifetimeChecker() : _instance(this, [](void*){}) { } + virtual ~LifetimeChecker() = default; + +protected: + std::shared_ptr _instance; +}; + +/** + * @class AirMapSharedState + * contains state & settings that need to be shared (such as login) + */ +class AirMapSharedState : public QObject +{ + Q_OBJECT +public: + struct Settings { + QString apiKey; + + // login credentials + QString clientID; + QString userName; ///< use anonymous login if empty + QString password; + }; + + void setSettings(const Settings& settings); + const Settings& settings() const { return _settings; } + + void setClient(airmap::qt::Client* client) { _client = client; } + + /** + * Get the current client instance. It can be NULL. If not NULL, it implies + * there's an API key set. + */ + airmap::qt::Client* client() const { return _client; } + + bool hasAPIKey() const { return _settings.apiKey != ""; } + + bool isLoggedIn() const { return _loginToken != ""; } + + using Callback = std::function; + + /** + * Do a request that requires user login: if not yet logged in, the request is queued and + * processed after successful login, otherwise it's executed directly. + */ + void doRequestWithLogin(const Callback& callback); + + void login(); + + void logout(); + + const QString& loginToken() const { return _loginToken; } + +signals: + void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); + +private: + void _processPendingRequests(); + + bool _isLoginInProgress = false; + QString _loginToken; ///< login token: empty when not logged in + + airmap::qt::Client* _client = nullptr; + + Settings _settings; + + QQueue _pendingRequests; ///< pending requests that are processed after a successful login +}; + + +/// class to download polygons from AirMap +class AirMapRestrictionManager : public AirspaceRestrictionProvider, public LifetimeChecker +{ + Q_OBJECT +public: + AirMapRestrictionManager(AirMapSharedState& shared); + + void setROI(const QGeoCoordinate& center, double radiusMeters) override; + +signals: + void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); + +private: + + static void _addPolygonToList(const airmap::Geometry::Polygon& polygon, QList& list); + + enum class State { + Idle, + RetrieveItems, + }; + + State _state = State::Idle; + AirMapSharedState& _shared; +}; + + +/// class to upload a flight +class AirMapFlightManager : public QObject, public LifetimeChecker +{ + Q_OBJECT +public: + AirMapFlightManager(AirMapSharedState& shared); + + /// Send flight path to AirMap + void createFlight(const QList& missionItems); + + AirspaceAuthorization::PermitStatus flightPermitStatus() const { return _flightPermitStatus; } + + const QString& flightID() const { return _currentFlightId; } + +public slots: + void endFlight(); + +signals: + void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); + void flightPermitStatusChanged(); + +private slots: + void _pollBriefing(); + +private: + + /** + * upload flight stored in _flight + */ + void _uploadFlight(); + + /** + * query the active flights and end the first one (because only a single flight can be active at a time). + */ + void _endFirstFlight(); + + /** + * implementation of endFlight() + */ + void _endFlight(const QString& flightID); + + /** + * check if the briefing response is valid and call _submitPendingFlightPlan() if it is. + */ + void _checkForValidBriefing(); + + void _submitPendingFlightPlan(); + + enum class State { + Idle, + GetPilotID, + FlightUpload, + FlightBrief, + FlightSubmit, + FlightPolling, // poll & check for approval + FlightEnd, + EndFirstFlight, // get a list of open flights & end the first one (because there can only be 1 active at a time) + }; + struct Flight { + QList coords; + QGeoCoordinate takeoffCoord; + float maxAltitude = 0; + + void reset() { + coords.clear(); + maxAltitude = 0; + } + }; + Flight _flight; ///< flight pending to be uploaded + + State _state = State::Idle; + AirMapSharedState& _shared; + QString _currentFlightId; ///< Flight ID, empty if there is none + QString _pendingFlightId; ///< current flight ID, not necessarily accepted yet (once accepted, it's equal to _currentFlightId) + QString _pendingFlightPlan; ///< current flight plan, waiting to be submitted + AirspaceAuthorization::PermitStatus _flightPermitStatus = AirspaceAuthorization::PermitUnknown; + QString _pilotID; ///< Pilot ID in the form "auth0|abc123" + bool _noFlightCreatedYet = true; + QTimer _pollTimer; ///< timer to poll for approval check +}; + +/// class to send telemetry data to AirMap +class AirMapTelemetry : public QObject, public LifetimeChecker +{ + Q_OBJECT +public: + AirMapTelemetry(AirMapSharedState& shared); + virtual ~AirMapTelemetry() = default; + + /** + * Setup the connection to start sending telemetry + */ + void startTelemetryStream(const QString& flightID); + + void stopTelemetryStream(); + + bool isTelemetryStreaming() const; + +signals: + void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); + +public slots: + void vehicleMavlinkMessageReceived(const mavlink_message_t& message); + +private: + + void _handleGlobalPositionInt(const mavlink_message_t& message); + void _handleGPSRawInt(const mavlink_message_t& message); + + enum class State { + Idle, + StartCommunication, + EndCommunication, + Streaming, + }; + + State _state = State::Idle; + + AirMapSharedState& _shared; + std::string _key; ///< key for AES encryption (16 bytes) + QString _flightID; + + float _lastHdop = 1.f; +}; + + +class AirMapTrafficMonitor : public QObject, public LifetimeChecker +{ + Q_OBJECT +public: + AirMapTrafficMonitor(AirMapSharedState& shared) + : _shared(shared) + { + } + virtual ~AirMapTrafficMonitor(); + + void startConnection(const QString& flightID); + + void stop(); + +signals: + void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); + void trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading); + +private: + void _update(airmap::Traffic::Update::Type type, const std::vector& update); + +private: + QString _flightID; + AirMapSharedState& _shared; + std::shared_ptr _monitor; + std::shared_ptr _subscriber; +}; + + + +/// AirMap per vehicle management class. +class AirMapManagerPerVehicle : public AirspaceManagerPerVehicle +{ + Q_OBJECT +public: + AirMapManagerPerVehicle(AirMapSharedState& shared, const Vehicle& vehicle, QGCToolbox& toolbox); + virtual ~AirMapManagerPerVehicle() = default; + + + void createFlight(const QList& missionItems) override; + + AirspaceAuthorization::PermitStatus flightPermitStatus() const override; + + void startTelemetryStream() override; + + void stopTelemetryStream() override; + + bool isTelemetryStreaming() const override; + +signals: + void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); + +public slots: + void endFlight() override; + +protected slots: + virtual void vehicleMavlinkMessageReceived(const mavlink_message_t& message) override; +private slots: + void _flightPermitStatusChanged(); +private: + AirMapSharedState& _shared; + + AirMapFlightManager _flightManager; + AirMapTelemetry _telemetry; + AirMapTrafficMonitor _trafficMonitor; + + QGCToolbox& _toolbox; +}; + + +class AirMapManager : public AirspaceManager +{ + Q_OBJECT + +public: + AirMapManager(QGCApplication* app, QGCToolbox* toolbox); + virtual ~AirMapManager(); + + void setToolbox(QGCToolbox* toolbox) override; + + AirspaceManagerPerVehicle* instantiateVehicle(const Vehicle& vehicle) override; + + AirspaceRestrictionProvider* instantiateRestrictionProvider() override; + + QString name() const override { return "AirMap"; } + + void requestWeatherUpdate(const QGeoCoordinate& coordinate) override; + +private slots: + void _error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); + + void _settingsChanged(); +private: + + AirMapSharedState _shared; + + std::shared_ptr _logger; + std::shared_ptr _dispatchingLogger; +}; + + diff --git a/src/Airmap/AirMapSettings.cc b/src/Airmap/AirMapSettings.cc new file mode 100644 index 0000000..a2cc7ce --- /dev/null +++ b/src/Airmap/AirMapSettings.cc @@ -0,0 +1,30 @@ +/**************************************************************************** + * + * (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. + * + ****************************************************************************/ + +#include "AirMapSettings.h" +#include "QGCPalette.h" +#include "QGCApplication.h" + +#include +#include + +DECLARE_SETTINGGROUP(AirMap) +{ + INIT_SETTINGFACT(apiKey); + INIT_SETTINGFACT(clientID); + INIT_SETTINGFACT(userName); + INIT_SETTINGFACT(password); + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + qmlRegisterUncreatableType("QGroundControl.SettingsManager", 1, 0, "AirMapSettings", "Reference only"); +} + +DECLARE_SETTINGSFACT(AirMapSettings, apiKey) +DECLARE_SETTINGSFACT(AirMapSettings, clientID) +DECLARE_SETTINGSFACT(AirMapSettings, userName) +DECLARE_SETTINGSFACT(AirMapSettings, password) diff --git a/src/Airmap/AirMapSettings.h b/src/Airmap/AirMapSettings.h new file mode 100644 index 0000000..04542d5 --- /dev/null +++ b/src/Airmap/AirMapSettings.h @@ -0,0 +1,28 @@ +/**************************************************************************** + * + * (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 "SettingsGroup.h" +#include "QGCMAVLink.h" + +class AirMapSettings : public SettingsGroup +{ + Q_OBJECT +public: + AirMapSettings(QObject* parent = NULL); + + DEFINE_SETTINGGROUP(AirMap) + + DEFINE_SETTINGFACT(apiKey) + DEFINE_SETTINGFACT(clientID) + DEFINE_SETTINGFACT(userName) + DEFINE_SETTINGFACT(password) + +}; diff --git a/src/Airmap/AirspaceController.cc b/src/Airmap/AirspaceController.cc new file mode 100644 index 0000000..d81d763 --- /dev/null +++ b/src/Airmap/AirspaceController.cc @@ -0,0 +1,20 @@ +/**************************************************************************** + * + * (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. + * + ****************************************************************************/ + +#include "AirspaceController.h" +#include "AirspaceManagement.h" +#include "QGCApplication.h" +#include "QGCQGeoCoordinate.h" + +AirspaceController::AirspaceController(QObject* parent) + : QObject(parent) + , _manager(qgcApp()->toolbox()->airspaceManager()) +{ +} + diff --git a/src/Airmap/AirspaceController.h b/src/Airmap/AirspaceController.h new file mode 100644 index 0000000..b97c6a8 --- /dev/null +++ b/src/Airmap/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/Airmap/AirspaceManagement.cc b/src/Airmap/AirspaceManagement.cc new file mode 100644 index 0000000..6b0aa1d --- /dev/null +++ b/src/Airmap/AirspaceManagement.cc @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * (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"); + qRegisterMetaType(); +} + +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/Airmap/AirspaceManagement.h b/src/Airmap/AirspaceManagement.h new file mode 100644 index 0000000..1a17b6e --- /dev/null +++ b/src/Airmap/AirspaceManagement.h @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * (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; + +struct WeatherInformation +{ + QString condition; ///< The overall weather condition. + QString icon; ///< The icon or class of icon that should be used for display purposes. + uint32_t windHeading = 0; ///< The heading in [°]. + uint32_t windSpeed = 0; ///< The speed in [°]. + uint32_t windGusting = 0; + int32_t temperature = 0; ///< The temperature in [°C]. + float humidity = 0.0; + uint32_t visibility = 0; ///< Visibility in [m]. + uint32_t precipitation = 0; ///< The probability of precipitation in [%]. +}; +Q_DECLARE_METATYPE(WeatherInformation); + +/** + * @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; + + /** + * Request weather information update. When done, it will emit the weatherUpdate() signal. + * @param coordinate request update for this coordinate + */ + virtual void requestWeatherUpdate(const QGeoCoordinate& coordinate) = 0; + +signals: + void weatherUpdate(bool success, QGeoCoordinate coordinate, WeatherInformation weather); + +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/Airmap/airmap.qrc b/src/Airmap/airmap.qrc new file mode 100644 index 0000000..5130d4e --- /dev/null +++ b/src/Airmap/airmap.qrc @@ -0,0 +1,17 @@ + + + QGroundControl.Airmap.qmldir + AirspaceControl.qml + AirspaceRegulation.qml + AirmapSettings.qml + + + AirMap.SettingsGroup.json + + + images/advisory-icon.svg + images/colapse.svg + images/expand.svg + images/pencil.svg + + diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 3542909..d860fc3 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -633,7 +633,7 @@ QGCView { 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 + visible: QGroundControl.airmapSupported && _mainIsMap && flightPermit && flightPermit !== AirspaceAuthorization.PermitUnknown && !messageArea.visible && !criticalMmessageArea.visible radius: 3 border.width: 1 border.color: qgcPal.globalTheme === QGCPalette.Light ? Qt.rgba(0,0,0,0.35) : Qt.rgba(1,1,1,0.35) @@ -669,8 +669,8 @@ QGCView { anchors.verticalCenter: parent.verticalCenter; } } - property var flightPermit: _activeVehicle ? _activeVehicle.flightPermitStatus : null - property var providerName: _activeVehicle ? _activeVehicle.airspaceController.providerName : "" + property var flightPermit: (QGroundControl.airmapSupported && _activeVehicle) ? _activeVehicle.flightPermitStatus : null + property var providerName: _activeVehicle ? _activeVehicle.airspaceController.providerName : "" } } diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 281d789..87ecb3d 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -58,7 +58,7 @@ FlightMap { // Track last known map position and zoom from Fly view in settings onZoomLevelChanged: QGroundControl.flightMapZoom = zoomLevel onCenterChanged: { - if(_activeVehicle) { + if(_activeVehicle && QGroundControl.airmapSupported) { _activeVehicle.airspaceController.setROI(center, 5000) } QGroundControl.flightMapPosition = center @@ -331,7 +331,7 @@ FlightMap { // Airspace overlap support MapItemView { - model: _activeVehicle ? _activeVehicle.airspaceController.circles : [] + model: QGroundControl.airmapSupported && _activeVehicle ? _activeVehicle.airspaceController.circles : [] delegate: MapCircle { center: object.center radius: object.radius @@ -342,7 +342,7 @@ FlightMap { } MapItemView { - model: _activeVehicle ? _activeVehicle.airspaceController.polygons : [] + model: QGroundControl.airmapSupported && _activeVehicle ? _activeVehicle.airspaceController.polygons : [] delegate: MapPolygon { border.color: "white" color: "yellow" diff --git a/src/MissionManager/AirMapManager.cc b/src/MissionManager/AirMapManager.cc deleted file mode 100644 index d75772a..0000000 --- a/src/MissionManager/AirMapManager.cc +++ /dev/null @@ -1,981 +0,0 @@ -/**************************************************************************** - * - * (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 "AirMapManager.h" -#include "Vehicle.h" -#include "QmlObjectListModel.h" -#include "JsonHelper.h" -#include "SettingsManager.h" -#include "AppSettings.h" -#include "AirMapSettings.h" -#include "QGCQGeoCoordinate.h" -#include "QGCApplication.h" - -#include -#include -#include -#include -#include -#include -#include - -using namespace airmap; - -QGC_LOGGING_CATEGORY(AirMapManagerLog, "AirMapManagerLog") - - -void AirMapSharedState::setSettings(const Settings& settings) -{ - logout(); - _settings = settings; -} - -void AirMapSharedState::doRequestWithLogin(const Callback& callback) -{ - if (isLoggedIn()) { - callback(_loginToken); - } else { - login(); - _pendingRequests.enqueue(callback); - } -} - -void AirMapSharedState::login() -{ - if (isLoggedIn() || _isLoginInProgress) { - return; - } - _isLoginInProgress = true; - - if (_settings.userName == "") { //use anonymous login - - Authenticator::AuthenticateAnonymously::Params params; - params.id = ""; - _client->authenticator().authenticate_anonymously(params, - [this](const Authenticator::AuthenticateAnonymously::Result& result) { - if (!_isLoginInProgress) { // was logout() called in the meanwhile? - return; - } - if (result) { - qCDebug(AirMapManagerLog)<<"Successfully authenticated with AirMap: id="<< result.value().id.c_str(); - _loginToken = QString::fromStdString(result.value().id); - _processPendingRequests(); - } else { - _pendingRequests.clear(); - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Failed to authenticate with AirMap", - QString::fromStdString(result.error().message()), description); - } - }); - - } else { - - Authenticator::AuthenticateWithPassword::Params params; - params.oauth.username = _settings.userName.toStdString(); - params.oauth.password = _settings.password.toStdString(); - params.oauth.client_id = _settings.clientID.toStdString(); - params.oauth.device_id = "QGroundControl"; - _client->authenticator().authenticate_with_password(params, - [this](const Authenticator::AuthenticateWithPassword::Result& result) { - if (!_isLoginInProgress) { // was logout() called in the meanwhile? - return; - } - if (result) { - qCDebug(AirMapManagerLog)<<"Successfully authenticated with AirMap: id="<< result.value().id.c_str()<<", access=" - < isAlive(_instance); - _shared.client()->airspaces().search(params, - [this, isAlive](const Airspaces::Search::Result& result) { - - if (!isAlive.lock()) return; - if (_state != State::RetrieveItems) return; - - if (result) { - const std::vector& airspaces = result.value(); - qCDebug(AirMapManagerLog)<<"Successful search. Items:" << airspaces.size(); - for (const auto& airspace : airspaces) { - - const Geometry& geometry = airspace.geometry(); - switch(geometry.type()) { - case Geometry::Type::polygon: { - const Geometry::Polygon& polygon = geometry.details_for_polygon(); - _addPolygonToList(polygon, _polygonList); - } - break; - case Geometry::Type::multi_polygon: { - const Geometry::MultiPolygon& multiPolygon = geometry.details_for_multi_polygon(); - for (const auto& polygon : multiPolygon) { - _addPolygonToList(polygon, _polygonList); - } - } - break; - case Geometry::Type::point: { - const Geometry::Point& point = geometry.details_for_point(); - _circleList.append(new CircularAirspaceRestriction(QGeoCoordinate(point.latitude, point.longitude), 0.)); - // TODO: radius??? - } - break; - default: - qWarning() << "unsupported geometry type: "<<(int)geometry.type(); - break; - } - - } - - } else { - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Failed to retrieve Geofences", - QString::fromStdString(result.error().message()), description); - } - emit requestDone(true); - _state = State::Idle; - }); -} - -void AirMapRestrictionManager::_addPolygonToList(const airmap::Geometry::Polygon& polygon, QList& list) -{ - QVariantList polygonArray; - for (const auto& vertex : polygon.outer_ring.coordinates) { - QGeoCoordinate coord; - if (vertex.altitude) { - coord = QGeoCoordinate(vertex.latitude, vertex.longitude, vertex.altitude.get()); - } else { - coord = QGeoCoordinate(vertex.latitude, vertex.longitude); - } - polygonArray.append(QVariant::fromValue(coord)); - } - list.append(new PolygonAirspaceRestriction(polygonArray)); - - if (polygon.inner_rings.size() > 0) { - // no need to support those (they are rare, and in most cases, there's a more restrictive polygon filling the hole) - qCDebug(AirMapManagerLog) << "Polygon with holes. Size: "<& missionItems) -{ - if (!_shared.client()) { - qCDebug(AirMapManagerLog) << "No AirMap client instance. Will not create a flight"; - return; - } - - if (_state != State::Idle) { - qCWarning(AirMapManagerLog) << "AirMapFlightManager::createFlight: State not idle"; - return; - } - - _flight.reset(); - - // get the flight trajectory - for(const auto &item : missionItems) { - switch(item->command()) { - case MAV_CMD_NAV_WAYPOINT: - case MAV_CMD_NAV_LAND: - case MAV_CMD_NAV_TAKEOFF: - // TODO: others too? - { - // TODO: handle different coordinate frames? - double lat = item->param5(); - double lon = item->param6(); - double alt = item->param7(); - _flight.coords.append(QGeoCoordinate(lat, lon, alt)); - if (alt > _flight.maxAltitude) { - _flight.maxAltitude = alt; - } - if (item->command() == MAV_CMD_NAV_TAKEOFF) { - _flight.takeoffCoord = _flight.coords.last(); - } - } - break; - default: - break; - } - } - if (_flight.coords.empty()) { - return; - } - - _flight.maxAltitude += 5; // add a safety buffer - - - if (_pilotID == "") { - // need to get the pilot id before uploading the flight - qCDebug(AirMapManagerLog) << "Getting pilot ID"; - _state = State::GetPilotID; - std::weak_ptr isAlive(_instance); - _shared.doRequestWithLogin([this, isAlive](const QString& login_token) { - if (!isAlive.lock()) return; - - Pilots::Authenticated::Parameters params; - params.authorization = login_token.toStdString(); - _shared.client()->pilots().authenticated(params, [this, isAlive](const Pilots::Authenticated::Result& result) { - - if (!isAlive.lock()) return; - if (_state != State::GetPilotID) return; - - if (result) { - _pilotID = QString::fromStdString(result.value().id); - qCDebug(AirMapManagerLog) << "Got Pilot ID:"<<_pilotID; - _uploadFlight(); - } else { - _flightPermitStatus = AirspaceAuthorization::PermitUnknown; - emit flightPermitStatusChanged(); - _state = State::Idle; - - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Failed to create Flight Plan", - QString::fromStdString(result.error().message()), description); - } - }); - }); - } else { - _uploadFlight(); - } - - _flightPermitStatus = AirspaceAuthorization::PermitPending; - emit flightPermitStatusChanged(); -} - -void AirMapFlightManager::_endFirstFlight() -{ - // it could be that AirMap still has an open pending flight, but we don't know the flight ID. - // As there can only be one, we query the flights that end in the future, and close it if there's one. - - _state = State::EndFirstFlight; - - Flights::Search::Parameters params; - params.pilot_id = _pilotID.toStdString(); - params.end_after = Clock::universal_time() - Hours{1}; - - std::weak_ptr isAlive(_instance); - _shared.client()->flights().search(params, [this, isAlive](const Flights::Search::Result& result) { - if (!isAlive.lock()) return; - if (_state != State::EndFirstFlight) return; - - if (result && result.value().flights.size() > 0) { - - Q_ASSERT(_shared.loginToken() != ""); // at this point we know the user is logged in (we queried the pilot id) - - Flights::EndFlight::Parameters params; - params.authorization = _shared.loginToken().toStdString(); - params.id = result.value().flights[0].id; // pick the first flight (TODO: match the vehicle id) - _shared.client()->flights().end_flight(params, [this, isAlive](const Flights::EndFlight::Result& result) { - if (!isAlive.lock()) return; - if (_state != State::EndFirstFlight) return; - - if (!result) { - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Failed to end first Flight", - QString::fromStdString(result.error().message()), description); - } - _state = State::Idle; - _uploadFlight(); - }); - - } else { - _state = State::Idle; - _uploadFlight(); - } - }); -} - -void AirMapFlightManager::_uploadFlight() -{ - if (_pendingFlightId != "") { - // we need to end an existing flight first - _endFlight(_pendingFlightId); - return; - } - - if (_noFlightCreatedYet) { - _endFirstFlight(); - _noFlightCreatedYet = false; - return; - } - - qCDebug(AirMapManagerLog) << "uploading flight"; - _state = State::FlightUpload; - - std::weak_ptr isAlive(_instance); - _shared.doRequestWithLogin([this, isAlive](const QString& login_token) { - - if (!isAlive.lock()) return; - if (_state != State::FlightUpload) return; - - FlightPlans::Create::Parameters params; - params.max_altitude = _flight.maxAltitude; - params.buffer = 2.f; - params.latitude = _flight.takeoffCoord.latitude(); - params.longitude = _flight.takeoffCoord.longitude(); - params.pilot.id = _pilotID.toStdString(); - params.start_time = Clock::universal_time() + Minutes{5}; - params.end_time = Clock::universal_time() + Hours{2}; // TODO: user-configurable? - params.rulesets = { // TODO: which ones to use? - "che_drone_rules", - "che_notam", - "che_airmap_rules", - "che_nature_preserve" - }; - - // geometry: LineString - Geometry::LineString lineString; - for (const auto& qcoord : _flight.coords) { - Geometry::Coordinate coord; - coord.latitude = qcoord.latitude(); - coord.longitude = qcoord.longitude(); - lineString.coordinates.push_back(coord); - } - - params.geometry = Geometry(lineString); - params.authorization = login_token.toStdString(); - _flight.coords.clear(); - - _shared.client()->flight_plans().create_by_polygon(params, [this, isAlive](const FlightPlans::Create::Result& result) { - if (!isAlive.lock()) return; - if (_state != State::FlightUpload) return; - - if (result) { - _pendingFlightPlan = QString::fromStdString(result.value().id); - qCDebug(AirMapManagerLog) << "Flight Plan created:"<<_pendingFlightPlan; - - _checkForValidBriefing(); - - } else { - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Flight Plan creation failed", - QString::fromStdString(result.error().message()), description); - } - - }); - - }); -} - -void AirMapFlightManager::_checkForValidBriefing() -{ - _state = State::FlightBrief; - FlightPlans::RenderBriefing::Parameters params; - params.authorization = _shared.loginToken().toStdString(); - params.id = _pendingFlightPlan.toStdString(); - std::weak_ptr isAlive(_instance); - _shared.client()->flight_plans().render_briefing(params, [this, isAlive](const FlightPlans::RenderBriefing::Result& result) { - if (!isAlive.lock()) return; - if (_state != State::FlightBrief) return; - - if (result) { - bool allValid = true; - for (const auto& validation : result.value().evaluation.validations) { - if (validation.status != Evaluation::Validation::Status::valid) { - emit error(QString("%1 registration identifier is invalid: %2").arg( - QString::fromStdString(validation.authority.name)).arg(QString::fromStdString(validation.message)), "", ""); - allValid = false; - } - } - if (allValid) { - _submitPendingFlightPlan(); - } else { - _flightPermitStatus = AirspaceAuthorization::PermitRejected; - emit flightPermitStatusChanged(); - _state = State::Idle; - } - - } else { - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Brief Request failed", - QString::fromStdString(result.error().message()), description); - _state = State::Idle; - } - }); -} - -void AirMapFlightManager::_submitPendingFlightPlan() -{ - _state = State::FlightSubmit; - FlightPlans::Submit::Parameters params; - params.authorization = _shared.loginToken().toStdString(); - params.id = _pendingFlightPlan.toStdString(); - std::weak_ptr isAlive(_instance); - _shared.client()->flight_plans().submit(params, [this, isAlive](const FlightPlans::Submit::Result& result) { - if (!isAlive.lock()) return; - if (_state != State::FlightSubmit) return; - - if (result) { - _pendingFlightId = QString::fromStdString(result.value().flight_id.get()); - _state = State::FlightPolling; - _pollBriefing(); - - } else { - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Failed to submit Flight Plan", - QString::fromStdString(result.error().message()), description); - _state = State::Idle; - } - }); -} - -void AirMapFlightManager::_pollBriefing() -{ - if (_state != State::FlightPolling) { - qCWarning(AirMapManagerLog) << "AirMapFlightManager::_pollBriefing: not in polling state"; - return; - } - - FlightPlans::RenderBriefing::Parameters params; - params.authorization = _shared.loginToken().toStdString(); - params.id = _pendingFlightPlan.toStdString(); - std::weak_ptr isAlive(_instance); - _shared.client()->flight_plans().render_briefing(params, [this, isAlive](const FlightPlans::RenderBriefing::Result& result) { - if (!isAlive.lock()) return; - if (_state != State::FlightPolling) return; - - if (result) { - const FlightPlan::Briefing& briefing = result.value(); - qCDebug(AirMapManagerLog) << "flight polling/briefing response"; - bool rejected = false; - bool accepted = false; - bool pending = false; - for (const auto& authorization : briefing.evaluation.authorizations) { - switch (authorization.status) { - case Evaluation::Authorization::Status::accepted: - case Evaluation::Authorization::Status::accepted_upon_submission: - accepted = true; - break; - case Evaluation::Authorization::Status::rejected: - case Evaluation::Authorization::Status::rejected_upon_submission: - rejected = true; - break; - case Evaluation::Authorization::Status::pending: - pending = true; - break; - } - } - - if (briefing.evaluation.authorizations.size() == 0) { - // if we don't get any authorizations, we assume it's accepted - accepted = true; - } - - qCDebug(AirMapManagerLog) << "flight approval: accepted=" << accepted << "rejected" << rejected << "pending" << pending; - - if ((rejected || accepted) && !pending) { - if (rejected) { // rejected has priority - _flightPermitStatus = AirspaceAuthorization::PermitRejected; - } else { - _flightPermitStatus = AirspaceAuthorization::PermitAccepted; - } - _currentFlightId = _pendingFlightId; - _pendingFlightPlan = ""; - emit flightPermitStatusChanged(); - _state = State::Idle; - } else { - // wait until we send the next polling request - _pollTimer.setSingleShot(true); - _pollTimer.start(2000); - } - } else { - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Brief Request failed", - QString::fromStdString(result.error().message()), description); - _state = State::Idle; - } - }); -} - -void AirMapFlightManager::endFlight() -{ - if (_currentFlightId.length() == 0) { - return; - } - if (_state != State::Idle) { - qCWarning(AirMapManagerLog) << "AirMapFlightManager::endFlight: State not idle"; - return; - } - _endFlight(_currentFlightId); - - _flightPermitStatus = AirspaceAuthorization::PermitUnknown; - emit flightPermitStatusChanged(); -} - -void AirMapFlightManager::_endFlight(const QString& flightID) -{ - qCDebug(AirMapManagerLog) << "ending flight" << flightID; - - _state = State::FlightEnd; - - Q_ASSERT(_shared.loginToken() != ""); // Since we have a flight ID, we need to be logged in - - Flights::EndFlight::Parameters params; - params.authorization = _shared.loginToken().toStdString(); - params.id = flightID.toStdString(); - std::weak_ptr isAlive(_instance); - _shared.client()->flights().end_flight(params, [this, isAlive](const Flights::EndFlight::Result& result) { - if (!isAlive.lock()) return; - if (_state != State::FlightEnd) return; - - _state = State::Idle; - _pendingFlightId = ""; - _pendingFlightPlan = ""; - _currentFlightId = ""; - if (result) { - if (!_flight.coords.empty()) { - _uploadFlight(); - } - } else { - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Failed to end Flight", - QString::fromStdString(result.error().message()), description); - } - }); -} - - -AirMapTelemetry::AirMapTelemetry(AirMapSharedState& shared) -: _shared(shared) -{ -} - -void AirMapTelemetry::vehicleMavlinkMessageReceived(const mavlink_message_t& message) -{ - switch (message.msgid) { - case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: - _handleGlobalPositionInt(message); - break; - case MAVLINK_MSG_ID_GPS_RAW_INT: - _handleGPSRawInt(message); - break; - } - -} - -bool AirMapTelemetry::isTelemetryStreaming() const -{ - return _state == State::Streaming; -} - -void AirMapTelemetry::_handleGPSRawInt(const mavlink_message_t& message) -{ - if (!isTelemetryStreaming()) { - return; - } - - mavlink_gps_raw_int_t gps_raw; - mavlink_msg_gps_raw_int_decode(&message, &gps_raw); - - if (gps_raw.eph == UINT16_MAX) { - _lastHdop = 1.f; - } else { - _lastHdop = gps_raw.eph / 100.f; - } -} - -void AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message) -{ - if (!isTelemetryStreaming()) { - return; - } - - mavlink_global_position_int_t globalPosition; - mavlink_msg_global_position_int_decode(&message, &globalPosition); - - Telemetry::Position position{ - milliseconds_since_epoch(Clock::universal_time()), - (double) globalPosition.lat / 1e7, - (double) globalPosition.lon / 1e7, - (float) globalPosition.alt / 1000.f, - (float) globalPosition.relative_alt / 1000.f, - _lastHdop - }; - Telemetry::Speed speed{ - milliseconds_since_epoch(Clock::universal_time()), - globalPosition.vx / 100.f, - globalPosition.vy / 100.f, - globalPosition.vz / 100.f - }; - - Flight flight; - flight.id = _flightID.toStdString(); - _shared.client()->telemetry().submit_updates(flight, _key, - {Telemetry::Update{position}, Telemetry::Update{speed}}); - -} - -void AirMapTelemetry::startTelemetryStream(const QString& flightID) -{ - if (_state != State::Idle) { - qCWarning(AirMapManagerLog) << "Not starting telemetry: not in idle state:" << (int)_state; - return; - } - - qCInfo(AirMapManagerLog) << "Starting Telemetry stream with flightID" << flightID; - - _state = State::StartCommunication; - _flightID = flightID; - - Flights::StartFlightCommunications::Parameters params; - params.authorization = _shared.loginToken().toStdString(); - params.id = _flightID.toStdString(); - std::weak_ptr isAlive(_instance); - _shared.client()->flights().start_flight_communications(params, [this, isAlive](const Flights::StartFlightCommunications::Result& result) { - if (!isAlive.lock()) return; - if (_state != State::StartCommunication) return; - - if (result) { - _key = result.value().key; - _state = State::Streaming; - } else { - _state = State::Idle; - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Failed to start telemetry streaming", - QString::fromStdString(result.error().message()), description); - } - }); -} - -void AirMapTelemetry::stopTelemetryStream() -{ - if (_state == State::Idle) { - return; - } - qCInfo(AirMapManagerLog) << "Stopping Telemetry stream with flightID" << _flightID; - - _state = State::EndCommunication; - Flights::EndFlightCommunications::Parameters params; - params.authorization = _shared.loginToken().toStdString(); - params.id = _flightID.toStdString(); - std::weak_ptr isAlive(_instance); - _shared.client()->flights().end_flight_communications(params, [this, isAlive](const Flights::EndFlightCommunications::Result& result) { - Q_UNUSED(result); - if (!isAlive.lock()) return; - if (_state != State::EndCommunication) return; - - _key = ""; - _state = State::Idle; - }); -} - -AirMapTrafficMonitor::~AirMapTrafficMonitor() -{ - stop(); -} - -void AirMapTrafficMonitor::startConnection(const QString& flightID) -{ - _flightID = flightID; - std::weak_ptr isAlive(_instance); - auto handler = [this, isAlive](const Traffic::Monitor::Result& result) { - if (!isAlive.lock()) return; - if (result) { - _monitor = result.value(); - _subscriber = std::make_shared( - std::bind(&AirMapTrafficMonitor::_update, this, std::placeholders::_1, std::placeholders::_2)); - _monitor->subscribe(_subscriber); - } else { - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Failed to start Traffic Monitoring", - QString::fromStdString(result.error().message()), description); - } - }; - - Traffic::Monitor::Params params{flightID.toStdString(), _shared.loginToken().toStdString()}; - _shared.client()->traffic().monitor(params, handler); -} - -void AirMapTrafficMonitor::_update(Traffic::Update::Type type, const std::vector& update) -{ - qCDebug(AirMapManagerLog) << "Traffic update with" << update.size() << "elements"; - - if (type != Traffic::Update::Type::situational_awareness) - return; // currently we're only interested in situational awareness - - for (const auto& traffic : update) { - QString traffic_id = QString::fromStdString(traffic.id); - QString vehicle_id = QString::fromStdString(traffic.aircraft_id); - emit trafficUpdate(traffic_id, vehicle_id, QGeoCoordinate(traffic.latitude, traffic.longitude, traffic.altitude), - traffic.heading); - } -} - -void AirMapTrafficMonitor::stop() -{ - if (_monitor) { - _monitor->unsubscribe(_subscriber); - _subscriber.reset(); - _monitor.reset(); - } -} - -AirMapManagerPerVehicle::AirMapManagerPerVehicle(AirMapSharedState& shared, const Vehicle& vehicle, - QGCToolbox& toolbox) - : AirspaceManagerPerVehicle(vehicle), _shared(shared), - _flightManager(shared), _telemetry(shared), _trafficMonitor(shared), - _toolbox(toolbox) -{ - connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged, - this, &AirMapManagerPerVehicle::flightPermitStatusChanged); - connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged, - this, &AirMapManagerPerVehicle::_flightPermitStatusChanged); - connect(&_flightManager, &AirMapFlightManager::error, this, &AirMapManagerPerVehicle::error); - connect(&_telemetry, &AirMapTelemetry::error, this, &AirMapManagerPerVehicle::error); - connect(&_trafficMonitor, &AirMapTrafficMonitor::error, this, &AirMapManagerPerVehicle::error); - connect(&_trafficMonitor, &AirMapTrafficMonitor::trafficUpdate, this, &AirspaceManagerPerVehicle::trafficUpdate); -} - -void AirMapManagerPerVehicle::createFlight(const QList& missionItems) -{ - if (!_shared.client()) { - qCDebug(AirMapManagerLog) << "No AirMap client instance. Will not create a flight"; - return; - } - _flightManager.createFlight(missionItems); -} - -AirspaceAuthorization::PermitStatus AirMapManagerPerVehicle::flightPermitStatus() const -{ - return _flightManager.flightPermitStatus(); -} - -void AirMapManagerPerVehicle::startTelemetryStream() -{ - if (_flightManager.flightID() != "") { - _telemetry.startTelemetryStream(_flightManager.flightID()); - } -} - -void AirMapManagerPerVehicle::stopTelemetryStream() -{ - _telemetry.stopTelemetryStream(); -} - -bool AirMapManagerPerVehicle::isTelemetryStreaming() const -{ - return _telemetry.isTelemetryStreaming(); -} - -void AirMapManagerPerVehicle::endFlight() -{ - _flightManager.endFlight(); - _trafficMonitor.stop(); -} - -void AirMapManagerPerVehicle::vehicleMavlinkMessageReceived(const mavlink_message_t& message) -{ - if (isTelemetryStreaming()) { - _telemetry.vehicleMavlinkMessageReceived(message); - } -} - -void AirMapManagerPerVehicle::_flightPermitStatusChanged() -{ - // 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 - _trafficMonitor.startConnection(_flightManager.flightID()); - } -} - -AirMapManager::AirMapManager(QGCApplication* app, QGCToolbox* toolbox) - : AirspaceManager(app, toolbox) -{ - _logger = std::make_shared(); - qt::register_types(); // TODO: still needed? - _logger->logging_category().setEnabled(QtDebugMsg, true); - _logger->logging_category().setEnabled(QtInfoMsg, true); - _logger->logging_category().setEnabled(QtWarningMsg, true); - _dispatchingLogger = std::make_shared(_logger); - - connect(&_shared, &AirMapSharedState::error, this, &AirMapManager::_error); -} - -AirMapManager::~AirMapManager() -{ - if (_shared.client()) { - delete _shared.client(); - } -} - -void AirMapManager::setToolbox(QGCToolbox* toolbox) -{ - AirspaceManager::setToolbox(toolbox); - AirMapSettings* ap = toolbox->settingsManager()->airMapSettings(); - - connect(ap->apiKey(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); - connect(ap->clientID(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); - connect(ap->userName(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); - connect(ap->password(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); - - _settingsChanged(); - -} - -void AirMapManager::_error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails) -{ - qCDebug(AirMapManagerLog) << "Error: "<showMessage(QString("AirMap Error: %1. %2").arg(what).arg(airmapdMessage)); -} - -void AirMapManager::requestWeatherUpdate(const QGeoCoordinate& coordinate) -{ - if (!_shared.client()) { - qCDebug(AirMapManagerLog) << "No AirMap client instance. Not updating Weather information"; - emit weatherUpdate(false, QGeoCoordinate{}, WeatherInformation{}); - return; - } - - Status::GetStatus::Parameters params; - params.longitude = coordinate.longitude(); - params.latitude = coordinate.latitude(); - params.weather = true; - - _shared.client()->status().get_status_by_point(params, [this, coordinate](const Status::GetStatus::Result& result) { - - if (result) { - - const Status::Weather& weather = result.value().weather; - WeatherInformation weatherUpdateInfo; - - weatherUpdateInfo.condition = QString::fromStdString(weather.condition); - weatherUpdateInfo.icon = QString::fromStdString(weather.icon); - weatherUpdateInfo.windHeading = weather.wind.heading; - weatherUpdateInfo.windSpeed = weather.wind.speed; - weatherUpdateInfo.windGusting = weather.wind.gusting; - weatherUpdateInfo.temperature = weather.temperature; - weatherUpdateInfo.humidity = weather.humidity; - weatherUpdateInfo.visibility = weather.visibility; - weatherUpdateInfo.precipitation = weather.precipitation; - emit weatherUpdate(true, coordinate, weatherUpdateInfo); - - } else { - emit weatherUpdate(false, coordinate, WeatherInformation{}); - } - }); - -} - -void AirMapManager::_settingsChanged() -{ - qCDebug(AirMapManagerLog) << "AirMap settings changed"; - - AirMapSettings* ap = _toolbox->settingsManager()->airMapSettings(); - - AirMapSharedState::Settings settings; - settings.apiKey = ap->apiKey()->rawValueString(); - bool apiKeyChanged = settings.apiKey != _shared.settings().apiKey; - settings.clientID = ap->clientID()->rawValueString(); - settings.userName = ap->userName()->rawValueString(); - settings.password = ap->password()->rawValueString(); - _shared.setSettings(settings); - - // need to re-create the client if the API key changed - if (_shared.client() && apiKeyChanged) { - delete _shared.client(); - _shared.setClient(nullptr); - } - - - if (!_shared.client() && settings.apiKey != "") { - qCDebug(AirMapManagerLog) << "Creating AirMap client"; - - auto credentials = Credentials{}; - credentials.api_key = _shared.settings().apiKey.toStdString(); - auto configuration = Client::default_staging_configuration(credentials); - qt::Client::create(configuration, _dispatchingLogger, this, [this, ap](const qt::Client::CreateResult& result) { - if (result) { - qCDebug(AirMapManagerLog) << "Successfully created airmap::qt::Client instance"; - _shared.setClient(result.value()); - - } else { - qWarning("Failed to create airmap::qt::Client instance"); - QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - _error("Failed to create airmap::qt::Client instance", - QString::fromStdString(result.error().message()), description); - } - }); - } -} - -AirspaceManagerPerVehicle* AirMapManager::instantiateVehicle(const Vehicle& vehicle) -{ - AirMapManagerPerVehicle* manager = new AirMapManagerPerVehicle(_shared, vehicle, *_toolbox); - connect(manager, &AirMapManagerPerVehicle::error, this, &AirMapManager::_error); - return manager; -} - -AirspaceRestrictionProvider* AirMapManager::instantiateRestrictionProvider() -{ - AirMapRestrictionManager* restrictionManager = new AirMapRestrictionManager(_shared); - connect(restrictionManager, &AirMapRestrictionManager::error, this, &AirMapManager::_error); - return restrictionManager; -} - diff --git a/src/MissionManager/AirMapManager.h b/src/MissionManager/AirMapManager.h deleted file mode 100644 index 99f85ee..0000000 --- a/src/MissionManager/AirMapManager.h +++ /dev/null @@ -1,367 +0,0 @@ -/**************************************************************************** - * - * (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 - -#include "QGCToolbox.h" -#include "QGCLoggingCategory.h" -#include "QmlObjectListModel.h" -#include "MissionItem.h" -#include "MultiVehicleManager.h" -#include "AirspaceManagement.h" - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -Q_DECLARE_LOGGING_CATEGORY(AirMapManagerLog) - - -/** - * @class LifetimeChecker - * Base class which helps to check if an object instance still exists. - * A subclass can take a weak pointer from _instance and then check if the object was deleted. - * This is used in callbacks that access 'this', but the instance might already be deleted (e.g. vehicle disconnect). - */ -class LifetimeChecker -{ -public: - LifetimeChecker() : _instance(this, [](void*){}) { } - virtual ~LifetimeChecker() = default; - -protected: - std::shared_ptr _instance; -}; - -/** - * @class AirMapSharedState - * contains state & settings that need to be shared (such as login) - */ -class AirMapSharedState : public QObject -{ - Q_OBJECT -public: - struct Settings { - QString apiKey; - - // login credentials - QString clientID; - QString userName; ///< use anonymous login if empty - QString password; - }; - - void setSettings(const Settings& settings); - const Settings& settings() const { return _settings; } - - void setClient(airmap::qt::Client* client) { _client = client; } - - /** - * Get the current client instance. It can be NULL. If not NULL, it implies - * there's an API key set. - */ - airmap::qt::Client* client() const { return _client; } - - bool hasAPIKey() const { return _settings.apiKey != ""; } - - bool isLoggedIn() const { return _loginToken != ""; } - - using Callback = std::function; - - /** - * Do a request that requires user login: if not yet logged in, the request is queued and - * processed after successful login, otherwise it's executed directly. - */ - void doRequestWithLogin(const Callback& callback); - - void login(); - - void logout(); - - const QString& loginToken() const { return _loginToken; } - -signals: - void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); - -private: - void _processPendingRequests(); - - bool _isLoginInProgress = false; - QString _loginToken; ///< login token: empty when not logged in - - airmap::qt::Client* _client = nullptr; - - Settings _settings; - - QQueue _pendingRequests; ///< pending requests that are processed after a successful login -}; - - -/// class to download polygons from AirMap -class AirMapRestrictionManager : public AirspaceRestrictionProvider, public LifetimeChecker -{ - Q_OBJECT -public: - AirMapRestrictionManager(AirMapSharedState& shared); - - void setROI(const QGeoCoordinate& center, double radiusMeters) override; - -signals: - void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); - -private: - - static void _addPolygonToList(const airmap::Geometry::Polygon& polygon, QList& list); - - enum class State { - Idle, - RetrieveItems, - }; - - State _state = State::Idle; - AirMapSharedState& _shared; -}; - - -/// class to upload a flight -class AirMapFlightManager : public QObject, public LifetimeChecker -{ - Q_OBJECT -public: - AirMapFlightManager(AirMapSharedState& shared); - - /// Send flight path to AirMap - void createFlight(const QList& missionItems); - - AirspaceAuthorization::PermitStatus flightPermitStatus() const { return _flightPermitStatus; } - - const QString& flightID() const { return _currentFlightId; } - -public slots: - void endFlight(); - -signals: - void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); - void flightPermitStatusChanged(); - -private slots: - void _pollBriefing(); - -private: - - /** - * upload flight stored in _flight - */ - void _uploadFlight(); - - /** - * query the active flights and end the first one (because only a single flight can be active at a time). - */ - void _endFirstFlight(); - - /** - * implementation of endFlight() - */ - void _endFlight(const QString& flightID); - - /** - * check if the briefing response is valid and call _submitPendingFlightPlan() if it is. - */ - void _checkForValidBriefing(); - - void _submitPendingFlightPlan(); - - enum class State { - Idle, - GetPilotID, - FlightUpload, - FlightBrief, - FlightSubmit, - FlightPolling, // poll & check for approval - FlightEnd, - EndFirstFlight, // get a list of open flights & end the first one (because there can only be 1 active at a time) - }; - struct Flight { - QList coords; - QGeoCoordinate takeoffCoord; - float maxAltitude = 0; - - void reset() { - coords.clear(); - maxAltitude = 0; - } - }; - Flight _flight; ///< flight pending to be uploaded - - State _state = State::Idle; - AirMapSharedState& _shared; - QString _currentFlightId; ///< Flight ID, empty if there is none - QString _pendingFlightId; ///< current flight ID, not necessarily accepted yet (once accepted, it's equal to _currentFlightId) - QString _pendingFlightPlan; ///< current flight plan, waiting to be submitted - AirspaceAuthorization::PermitStatus _flightPermitStatus = AirspaceAuthorization::PermitUnknown; - QString _pilotID; ///< Pilot ID in the form "auth0|abc123" - bool _noFlightCreatedYet = true; - QTimer _pollTimer; ///< timer to poll for approval check -}; - -/// class to send telemetry data to AirMap -class AirMapTelemetry : public QObject, public LifetimeChecker -{ - Q_OBJECT -public: - AirMapTelemetry(AirMapSharedState& shared); - virtual ~AirMapTelemetry() = default; - - /** - * Setup the connection to start sending telemetry - */ - void startTelemetryStream(const QString& flightID); - - void stopTelemetryStream(); - - bool isTelemetryStreaming() const; - -signals: - void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); - -public slots: - void vehicleMavlinkMessageReceived(const mavlink_message_t& message); - -private: - - void _handleGlobalPositionInt(const mavlink_message_t& message); - void _handleGPSRawInt(const mavlink_message_t& message); - - enum class State { - Idle, - StartCommunication, - EndCommunication, - Streaming, - }; - - State _state = State::Idle; - - AirMapSharedState& _shared; - std::string _key; ///< key for AES encryption (16 bytes) - QString _flightID; - - float _lastHdop = 1.f; -}; - - -class AirMapTrafficMonitor : public QObject, public LifetimeChecker -{ - Q_OBJECT -public: - AirMapTrafficMonitor(AirMapSharedState& shared) - : _shared(shared) - { - } - virtual ~AirMapTrafficMonitor(); - - void startConnection(const QString& flightID); - - void stop(); - -signals: - void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); - void trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading); - -private: - void _update(airmap::Traffic::Update::Type type, const std::vector& update); - -private: - QString _flightID; - AirMapSharedState& _shared; - std::shared_ptr _monitor; - std::shared_ptr _subscriber; -}; - - - -/// AirMap per vehicle management class. -class AirMapManagerPerVehicle : public AirspaceManagerPerVehicle -{ - Q_OBJECT -public: - AirMapManagerPerVehicle(AirMapSharedState& shared, const Vehicle& vehicle, QGCToolbox& toolbox); - virtual ~AirMapManagerPerVehicle() = default; - - - void createFlight(const QList& missionItems) override; - - AirspaceAuthorization::PermitStatus flightPermitStatus() const override; - - void startTelemetryStream() override; - - void stopTelemetryStream() override; - - bool isTelemetryStreaming() const override; - -signals: - void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); - -public slots: - void endFlight() override; - -protected slots: - virtual void vehicleMavlinkMessageReceived(const mavlink_message_t& message) override; -private slots: - void _flightPermitStatusChanged(); -private: - AirMapSharedState& _shared; - - AirMapFlightManager _flightManager; - AirMapTelemetry _telemetry; - AirMapTrafficMonitor _trafficMonitor; - - QGCToolbox& _toolbox; -}; - - -class AirMapManager : public AirspaceManager -{ - Q_OBJECT - -public: - AirMapManager(QGCApplication* app, QGCToolbox* toolbox); - virtual ~AirMapManager(); - - void setToolbox(QGCToolbox* toolbox) override; - - AirspaceManagerPerVehicle* instantiateVehicle(const Vehicle& vehicle) override; - - AirspaceRestrictionProvider* instantiateRestrictionProvider() override; - - QString name() const override { return "AirMap"; } - - void requestWeatherUpdate(const QGeoCoordinate& coordinate) override; - -private slots: - void _error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); - - void _settingsChanged(); -private: - - AirMapSharedState _shared; - - std::shared_ptr _logger; - std::shared_ptr _dispatchingLogger; -}; - - diff --git a/src/MissionManager/AirspaceController.cc b/src/MissionManager/AirspaceController.cc deleted file mode 100644 index d81d763..0000000 --- a/src/MissionManager/AirspaceController.cc +++ /dev/null @@ -1,20 +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. - * - ****************************************************************************/ - -#include "AirspaceController.h" -#include "AirspaceManagement.h" -#include "QGCApplication.h" -#include "QGCQGeoCoordinate.h" - -AirspaceController::AirspaceController(QObject* parent) - : QObject(parent) - , _manager(qgcApp()->toolbox()->airspaceManager()) -{ -} - diff --git a/src/MissionManager/AirspaceController.h b/src/MissionManager/AirspaceController.h deleted file mode 100644 index b97c6a8..0000000 --- a/src/MissionManager/AirspaceController.h +++ /dev/null @@ -1,36 +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. - * - ****************************************************************************/ - -#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 deleted file mode 100644 index 6b0aa1d..0000000 --- a/src/MissionManager/AirspaceManagement.cc +++ /dev/null @@ -1,121 +0,0 @@ -/**************************************************************************** - * - * (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"); - qRegisterMetaType(); -} - -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 deleted file mode 100644 index 1a17b6e..0000000 --- a/src/MissionManager/AirspaceManagement.h +++ /dev/null @@ -1,255 +0,0 @@ -/**************************************************************************** - * - * (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; - -struct WeatherInformation -{ - QString condition; ///< The overall weather condition. - QString icon; ///< The icon or class of icon that should be used for display purposes. - uint32_t windHeading = 0; ///< The heading in [°]. - uint32_t windSpeed = 0; ///< The speed in [°]. - uint32_t windGusting = 0; - int32_t temperature = 0; ///< The temperature in [°C]. - float humidity = 0.0; - uint32_t visibility = 0; ///< Visibility in [m]. - uint32_t precipitation = 0; ///< The probability of precipitation in [%]. -}; -Q_DECLARE_METATYPE(WeatherInformation); - -/** - * @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; - - /** - * Request weather information update. When done, it will emit the weatherUpdate() signal. - * @param coordinate request update for this coordinate - */ - virtual void requestWeatherUpdate(const QGeoCoordinate& coordinate) = 0; - -signals: - void weatherUpdate(bool success, QGeoCoordinate coordinate, WeatherInformation weather); - -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 ddda1d1..05ce7ae 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -21,7 +21,9 @@ GeoFenceManager::GeoFenceManager(Vehicle* vehicle) : _vehicle (vehicle) , _planManager (vehicle, MAV_MISSION_TYPE_FENCE) , _firstParamLoadComplete (false) +#if defined(QGC_AIRMAP_ENABLED) , _airspaceManager (qgcApp()->toolbox()->airspaceManager()) +#endif { connect(&_planManager, &PlanManager::inProgressChanged, this, &GeoFenceManager::inProgressChanged); connect(&_planManager, &PlanManager::error, this, &GeoFenceManager::error); diff --git a/src/MissionManager/GeoFenceManager.h b/src/MissionManager/GeoFenceManager.h index 26ec69d..9c7bdd5 100644 --- a/src/MissionManager/GeoFenceManager.h +++ b/src/MissionManager/GeoFenceManager.h @@ -13,7 +13,10 @@ #include #include +#if defined(QGC_AIRMAP_ENABLED) #include "AirspaceManagement.h" +#endif + #include "QGCLoggingCategory.h" #include "FactSystem.h" #include "PlanManager.h" @@ -97,7 +100,9 @@ private: bool _firstParamLoadComplete; QList _sendPolygons; QList _sendCircles; +#if defined(QGC_AIRMAP_ENABLED) AirspaceManager* _airspaceManager; +#endif }; #endif diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index c73286c..e90ebbd 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -79,11 +79,13 @@ void PlanManager::writeMissionItems(const QList& missionItems) } if (_planType == MAV_MISSION_TYPE_MISSION) { +#if defined(QGC_AIRMAP_ENABLED) // upload the flight to the airspace management backend AirspaceManagerPerVehicle* airspaceManager = _vehicle->airspaceManager(); if (airspaceManager) { airspaceManager->createFlight(missionItems); } +#endif } _clearAndDeleteWriteMissionItems(); diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 832fadb..f97a037 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -84,8 +84,9 @@ #include "CameraCalc.h" #include "VisualMissionItem.h" #include "EditPositionDialogController.h" +#if defined(QGC_AIRMAP_ENABLED) #include "AirspaceController.h" - +#endif #ifndef NO_SERIAL_LINK #include "SerialLink.h" #endif @@ -374,7 +375,6 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "ParameterManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "QGCCameraManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "QGCCameraControl", "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"); @@ -383,6 +383,9 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType ("QGroundControl.Controllers", 1, 0, "GeoFenceController", "Reference only"); qmlRegisterUncreatableType("QGroundControl.Controllers", 1, 0, "RallyPointController", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Controllers", 1, 0, "VisualMissionItem", "Reference only"); +#if defined(QGC_AIRMAP_ENABLED) + qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "AirspaceController", "Reference only"); +#endif qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ParameterEditorController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ESP8266ComponentController"); diff --git a/src/QGCToolbox.cc b/src/QGCToolbox.cc index 94d5795..bdf944b 100644 --- a/src/QGCToolbox.cc +++ b/src/QGCToolbox.cc @@ -30,7 +30,9 @@ #include "QGCOptions.h" #include "SettingsManager.h" #include "QGCApplication.h" +#if defined(QGC_AIRMAP_ENABLED) #include "AirMapManager.h" +#endif #if defined(QGC_CUSTOM_BUILD) #include CUSTOMHEADER @@ -57,7 +59,9 @@ QGCToolbox::QGCToolbox(QGCApplication* app) , _mavlinkLogManager (NULL) , _corePlugin (NULL) , _settingsManager (NULL) +#if defined(QGC_AIRMAP_ENABLED) , _airspaceManager (NULL) +#endif { // SettingsManager must be first so settings are available to any subsequent tools _settingsManager = new SettingsManager(app, this); @@ -82,7 +86,9 @@ QGCToolbox::QGCToolbox(QGCApplication* app) _followMe = new FollowMe (app, this); _videoManager = new VideoManager (app, this); _mavlinkLogManager = new MAVLinkLogManager (app, this); +#if defined(QGC_AIRMAP_ENABLED) _airspaceManager = new AirMapManager (app, this); +#endif } void QGCToolbox::setChildToolboxes(void) @@ -109,7 +115,9 @@ void QGCToolbox::setChildToolboxes(void) _qgcPositionManager->setToolbox(this); _videoManager->setToolbox(this); _mavlinkLogManager->setToolbox(this); +#if defined(QGC_AIRMAP_ENABLED) _airspaceManager->setToolbox(this); +#endif } void QGCToolbox::_scanAndLoadPlugins(QGCApplication* app) diff --git a/src/QGCToolbox.h b/src/QGCToolbox.h index 5ece9be..ab19798 100644 --- a/src/QGCToolbox.h +++ b/src/QGCToolbox.h @@ -32,7 +32,9 @@ class VideoManager; class MAVLinkLogManager; class QGCCorePlugin; class SettingsManager; +#if defined(QGC_AIRMAP_ENABLED) class AirspaceManager; +#endif /// This is used to manage all of our top level services/tools class QGCToolbox : public QObject { @@ -57,8 +59,9 @@ public: MAVLinkLogManager* mavlinkLogManager(void) { return _mavlinkLogManager; } QGCCorePlugin* corePlugin(void) { return _corePlugin; } SettingsManager* settingsManager(void) { return _settingsManager; } +#if defined(QGC_AIRMAP_ENABLED) AirspaceManager* airspaceManager(void) { return _airspaceManager; } - +#endif #ifndef __mobile__ GPSManager* gpsManager(void) { return _gpsManager; } #endif @@ -88,8 +91,9 @@ private: MAVLinkLogManager* _mavlinkLogManager; QGCCorePlugin* _corePlugin; SettingsManager* _settingsManager; +#if defined(QGC_AIRMAP_ENABLED) AirspaceManager* _airspaceManager; - +#endif friend class QGCApplication; }; diff --git a/src/QmlControls/QGroundControlQmlGlobal.h b/src/QmlControls/QGroundControlQmlGlobal.h index 3f0fd2a..54af5a1 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.h +++ b/src/QmlControls/QGroundControlQmlGlobal.h @@ -53,6 +53,7 @@ public: Q_PROPERTY(QGCCorePlugin* corePlugin READ corePlugin CONSTANT) Q_PROPERTY(SettingsManager* settingsManager READ settingsManager CONSTANT) Q_PROPERTY(FactGroup* gpsRtk READ gpsRtkFactGroup CONSTANT) + Q_PROPERTY(bool airmapSupported READ airmapSupported CONSTANT) Q_PROPERTY(int supportedFirmwareCount READ supportedFirmwareCount CONSTANT) @@ -171,6 +172,11 @@ public: QString qgcVersion(void) const { return qgcApp()->applicationVersion(); } +#if defined(QGC_AIRMAP_ENABLED) + bool airmapSupported() { return true; } +#else + bool airmapSupported() { return false; } +#endif // Overrides from QGCTool virtual void setToolbox(QGCToolbox* toolbox); diff --git a/src/Settings/AirMap.SettingsGroup.json b/src/Settings/AirMap.SettingsGroup.json deleted file mode 100644 index 5806903..0000000 --- a/src/Settings/AirMap.SettingsGroup.json +++ /dev/null @@ -1,26 +0,0 @@ -[ -{ - "name": "apiKey", - "shortDescription": "AirMap API Key", - "type": "string", - "defaultValue": "" -}, -{ - "name": "clientID", - "shortDescription": "AirMap Client ID", - "type": "string", - "defaultValue": "" -}, -{ - "name": "userName", - "shortDescription": "AirMap User Name", - "type": "string", - "defaultValue": "" -}, -{ - "name": "password", - "shortDescription": "AirMap Password", - "type": "string", - "defaultValue": "" -} -] diff --git a/src/Settings/AirMapSettings.cc b/src/Settings/AirMapSettings.cc deleted file mode 100644 index a2cc7ce..0000000 --- a/src/Settings/AirMapSettings.cc +++ /dev/null @@ -1,30 +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. - * - ****************************************************************************/ - -#include "AirMapSettings.h" -#include "QGCPalette.h" -#include "QGCApplication.h" - -#include -#include - -DECLARE_SETTINGGROUP(AirMap) -{ - INIT_SETTINGFACT(apiKey); - INIT_SETTINGFACT(clientID); - INIT_SETTINGFACT(userName); - INIT_SETTINGFACT(password); - QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); - qmlRegisterUncreatableType("QGroundControl.SettingsManager", 1, 0, "AirMapSettings", "Reference only"); -} - -DECLARE_SETTINGSFACT(AirMapSettings, apiKey) -DECLARE_SETTINGSFACT(AirMapSettings, clientID) -DECLARE_SETTINGSFACT(AirMapSettings, userName) -DECLARE_SETTINGSFACT(AirMapSettings, password) diff --git a/src/Settings/AirMapSettings.h b/src/Settings/AirMapSettings.h deleted file mode 100644 index 04542d5..0000000 --- a/src/Settings/AirMapSettings.h +++ /dev/null @@ -1,28 +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. - * - ****************************************************************************/ - -#pragma once - -#include "SettingsGroup.h" -#include "QGCMAVLink.h" - -class AirMapSettings : public SettingsGroup -{ - Q_OBJECT -public: - AirMapSettings(QObject* parent = NULL); - - DEFINE_SETTINGGROUP(AirMap) - - DEFINE_SETTINGFACT(apiKey) - DEFINE_SETTINGFACT(clientID) - DEFINE_SETTINGFACT(userName) - DEFINE_SETTINGFACT(password) - -}; diff --git a/src/Settings/SettingsManager.cc b/src/Settings/SettingsManager.cc index b7d6a12..d002b49 100644 --- a/src/Settings/SettingsManager.cc +++ b/src/Settings/SettingsManager.cc @@ -14,7 +14,9 @@ SettingsManager::SettingsManager(QGCApplication* app, QGCToolbox* toolbox) : QGCTool(app, toolbox) +#if defined(QGC_AIRMAP_ENABLED) , _airMapSettings (NULL) +#endif , _appSettings (NULL) , _unitsSettings (NULL) , _autoConnectSettings (NULL) @@ -41,5 +43,7 @@ void SettingsManager::setToolbox(QGCToolbox *toolbox) _rtkSettings = new RTKSettings(this); _guidedSettings = new GuidedSettings(this); _brandImageSettings = new BrandImageSettings(this); +#if defined(QGC_AIRMAP_ENABLED) _airMapSettings = new AirMapSettings(this); +#endif } diff --git a/src/Settings/SettingsManager.h b/src/Settings/SettingsManager.h index bba1104..5ee29a1 100644 --- a/src/Settings/SettingsManager.h +++ b/src/Settings/SettingsManager.h @@ -22,8 +22,9 @@ #include "RTKSettings.h" #include "GuidedSettings.h" #include "BrandImageSettings.h" +#if defined(QGC_AIRMAP_ENABLED) #include "AirMapSettings.h" - +#endif #include /// Provides access to all app settings @@ -34,7 +35,9 @@ class SettingsManager : public QGCTool public: SettingsManager(QGCApplication* app, QGCToolbox* toolbox); +#if defined(QGC_AIRMAP_ENABLED) Q_PROPERTY(QObject* airMapSettings READ airMapSettings CONSTANT) +#endif Q_PROPERTY(QObject* appSettings READ appSettings CONSTANT) Q_PROPERTY(QObject* unitsSettings READ unitsSettings CONSTANT) Q_PROPERTY(QObject* autoConnectSettings READ autoConnectSettings CONSTANT) @@ -47,7 +50,9 @@ public: // Override from QGCTool virtual void setToolbox(QGCToolbox *toolbox); +#if defined(QGC_AIRMAP_ENABLED) AirMapSettings* airMapSettings (void) { return _airMapSettings; } +#endif AppSettings* appSettings (void) { return _appSettings; } UnitsSettings* unitsSettings (void) { return _unitsSettings; } AutoConnectSettings* autoConnectSettings (void) { return _autoConnectSettings; } @@ -58,7 +63,9 @@ public: BrandImageSettings* brandImageSettings (void) { return _brandImageSettings; } private: +#if defined(QGC_AIRMAP_ENABLED) AirMapSettings* _airMapSettings; +#endif AppSettings* _appSettings; UnitsSettings* _unitsSettings; AutoConnectSettings* _autoConnectSettings; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index de68709..c0dcc7e 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -38,8 +38,9 @@ #include "QGCCameraManager.h" #include "VideoReceiver.h" #include "VideoManager.h" +#if defined(QGC_AIRMAP_ENABLED) #include "AirspaceController.h" - +#endif QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") #define UPDATE_TIMER 50 @@ -140,8 +141,10 @@ Vehicle::Vehicle(LinkInterface* link, , _rallyPointManager(NULL) , _rallyPointManagerInitialRequestSent(false) , _parameterManager(NULL) +#if defined(QGC_AIRMAP_ENABLED) , _airspaceController(NULL) , _airspaceManagerPerVehicle(NULL) +#endif , _armed(false) , _base_mode(0) , _custom_mode(0) @@ -265,8 +268,8 @@ Vehicle::Vehicle(LinkInterface* link, _adsbTimer.setSingleShot(false); _adsbTimer.start(1000); +#if defined(QGC_AIRMAP_ENABLED) _airspaceController = new AirspaceController(this); - AirspaceManager* airspaceManager = _toolbox->airspaceManager(); if (airspaceManager) { _airspaceManagerPerVehicle = airspaceManager->instantiateVehicle(*this); @@ -275,7 +278,7 @@ Vehicle::Vehicle(LinkInterface* link, connect(_airspaceManagerPerVehicle, &AirspaceManagerPerVehicle::flightPermitStatusChanged, this, &Vehicle::flightPermitStatusChanged); } } - +#endif } // Disconnected Vehicle for offline editing @@ -336,8 +339,10 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _rallyPointManager(NULL) , _rallyPointManagerInitialRequestSent(false) , _parameterManager(NULL) +#if defined(QGC_AIRMAP_ENABLED) , _airspaceController(NULL) , _airspaceManagerPerVehicle(NULL) +#endif , _armed(false) , _base_mode(0) , _custom_mode(0) @@ -471,9 +476,11 @@ Vehicle::~Vehicle() delete _mav; _mav = NULL; +#if defined(QGC_AIRMAP_ENABLED) if (_airspaceManagerPerVehicle) { delete _airspaceManagerPerVehicle; } +#endif } void Vehicle::prepareDelete() diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index e1225e2..2f1f945 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -20,7 +20,9 @@ #include "MAVLinkProtocol.h" #include "UASMessageHandler.h" #include "SettingsFact.h" +#if defined(QGC_AIRMAP_ENABLED) #include +#endif class UAS; class UASInterface; @@ -36,7 +38,9 @@ class UASMessage; class SettingsManager; class ADSBVehicle; class QGCCameraManager; +#if defined(QGC_AIRMAP_ENABLED) class AirspaceController; +#endif Q_DECLARE_LOGGING_CATEGORY(VehicleLog) @@ -355,9 +359,10 @@ public: Q_PROPERTY(QString hobbsMeter READ hobbsMeter NOTIFY hobbsMeterChanged) Q_PROPERTY(bool vtolInFwdFlight READ vtolInFwdFlight WRITE setVtolInFwdFlight NOTIFY vtolInFwdFlightChanged) Q_PROPERTY(bool highLatencyLink READ highLatencyLink NOTIFY highLatencyLinkChanged) +#if defined(QGC_AIRMAP_ENABLED) Q_PROPERTY(AirspaceAuthorization::PermitStatus flightPermitStatus READ flightPermitStatus NOTIFY flightPermitStatusChanged) ///< state of flight permission Q_PROPERTY(AirspaceController* airspaceController READ airspaceController CONSTANT) - +#endif // Vehicle state used for guided control Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying Q_PROPERTY(bool landing READ landing NOTIFY landingChanged) ///< Vehicle is in landing pattern (DO_LAND_START) @@ -576,8 +581,9 @@ public: QmlObjectListModel* cameraTriggerPoints(void) { return &_cameraTriggerPoints; } QmlObjectListModel* adsbVehicles(void) { return &_adsbVehicles; } +#if defined(QGC_AIRMAP_ENABLED) AirspaceController* airspaceController() { return _airspaceController; } - +#endif int flowImageIndex() { return _flowImageIndex; } //-- Mavlink Logging @@ -761,10 +767,12 @@ public: /// Vehicle is about to be deleted void prepareDelete(); +#if defined(QGC_AIRMAP_ENABLED) AirspaceAuthorization::PermitStatus flightPermitStatus() const { return _airspaceManagerPerVehicle ? _airspaceManagerPerVehicle->flightPermitStatus() : AirspaceAuthorization::PermitUnknown; } AirspaceManagerPerVehicle* airspaceManager() const { return _airspaceManagerPerVehicle; } +#endif signals: void allLinksInactive(Vehicle* vehicle); @@ -800,8 +808,9 @@ signals: void capabilityBitsChanged(uint64_t capabilityBits); void toolBarIndicatorsChanged(void); void highLatencyLinkChanged(bool highLatencyLink); +#if defined(QGC_AIRMAP_ENABLED) void flightPermitStatusChanged(); - +#endif void messagesReceivedChanged (); void messagesSentChanged (); @@ -1055,8 +1064,10 @@ private: ParameterManager* _parameterManager; +#if defined(QGC_AIRMAP_ENABLED) AirspaceController* _airspaceController; AirspaceManagerPerVehicle* _airspaceManagerPerVehicle; +#endif bool _armed; ///< true: vehicle is armed uint8_t _base_mode; ///< base_mode from HEARTBEAT