From b3c506edfcdcaf2d0becfdfd71e024c57c226431 Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Fri, 16 Feb 2018 16:54:29 -0500 Subject: [PATCH] Flight Plan WIP Moved flight planning away from "Flight Management" to its own class. Planning does not need a connected vehicle. --- qgroundcontrol.pro | 5 +- src/Airmap/AirMapFlightManager.cc | 12 +- src/Airmap/AirMapFlightManager.h | 6 +- src/Airmap/AirMapFlightPlanManager.cc | 244 ++++++++++++++++++ src/Airmap/AirMapFlightPlanManager.h | 74 ++++++ src/Airmap/AirMapManager.cc | 15 +- src/Airmap/AirMapManager.h | 7 +- src/Airmap/AirMapVehicleManager.cc | 4 +- src/Airmap/AirMapVehicleManager.h | 2 +- .../AirspaceFlightPlanProvider.cc | 15 ++ .../AirspaceFlightPlanProvider.h | 47 ++++ src/AirspaceManagement/AirspaceManager.cc | 13 +- src/AirspaceManagement/AirspaceManager.h | 43 ++- .../AirspaceVehicleManager.h | 4 +- src/FlightDisplay/FlightDisplayView.qml | 14 +- src/MissionManager/MissionController.cc | 6 +- src/MissionManager/PlanManager.cc | 10 - src/Vehicle/Vehicle.cc | 1 - src/Vehicle/Vehicle.h | 9 - 19 files changed, 442 insertions(+), 89 deletions(-) create mode 100644 src/Airmap/AirMapFlightPlanManager.cc create mode 100644 src/Airmap/AirMapFlightPlanManager.h create mode 100644 src/AirspaceManagement/AirspaceFlightPlanProvider.cc create mode 100644 src/AirspaceManagement/AirspaceFlightPlanProvider.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 52176e0f7..0d088431c 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -1080,7 +1080,7 @@ contains (DEFINES, QGC_AIRMAP_ENABLED) { HEADERS += \ src/AirspaceManagement/AirspaceAdvisoryProvider.h \ - src/AirspaceManagement/AirspaceAuthorization.h \ + src/AirspaceManagement/AirspaceFlightPlanProvider.h \ src/AirspaceManagement/AirspaceManager.h \ src/AirspaceManagement/AirspaceRestriction.h \ src/AirspaceManagement/AirspaceRestrictionProvider.h \ @@ -1090,6 +1090,7 @@ contains (DEFINES, QGC_AIRMAP_ENABLED) { SOURCES += \ src/AirspaceManagement/AirspaceAdvisoryProvider.cc \ + src/AirspaceManagement/AirspaceFlightPlanProvider.cc \ src/AirspaceManagement/AirspaceManager.cc \ src/AirspaceManagement/AirspaceRestriction.cc \ src/AirspaceManagement/AirspaceRestrictionProvider.cc \ @@ -1107,6 +1108,7 @@ contains (DEFINES, QGC_AIRMAP_ENABLED) { HEADERS += \ src/Airmap/AirMapAdvisoryManager.h \ src/Airmap/AirMapFlightManager.h \ + src/Airmap/AirMapFlightPlanManager.h \ src/Airmap/AirMapManager.h \ src/Airmap/AirMapRestrictionManager.h \ src/Airmap/AirMapRulesetsManager.h \ @@ -1121,6 +1123,7 @@ contains (DEFINES, QGC_AIRMAP_ENABLED) { SOURCES += \ src/Airmap/AirMapAdvisoryManager.cc \ src/Airmap/AirMapFlightManager.cc \ + src/Airmap/AirMapFlightPlanManager.cc \ src/Airmap/AirMapManager.cc \ src/Airmap/AirMapRestrictionManager.cc \ src/Airmap/AirMapRulesetsManager.cc \ diff --git a/src/Airmap/AirMapFlightManager.cc b/src/Airmap/AirMapFlightManager.cc index b24f2d242..e7ea7fbc7 100644 --- a/src/Airmap/AirMapFlightManager.cc +++ b/src/Airmap/AirMapFlightManager.cc @@ -94,7 +94,7 @@ void AirMapFlightManager::createFlight(const QList& missionItems) qCDebug(AirMapManagerLog) << "Got Pilot ID:"<<_pilotID; _uploadFlight(); } else { - _flightPermitStatus = AirspaceAuthorization::PermitUnknown; + _flightPermitStatus = AirspaceFlightPlanProvider::PermitUnknown; emit flightPermitStatusChanged(); _state = State::Idle; @@ -108,7 +108,7 @@ void AirMapFlightManager::createFlight(const QList& missionItems) _uploadFlight(); } - _flightPermitStatus = AirspaceAuthorization::PermitPending; + _flightPermitStatus = AirspaceFlightPlanProvider::PermitPending; emit flightPermitStatusChanged(); } @@ -252,7 +252,7 @@ void AirMapFlightManager::_checkForValidBriefing() if (allValid) { _submitPendingFlightPlan(); } else { - _flightPermitStatus = AirspaceAuthorization::PermitRejected; + _flightPermitStatus = AirspaceFlightPlanProvider::PermitRejected; emit flightPermitStatusChanged(); _state = State::Idle; } @@ -337,9 +337,9 @@ void AirMapFlightManager::_pollBriefing() if ((rejected || accepted) && !pending) { if (rejected) { // rejected has priority - _flightPermitStatus = AirspaceAuthorization::PermitRejected; + _flightPermitStatus = AirspaceFlightPlanProvider::PermitRejected; } else { - _flightPermitStatus = AirspaceAuthorization::PermitAccepted; + _flightPermitStatus = AirspaceFlightPlanProvider::PermitAccepted; } _currentFlightId = _pendingFlightId; _pendingFlightPlan = ""; @@ -370,7 +370,7 @@ void AirMapFlightManager::endFlight() } _endFlight(_currentFlightId); - _flightPermitStatus = AirspaceAuthorization::PermitUnknown; + _flightPermitStatus = AirspaceFlightPlanProvider::PermitUnknown; emit flightPermitStatusChanged(); } diff --git a/src/Airmap/AirMapFlightManager.h b/src/Airmap/AirMapFlightManager.h index ccee49249..386d4a83a 100644 --- a/src/Airmap/AirMapFlightManager.h +++ b/src/Airmap/AirMapFlightManager.h @@ -11,7 +11,7 @@ #include "LifetimeChecker.h" #include "AirMapSharedState.h" -#include "AirspaceAuthorization.h" +#include "AirspaceFlightPlanProvider.h" #include #include @@ -31,7 +31,7 @@ public: /// Send flight path to AirMap void createFlight(const QList& missionItems); - AirspaceAuthorization::PermitStatus flightPermitStatus() const { return _flightPermitStatus; } + AirspaceFlightPlanProvider::PermitStatus flightPermitStatus() const { return _flightPermitStatus; } const QString& flightID() const { return _currentFlightId; } @@ -96,7 +96,7 @@ private: 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; + AirspaceFlightPlanProvider::PermitStatus _flightPermitStatus = AirspaceFlightPlanProvider::PermitUnknown; QString _pilotID; ///< Pilot ID in the form "auth0|abc123" bool _noFlightCreatedYet = true; QTimer _pollTimer; ///< timer to poll for approval check diff --git a/src/Airmap/AirMapFlightPlanManager.cc b/src/Airmap/AirMapFlightPlanManager.cc new file mode 100644 index 000000000..0d7df526d --- /dev/null +++ b/src/Airmap/AirMapFlightPlanManager.cc @@ -0,0 +1,244 @@ +/**************************************************************************** + * + * (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 "AirMapFlightPlanManager.h" +#include "AirMapManager.h" +#include "AirMapRulesetsManager.h" +#include "QGCApplication.h" + +#include "MissionController.h" +#include "QGCMAVLink.h" + +#include "airmap/pilots.h" +#include "airmap/flights.h" +#include "airmap/date_time.h" +#include "airmap/flight_plans.h" +#include "airmap/geometry.h" + +using namespace airmap; + +//----------------------------------------------------------------------------- +AirMapFlightPlanManager::AirMapFlightPlanManager(AirMapSharedState& shared, QObject *parent) + : AirspaceFlightPlanProvider(parent) + , _shared(shared) +{ + connect(&_pollTimer, &QTimer::timeout, this, &AirMapFlightPlanManager::_pollBriefing); +} + +//----------------------------------------------------------------------------- +void +AirMapFlightPlanManager::createFlight(MissionController* missionController) +{ + if (!_shared.client()) { + qCDebug(AirMapManagerLog) << "No AirMap client instance. Will not create a flight"; + return; + } + + if (_state != State::Idle) { + qCWarning(AirMapManagerLog) << "AirMapFlightPlanManager::createFlight: State not idle"; + return; + } + + //-- TODO: Check if there is an ongoing flight plan and do something about it + _createPlan = true; + if(!_controller) { + _controller = missionController; + connect(missionController, &MissionController::missionBoundingCubeChanged, this, &AirMapFlightPlanManager::_missionBoundingCubeChanged); + } +} + +//----------------------------------------------------------------------------- +void +AirMapFlightPlanManager::_createFlightPlan() +{ + _flight.reset(); + + //-- Get flight bounding cube and prepare (box) polygon + + //-- TODO: If single point, we need to set a point and a radius instead + + QGCGeoBoundingCube bc = _controller->travelBoundingCube(); + _flight.maxAltitude = fmax(bc.pointNW.altitude(), bc.pointSE.altitude()); + _flight.takeoffCoord = _controller->takeoffCoordinate(); + _flight.coords.append(QGeoCoordinate(bc.pointNW.latitude(), bc.pointNW.longitude(), _flight.maxAltitude)); + _flight.coords.append(QGeoCoordinate(bc.pointNW.latitude(), bc.pointSE.longitude(), _flight.maxAltitude)); + _flight.coords.append(QGeoCoordinate(bc.pointSE.latitude(), bc.pointSE.longitude(), _flight.maxAltitude)); + _flight.coords.append(QGeoCoordinate(bc.pointSE.latitude(), bc.pointNW.longitude(), _flight.maxAltitude)); + _flight.coords.append(QGeoCoordinate(bc.pointNW.latitude(), bc.pointNW.longitude(), _flight.maxAltitude)); + + _flight.maxAltitude += 5; // Add a safety buffer + + qCDebug(AirMapManagerLog) << "About to create flight plan"; + qCDebug(AirMapManagerLog) << "Takeoff: " << _flight.takeoffCoord; + qCDebug(AirMapManagerLog) << "Bounding box:" << bc.pointNW << bc.pointSE; + + return; + + if (_pilotID == "") { + //-- Need to get the pilot id before uploading the flight plan + 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; + _uploadFlightPlan(); + } else { + _flightPermitStatus = AirspaceFlightPlanProvider::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 { + _uploadFlightPlan(); + } + + _flightPermitStatus = AirspaceFlightPlanProvider::PermitPending; + emit flightPermitStatusChanged(); +} + +//----------------------------------------------------------------------------- +void +AirMapFlightPlanManager::_uploadFlightPlan() +{ + qCDebug(AirMapManagerLog) << "Uploading flight plan"; + _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? + //-- Rules + AirMapRulesetsManager* pRulesMgr = dynamic_cast(qgcApp()->toolbox()->airspaceManager()->ruleSets()); + if(pRulesMgr) { + foreach(QString ruleset, pRulesMgr->rulesetsIDs()) { + params.rulesets.push_back(ruleset.toStdString()); + } + } + //-- 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(); + //-- Create flight plan + _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) { + _flightPlan = QString::fromStdString(result.value().id); + qCDebug(AirMapManagerLog) << "Flight plan created:" << _flightPlan; + _state = State::FlightPolling; + _pollBriefing(); + } 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 +AirMapFlightPlanManager::_pollBriefing() +{ + if (_state != State::FlightPolling) { + qCWarning(AirMapManagerLog) << "AirMapFlightPlanManager::_pollBriefing: not in polling state"; + return; + } + FlightPlans::RenderBriefing::Parameters params; + params.authorization = _shared.loginToken().toStdString(); + params.id = _flightPlan.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 = AirspaceFlightPlanProvider::PermitRejected; + } else { + _flightPermitStatus = AirspaceFlightPlanProvider::PermitAccepted; + } + emit flightPermitStatusChanged(); + _state = State::Idle; + } else if(pending) { + //-- 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 +AirMapFlightPlanManager::_missionBoundingCubeChanged() +{ + //-- Creating a new flight plan? + if(_createPlan) { + _createPlan = false; + _createFlightPlan(); + } + //-- Plan is being modified + + + +} diff --git a/src/Airmap/AirMapFlightPlanManager.h b/src/Airmap/AirMapFlightPlanManager.h new file mode 100644 index 000000000..9f9183915 --- /dev/null +++ b/src/Airmap/AirMapFlightPlanManager.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * (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 "LifetimeChecker.h" +#include "AirMapSharedState.h" +#include "AirspaceFlightPlanProvider.h" + +#include +#include +#include +#include + +//----------------------------------------------------------------------------- +/// class to upload a flight +class AirMapFlightPlanManager : public AirspaceFlightPlanProvider, public LifetimeChecker +{ + Q_OBJECT +public: + AirMapFlightPlanManager(AirMapSharedState& shared, QObject *parent = nullptr); + + PermitStatus flightPermitStatus () const override { return _flightPermitStatus; } + void createFlight (MissionController* missionController) override; + QString flightID () { return _flightPlan; } + +signals: + void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); + +private slots: + void _pollBriefing (); + void _missionBoundingCubeChanged (); + +private: + void _uploadFlightPlan (); + void _createFlightPlan (); + +private: + enum class State { + Idle, + GetPilotID, + FlightUpload, + FlightPolling, + }; + + 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; + QTimer _pollTimer; ///< timer to poll for approval check + QString _flightPlan; ///< Current flight plan + QString _pilotID; ///< Pilot ID in the form "auth0|abc123" + MissionController* _controller = nullptr; + bool _createPlan = true; + + AirspaceFlightPlanProvider::PermitStatus _flightPermitStatus = AirspaceFlightPlanProvider::PermitUnknown; + +}; + diff --git a/src/Airmap/AirMapManager.cc b/src/Airmap/AirMapManager.cc index 9c7077300..f5aeeea8e 100644 --- a/src/Airmap/AirMapManager.cc +++ b/src/Airmap/AirMapManager.cc @@ -7,15 +7,16 @@ * ****************************************************************************/ -#include "AirMapManager.h" #include "AirMapAdvisoryManager.h" -#include "AirMapWeatherInfoManager.h" +#include "AirMapFlightPlanManager.h" +#include "AirMapManager.h" +#include "AirMapRestrictionManager.h" #include "AirMapRulesetsManager.h" #include "AirMapSettings.h" #include "AirMapTelemetry.h" -#include "AirMapRestrictionManager.h" #include "AirMapTrafficMonitor.h" #include "AirMapVehicleManager.h" +#include "AirMapWeatherInfoManager.h" #include "QmlObjectListModel.h" #include "JsonHelper.h" @@ -155,8 +156,10 @@ AirMapManager::_instantiateAirspaceRestrictionProvider() } //----------------------------------------------------------------------------- -void -AirMapManager::createFlight(MissionController* missionController) +AirspaceFlightPlanProvider* +AirMapManager::_instantiateAirspaceFlightPlanProvider() { - Q_UNUSED(missionController); + AirMapFlightPlanManager* flightPlan = new AirMapFlightPlanManager(_shared); + connect(flightPlan, &AirMapFlightPlanManager::error, this, &AirMapManager::_error); + return flightPlan; } diff --git a/src/Airmap/AirMapManager.h b/src/Airmap/AirMapManager.h index 43301b37a..5afcaa9d1 100644 --- a/src/Airmap/AirMapManager.h +++ b/src/Airmap/AirMapManager.h @@ -19,11 +19,6 @@ #include class QGCToolbox; -class AirspaceVehicleManager; -class AirspaceRulesetsProvider; -class AirspaceWeatherInfoProvider; -class AirspaceAdvisoryProvider; -class AirspaceRestrictionProvider; Q_DECLARE_LOGGING_CATEGORY(AirMapManagerLog) @@ -45,13 +40,13 @@ public: QString providerName () const override { return QString("AirMap"); } AirspaceVehicleManager* instantiateVehicle (const Vehicle& vehicle) override; - void createFlight (MissionController* missionController) override; protected: AirspaceRulesetsProvider* _instantiateRulesetsProvider () override; AirspaceWeatherInfoProvider* _instatiateAirspaceWeatherInfoProvider () override; AirspaceAdvisoryProvider* _instatiateAirspaceAdvisoryProvider () override; AirspaceRestrictionProvider* _instantiateAirspaceRestrictionProvider () override; + AirspaceFlightPlanProvider* _instantiateAirspaceFlightPlanProvider () override; private slots: void _error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); diff --git a/src/Airmap/AirMapVehicleManager.cc b/src/Airmap/AirMapVehicleManager.cc index 9ed63417f..50fc9b78c 100644 --- a/src/Airmap/AirMapVehicleManager.cc +++ b/src/Airmap/AirMapVehicleManager.cc @@ -38,7 +38,7 @@ AirMapVehicleManager::createFlight(const QList& missionItems) _flightManager.createFlight(missionItems); } -AirspaceAuthorization::PermitStatus +AirspaceFlightPlanProvider::PermitStatus AirMapVehicleManager::flightPermitStatus() const { return _flightManager.flightPermitStatus(); @@ -83,7 +83,7 @@ void AirMapVehicleManager::_flightPermitStatusChanged() { // activate traffic alerts - if (_flightManager.flightPermitStatus() == AirspaceAuthorization::PermitAccepted) { + if (_flightManager.flightPermitStatus() == AirspaceFlightPlanProvider::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()); diff --git a/src/Airmap/AirMapVehicleManager.h b/src/Airmap/AirMapVehicleManager.h index e7508aefc..a3debeff5 100644 --- a/src/Airmap/AirMapVehicleManager.h +++ b/src/Airmap/AirMapVehicleManager.h @@ -33,7 +33,7 @@ public: void stopTelemetryStream () override; bool isTelemetryStreaming () const override; - AirspaceAuthorization::PermitStatus flightPermitStatus() const override; + AirspaceFlightPlanProvider::PermitStatus flightPermitStatus() const override; signals: void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); diff --git a/src/AirspaceManagement/AirspaceFlightPlanProvider.cc b/src/AirspaceManagement/AirspaceFlightPlanProvider.cc new file mode 100644 index 000000000..888e9d0c3 --- /dev/null +++ b/src/AirspaceManagement/AirspaceFlightPlanProvider.cc @@ -0,0 +1,15 @@ +/**************************************************************************** + * + * (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 "AirspaceFlightPlanProvider.h" + +AirspaceFlightPlanProvider::AirspaceFlightPlanProvider(QObject *parent) + : QObject(parent) +{ +} diff --git a/src/AirspaceManagement/AirspaceFlightPlanProvider.h b/src/AirspaceManagement/AirspaceFlightPlanProvider.h new file mode 100644 index 000000000..ea28a903b --- /dev/null +++ b/src/AirspaceManagement/AirspaceFlightPlanProvider.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * (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 AirspaceFlightPlanProvider.h + * Create and maintain a flight plan + */ + +#include + +class MissionController; + +//----------------------------------------------------------------------------- +class AirspaceFlightPlanProvider : public QObject +{ + Q_OBJECT +public: + + enum PermitStatus { + PermitUnknown = 0, + PermitPending, + PermitAccepted, + PermitRejected, + }; + + Q_ENUM(PermitStatus) + + AirspaceFlightPlanProvider (QObject *parent = nullptr); + virtual ~AirspaceFlightPlanProvider () {} + + Q_PROPERTY(PermitStatus flightPermitStatus READ flightPermitStatus NOTIFY flightPermitStatusChanged) ///< state of flight permission + + virtual PermitStatus flightPermitStatus () const { return PermitUnknown; } + virtual void createFlight (MissionController* missionController) = 0; + + +signals: + void flightPermitStatusChanged (); +}; diff --git a/src/AirspaceManagement/AirspaceManager.cc b/src/AirspaceManagement/AirspaceManager.cc index 5b43782a8..a90306339 100644 --- a/src/AirspaceManagement/AirspaceManager.cc +++ b/src/AirspaceManagement/AirspaceManager.cc @@ -8,13 +8,14 @@ ****************************************************************************/ -#include "AirspaceManager.h" -#include "AirspaceWeatherInfoProvider.h" #include "AirspaceAdvisoryProvider.h" +#include "AirspaceFlightPlanProvider.h" +#include "AirspaceManager.h" #include "AirspaceRestriction.h" -#include "AirspaceRulesetsProvider.h" #include "AirspaceRestrictionProvider.h" +#include "AirspaceRulesetsProvider.h" #include "AirspaceVehicleManager.h" +#include "AirspaceWeatherInfoProvider.h" #include "Vehicle.h" #include "QGCApplication.h" @@ -29,7 +30,7 @@ AirspaceManager::AirspaceManager(QGCApplication* app, QGCToolbox* toolbox) _roiUpdateTimer.setSingleShot(true); connect(&_roiUpdateTimer, &QTimer::timeout, this, &AirspaceManager::_updateToROI); qmlRegisterUncreatableType ("QGroundControl.Airspace", 1, 0, "AirspaceAdvisoryProvider", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl.Airspace", 1, 0, "AirspaceAuthorization", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.Airspace", 1, 0, "AirspaceFlightPlanProvider", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Airspace", 1, 0, "AirspaceManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Airspace", 1, 0, "AirspaceRestrictionProvider", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Airspace", 1, 0, "AirspaceRule", "Reference only"); @@ -53,6 +54,9 @@ AirspaceManager::~AirspaceManager() if(_airspaces) { delete _airspaces; } + if(_flightPlan) { + delete _flightPlan; + } } void AirspaceManager::setToolbox(QGCToolbox* toolbox) @@ -63,6 +67,7 @@ void AirspaceManager::setToolbox(QGCToolbox* toolbox) _weatherProvider = _instatiateAirspaceWeatherInfoProvider(); _advisories = _instatiateAirspaceAdvisoryProvider(); _airspaces = _instantiateAirspaceRestrictionProvider(); + _flightPlan = _instantiateAirspaceFlightPlanProvider(); } void AirspaceManager::setROI(QGeoCoordinate center, double radiusMeters) diff --git a/src/AirspaceManagement/AirspaceManager.h b/src/AirspaceManagement/AirspaceManager.h index 6eb0f95de..c0f470b96 100644 --- a/src/AirspaceManagement/AirspaceManager.h +++ b/src/AirspaceManagement/AirspaceManager.h @@ -33,14 +33,14 @@ #include #include -class Vehicle; -class QGCApplication; -class AirspaceWeatherInfoProvider; -class AirspaceRulesetsProvider; -class AirspaceVehicleManager; class AirspaceAdvisoryProvider; +class AirspaceFlightPlanProvider; class AirspaceRestrictionProvider; -class MissionController; +class AirspaceRulesetsProvider; +class AirspaceVehicleManager; +class AirspaceWeatherInfoProvider; +class QGCApplication; +class Vehicle; Q_DECLARE_LOGGING_CATEGORY(AirspaceManagementLog) @@ -60,6 +60,7 @@ public: Q_PROPERTY(AirspaceAdvisoryProvider* advisories READ advisories CONSTANT) Q_PROPERTY(AirspaceRulesetsProvider* ruleSets READ ruleSets CONSTANT) Q_PROPERTY(AirspaceRestrictionProvider* airspaces READ airspaces CONSTANT) + Q_PROPERTY(AirspaceFlightPlanProvider* flightPlan READ flightPlan CONSTANT) Q_PROPERTY(bool airspaceVisible READ airspaceVisible WRITE setAirspaceVisible NOTIFY airspaceVisibleChanged) Q_INVOKABLE void setROI (QGeoCoordinate center, double radius); @@ -68,6 +69,7 @@ public: AirspaceAdvisoryProvider* advisories () { return _advisories; } AirspaceRulesetsProvider* ruleSets () { return _ruleSetsProvider; } AirspaceRestrictionProvider* airspaces () { return _airspaces; } + AirspaceFlightPlanProvider* flightPlan () { return _flightPlan; } void setToolbox(QGCToolbox* toolbox) override; @@ -81,11 +83,6 @@ public: */ virtual AirspaceVehicleManager* instantiateVehicle (const Vehicle& vehicle) = 0; - /** - * Create/upload a flight from a mission. - */ - virtual void createFlight (MissionController* missionController) = 0; - signals: void airspaceVisibleChanged (); @@ -98,31 +95,21 @@ protected: virtual void _setROI (const QGeoCoordinate& center, double radiusMeters); /** - * Factory method to create an AirspaceRulesetsProvider object + * Factory methods */ virtual AirspaceRulesetsProvider* _instantiateRulesetsProvider () = 0; - - /** - * Factory method to create an AirspaceRulesetsProvider object - */ virtual AirspaceWeatherInfoProvider* _instatiateAirspaceWeatherInfoProvider () = 0; - - /** - * Factory method to create an AirspaceAdvisoryProvider object - */ virtual AirspaceAdvisoryProvider* _instatiateAirspaceAdvisoryProvider () = 0; - - /** - * Factory method to create an AirspaceRestrictionProvider object - */ virtual AirspaceRestrictionProvider* _instantiateAirspaceRestrictionProvider () = 0; + virtual AirspaceFlightPlanProvider* _instantiateAirspaceFlightPlanProvider () = 0; protected: bool _airspaceVisible; - AirspaceRulesetsProvider* _ruleSetsProvider = nullptr; ///< Rulesets that are shown in the UI - AirspaceWeatherInfoProvider* _weatherProvider = nullptr; ///< Weather info that is shown in the UI - AirspaceAdvisoryProvider* _advisories = nullptr; ///< Advisory info that is shown in the UI - AirspaceRestrictionProvider* _airspaces = nullptr; ///< Airspace info that is shown in the UI + AirspaceRulesetsProvider* _ruleSetsProvider = nullptr; ///< Rulesets + AirspaceWeatherInfoProvider* _weatherProvider = nullptr; ///< Weather info + AirspaceAdvisoryProvider* _advisories = nullptr; ///< Advisory info + AirspaceRestrictionProvider* _airspaces = nullptr; ///< Airspace info + AirspaceFlightPlanProvider* _flightPlan = nullptr; ///< Flight plan management QTimer _roiUpdateTimer; QGeoCoordinate _roiCenter; double _roiRadius; diff --git a/src/AirspaceManagement/AirspaceVehicleManager.h b/src/AirspaceManagement/AirspaceVehicleManager.h index 448c097c8..aeeae3863 100644 --- a/src/AirspaceManagement/AirspaceVehicleManager.h +++ b/src/AirspaceManagement/AirspaceVehicleManager.h @@ -9,7 +9,7 @@ #pragma once -#include "AirspaceAuthorization.h" +#include "AirspaceFlightPlanProvider.h" #include "QGCMAVLink.h" #include @@ -40,7 +40,7 @@ public: /** * get the current flight permit status */ - virtual AirspaceAuthorization::PermitStatus flightPermitStatus() const = 0; + virtual AirspaceFlightPlanProvider::PermitStatus flightPermitStatus() const = 0; /** * Setup the connection and start sending telemetry diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index df03b8aa8..c5af36c3e 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -634,7 +634,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: QGroundControl.airmapSupported && _mainIsMap && flightPermit && flightPermit !== AirspaceAuthorization.PermitUnknown && !messageArea.visible && !criticalMmessageArea.visible + visible: QGroundControl.airmapSupported && _mainIsMap && flightPermit && flightPermit !== AirspaceFlightPlanProvider.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) @@ -649,20 +649,20 @@ QGCView { QGCLabel { text: { if(airspaceIndicator.flightPermit) { - if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitPending) + if(airspaceIndicator.flightPermit === AirspaceFlightPlanProvider.PermitPending) return qsTr("Approval Pending") - if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitAccepted) + if(airspaceIndicator.flightPermit === AirspaceFlightPlanProvider.PermitAccepted) return qsTr("Flight Approved") - if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitRejected) + if(airspaceIndicator.flightPermit === AirspaceFlightPlanProvider.PermitRejected) return qsTr("Flight Rejected") } return "" } color: { if(airspaceIndicator.flightPermit) { - if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitPending) + if(airspaceIndicator.flightPermit === AirspaceFlightPlanProvider.PermitPending) return qgcPal.colorOrange - if(airspaceIndicator.flightPermit === AirspaceAuthorization.PermitAccepted) + if(airspaceIndicator.flightPermit === AirspaceFlightPlanProvider.PermitAccepted) return qgcPal.colorGreen } return qgcPal.colorRed @@ -670,7 +670,7 @@ QGCView { anchors.verticalCenter: parent.verticalCenter; } } - property var flightPermit: (QGroundControl.airmapSupported && _activeVehicle) ? _activeVehicle.flightPermitStatus : null + property var flightPermit: QGroundControl.airmapSupported ? QGroundControl.airspaceManager.flightPlan.flightPermitStatus : null property var providerName: QGroundControl.airspaceManager.providerName } diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 4e4ef1053..2934e91ea 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -203,7 +203,7 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque //-- If airspace management enabled, create new flight #if defined(QGC_AIRMAP_ENABLED) if(qgcApp()->toolbox()->settingsManager()->airMapSettings()->enableAirMap()->rawValue().toBool()) { - qgcApp()->toolbox()->airspaceManager()->createFlight(this); + qgcApp()->toolbox()->airspaceManager()->flightPlan()->createFlight(this); } #endif } @@ -371,7 +371,7 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i) //-- If airspace management enabled and this is the first item, create new flight #if defined(QGC_AIRMAP_ENABLED) if(_visualItems->count() == 2 && qgcApp()->toolbox()->settingsManager()->airMapSettings()->enableAirMap()->rawValue().toBool()) { - qgcApp()->toolbox()->airspaceManager()->createFlight(this); + qgcApp()->toolbox()->airspaceManager()->flightPlan()->createFlight(this); } #endif @@ -464,7 +464,7 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate //-- If airspace management enabled and this is the first item, create new flight #if defined(QGC_AIRMAP_ENABLED) if(_visualItems->count() == 2 && qgcApp()->toolbox()->settingsManager()->airMapSettings()->enableAirMap()->rawValue().toBool()) { - qgcApp()->toolbox()->airspaceManager()->createFlight(this); + qgcApp()->toolbox()->airspaceManager()->flightPlan()->createFlight(this); } #endif diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index b89408970..35490090d 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -78,16 +78,6 @@ void PlanManager::writeMissionItems(const QList& missionItems) return; } - if (_planType == MAV_MISSION_TYPE_MISSION) { -#if defined(QGC_AIRMAP_ENABLED) - // upload the flight to the airspace management backend - AirspaceVehicleManager* airspaceManager = _vehicle->airspaceManager(); - if (airspaceManager) { - airspaceManager->createFlight(missionItems); - } -#endif - } - _clearAndDeleteWriteMissionItems(); bool skipFirstItem = _planType == MAV_MISSION_TYPE_MISSION && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle(); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 2babeb483..13fd5ab74 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -461,7 +461,6 @@ void Vehicle::_commonInit(void) _airspaceVehicleManager = airspaceManager->instantiateVehicle(*this); if (_airspaceVehicleManager) { connect(_airspaceVehicleManager, &AirspaceVehicleManager::trafficUpdate, this, &Vehicle::_trafficUpdate); - connect(_airspaceVehicleManager, &AirspaceVehicleManager::flightPermitStatusChanged, this, &Vehicle::flightPermitStatusChanged); } } #endif diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 4af71041a..4bf19a3a1 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -357,9 +357,6 @@ 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 -#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) @@ -762,9 +759,6 @@ public: void prepareDelete(); #if defined(QGC_AIRMAP_ENABLED) - AirspaceAuthorization::PermitStatus flightPermitStatus() const - { return _airspaceVehicleManager ? _airspaceVehicleManager->flightPermitStatus() : AirspaceAuthorization::PermitUnknown; } - AirspaceVehicleManager* airspaceManager() const { return _airspaceVehicleManager; } #endif @@ -802,9 +796,6 @@ 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 (); -- 2.22.0