From 9359c1545be17218f7e8cdf85869c0e64c77594a Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Tue, 27 Feb 2018 00:21:26 -0500 Subject: [PATCH] Cleaning up old vehicle manager and getting it ready to actually provide services for a vehicle (flight management) Added submit flight plan Added update flight plan Disable flight features as these don't seem to be quite working (values must be initialized and validated before submission as backend doesn't validate and the objects don't initialize their members) --- src/Airmap/AirMapAdvisoryManager.cc | 3 +- src/Airmap/AirMapFlightManager.cc | 387 ++---------------- src/Airmap/AirMapFlightManager.h | 81 +--- src/Airmap/AirMapFlightPlanManager.cc | 96 ++++- src/Airmap/AirMapFlightPlanManager.h | 38 +- src/Airmap/AirMapManager.cc | 4 +- src/Airmap/AirMapRulesetsManager.cc | 14 +- src/Airmap/AirMapTelemetry.cc | 43 +- src/Airmap/AirMapTelemetry.h | 29 +- src/Airmap/AirMapTrafficMonitor.cc | 10 + src/Airmap/AirMapTrafficMonitor.h | 15 +- src/Airmap/AirMapVehicleManager.cc | 49 +-- src/Airmap/AirMapVehicleManager.h | 14 +- src/Airmap/AirMapWeatherInfoManager.cc | 3 +- src/Airmap/AirspaceControl.qml | 3 +- src/Airmap/FlightBrief.qml | 17 +- .../AirspaceFlightPlanProvider.h | 4 + .../AirspaceVehicleManager.cc | 1 + .../AirspaceVehicleManager.h | 14 +- src/MissionManager/PlanMasterController.cc | 3 + src/QmlControls/QGCGeoBoundingCube.cc | 12 + src/QmlControls/QGCGeoBoundingCube.h | 2 + src/Vehicle/Vehicle.cc | 4 + src/Vehicle/Vehicle.h | 11 +- src/ui/MainWindow.cc | 4 +- 25 files changed, 272 insertions(+), 589 deletions(-) diff --git a/src/Airmap/AirMapAdvisoryManager.cc b/src/Airmap/AirMapAdvisoryManager.cc index 40d09c982..090b6af48 100644 --- a/src/Airmap/AirMapAdvisoryManager.cc +++ b/src/Airmap/AirMapAdvisoryManager.cc @@ -97,7 +97,8 @@ AirMapAdvisoryManager::_requestAdvisories() _advisories.endReset(); _valid = true; } else { - qCDebug(AirMapManagerLog) << "Advisories Request Failed"; + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + qCDebug(AirMapManagerLog) << "Advisories Request Failed" << QString::fromStdString(result.error().message()) << description; } emit advisoryChanged(); }); diff --git a/src/Airmap/AirMapFlightManager.cc b/src/Airmap/AirMapFlightManager.cc index 1d34f87de..72ac56c05 100644 --- a/src/Airmap/AirMapFlightManager.cc +++ b/src/Airmap/AirMapFlightManager.cc @@ -12,7 +12,6 @@ #include "AirMapRulesetsManager.h" #include "QGCApplication.h" -#include "MissionItem.h" #include "QGCMAVLink.h" #include "airmap/pilots.h" @@ -23,367 +22,71 @@ using namespace airmap; +//----------------------------------------------------------------------------- AirMapFlightManager::AirMapFlightManager(AirMapSharedState& shared) : _shared(shared) { - connect(&_pollTimer, &QTimer::timeout, this, &AirMapFlightManager::_pollBriefing); -} - -void AirMapFlightManager::createFlight(const QList& 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 = AirspaceFlightPlanProvider::PermitNone; - 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 = AirspaceFlightPlanProvider::PermitPending; - emit flightPermitStatusChanged(); } -void AirMapFlightManager::_endFirstFlight() +//----------------------------------------------------------------------------- +void +AirMapFlightManager::findFlight(const QGCGeoBoundingCube& bc) { - // 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; - + _state = State::FetchFlights; 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? - //-- Rules - AirMapRulesetsManager* pRulesMgr = dynamic_cast(qgcApp()->toolbox()->airspaceManager()->ruleSets()); - if(pRulesMgr) { - for(int rs = 0; rs < pRulesMgr->ruleSets()->count(); rs++) { - AirMapRuleSet* ruleSet = qobject_cast(pRulesMgr->ruleSets()->get(rs)); - //-- If this ruleset is selected - if(ruleSet && ruleSet->selected()) { - params.rulesets.push_back(ruleSet->id().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(); - _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 = AirspaceFlightPlanProvider::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; + QList coords = bc.polygon2D(); + Geometry::LineString lineString; + for (const auto& qcoord : coords) { + Geometry::Coordinate coord; + coord.latitude = qcoord.latitude(); + coord.longitude = qcoord.longitude(); + lineString.coordinates.push_back(coord); } - - FlightPlans::RenderBriefing::Parameters params; - params.authorization = _shared.loginToken().toStdString(); - params.id = _pendingFlightPlan.toStdString(); + _flightID.clear(); + params.geometry = Geometry(lineString); std::weak_ptr isAlive(_instance); - _shared.client()->flight_plans().render_briefing(params, [this, isAlive](const FlightPlans::RenderBriefing::Result& result) { + _shared.client()->flights().search(params, [this, isAlive, bc](const Flights::Search::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; + if (_state != State::FetchFlights) return; + if (result && result.value().flights.size() > 0) { + const Flights::Search::Response& response = result.value(); + qCDebug(AirMapManagerLog) << "Find flights response"; + for (const auto& flight : response.flights) { + QString fid = QString::fromStdString(flight.id); + qCDebug(AirMapManagerLog) << "Checking flight:" << fid; + if(flight.geometry.type() == Geometry::Type::line_string) { + const Geometry::LineString& lineString = flight.geometry.details_for_line_string(); + QList rcoords; + for (const auto& vertex : lineString.coordinates) { + rcoords.append(QGeoCoordinate(vertex.latitude, vertex.longitude)); + } + if(bc == rcoords) { + qCDebug(AirMapManagerLog) << "Found match:" << fid; + _flightID = fid; + emit flightIDChanged(); + return; + } } - _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; } + qCDebug(AirMapManagerLog) << "No flights found"; + emit flightIDChanged(); }); } -void AirMapFlightManager::endFlight() +//----------------------------------------------------------------------------- +void +AirMapFlightManager::endFlight(const QString& flightID) { - if (_currentFlightId.length() == 0) { - return; - } if (_state != State::Idle) { qCWarning(AirMapManagerLog) << "AirMapFlightManager::endFlight: State not idle"; return; } - _endFlight(_currentFlightId); - - _flightPermitStatus = AirspaceFlightPlanProvider::PermitNone; - emit flightPermitStatusChanged(); -} - -void AirMapFlightManager::_endFlight(const QString& flightID) -{ - qCDebug(AirMapManagerLog) << "ending flight" << 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(); @@ -391,16 +94,8 @@ void AirMapFlightManager::_endFlight(const QString& flightID) _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 { + if (!result) { QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); emit error("Failed to end Flight", QString::fromStdString(result.error().message()), description); diff --git a/src/Airmap/AirMapFlightManager.h b/src/Airmap/AirMapFlightManager.h index e708d690c..e1286831d 100644 --- a/src/Airmap/AirMapFlightManager.h +++ b/src/Airmap/AirMapFlightManager.h @@ -18,19 +18,7 @@ #include #include -class MissionItem; - - - -/* - * TODO: This is here for reference. It will have to modifed quite a bit before it - * can be used. This was the original file that combined both the creation of a - * flight plan and immediate submission of a flight. The flight plan is now handled - * by its own class. This now needs to be made into a proper "Flight Manager". -*/ - - - +//-- TODO: This is not even WIP yet. Just a skeleton of what's to come. //----------------------------------------------------------------------------- /// class to upload a flight @@ -38,79 +26,28 @@ class AirMapFlightManager : public QObject, public LifetimeChecker { Q_OBJECT public: - AirMapFlightManager(AirMapSharedState& shared); - - /// Send flight path to AirMap - void createFlight(const QList& missionItems); - - AirspaceFlightPlanProvider::PermitStatus flightPermitStatus() const { return _flightPermitStatus; } - - const QString& flightID() const { return _currentFlightId; } + AirMapFlightManager (AirMapSharedState& shared); -public slots: - void endFlight(); + void findFlight (const QGCGeoBoundingCube& bc); + void endFlight (const QString& id); + QString flightID () { return _flightID; } signals: - void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); - void flightPermitStatusChanged(); - -private slots: - void _pollBriefing(); + void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); + void flightIDChanged (); 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 + FetchFlights, 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 - AirspaceFlightPlanProvider::PermitStatus _flightPermitStatus = AirspaceFlightPlanProvider::PermitNone; + QString _flightID; 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 index e0f3db5d8..bc854cde4 100644 --- a/src/Airmap/AirMapFlightPlanManager.cc +++ b/src/Airmap/AirMapFlightPlanManager.cc @@ -86,6 +86,13 @@ AirMapFlightPlanManager::startFlightPlanning(PlanMasterController *planControlle } //-- TODO: Check if there is an ongoing flight plan and do something about it (Delete it?) + + /* + * if(!_flightPlan.isEmpty()) { + * do something; + * } + */ + if(!_planController) { _planController = planController; //-- Get notified of mission changes @@ -93,6 +100,52 @@ AirMapFlightPlanManager::startFlightPlanning(PlanMasterController *planControlle } } +//----------------------------------------------------------------------------- +void +AirMapFlightPlanManager::submitFlightPlan() +{ + if(_flightPlan.isEmpty()) { + qCWarning(AirMapManagerLog) << "Submit flight with no flight plan."; + return; + } + _state = State::FlightSubmit; + FlightPlans::Submit::Parameters params; + params.authorization = _shared.loginToken().toStdString(); + params.id = _flightPlan.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) { + _flightId = 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 +AirMapFlightPlanManager::updateFlightPlan() +{ + //-- Are we enabled? + if(!qgcApp()->toolbox()->settingsManager()->airMapSettings()->enableAirMap()->rawValue().toBool()) { + return; + } + //-- Do we have a license? + if(!_shared.hasAPIKey()) { + return; + } + _flightPermitStatus = AirspaceFlightPlanProvider::PermitPending; + emit flightPermitStatusChanged(); + _updateFlightPlan(); +} + //----------------------------------------------------------------------------- bool AirMapFlightPlanManager::_collectFlightDtata() @@ -215,6 +268,7 @@ AirMapFlightPlanManager::_uploadFlightPlan() if(ruleSet && ruleSet->selected()) { params.rulesets.push_back(ruleSet->id().toStdString()); //-- Features within each rule + /* for(int r = 0; r < ruleSet->rules()->count(); r++) { AirMapRule* rule = qobject_cast(ruleSet->rules()->get(r)); if(rule) { @@ -238,6 +292,7 @@ AirMapFlightPlanManager::_uploadFlightPlan() } } } + */ } } } @@ -258,12 +313,12 @@ AirMapFlightPlanManager::_uploadFlightPlan() if (result) { _flightPlan = QString::fromStdString(result.value().id); qCDebug(AirMapManagerLog) << "Flight plan created:" << _flightPlan; - _state = State::Idle; + _state = State::FlightPlanPolling; _pollBriefing(); } else { _state = State::Idle; QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); - emit error("Flight Plan creation failed", QString::fromStdString(result.error().message()), description); + emit error("Flight Plan update failed", QString::fromStdString(result.error().message()), description); } }); }); @@ -293,12 +348,13 @@ AirMapFlightPlanManager::_updateFlightPlan() _shared.doRequestWithLogin([this, isAlive](const QString& login_token) { if (!isAlive.lock()) return; if (_state != State::FlightUpdate) return; - FlightPlans::Update::Parameters params; - params.authorization = login_token.toStdString(); - params.flight_plan.id = _flightPlan.toStdString(); - params.flight_plan.pilot.id = _pilotID.toStdString(); - params.flight_plan.altitude_agl.max = _flight.maxAltitude; - params.flight_plan.buffer = 2.f; + FlightPlans::Update::Parameters params = {}; + params.authorization = login_token.toStdString(); + params.flight_plan.id = _flightPlan.toStdString(); + params.flight_plan.pilot.id = _pilotID.toStdString(); + params.flight_plan.altitude_agl.max = _flight.maxAltitude; + params.flight_plan.altitude_agl.min = 0.0f; + params.flight_plan.buffer = 2.f; params.flight_plan.takeoff.latitude = _flight.takeoffCoord.latitude(); params.flight_plan.takeoff.longitude = _flight.takeoffCoord.longitude(); /* @@ -309,8 +365,8 @@ AirMapFlightPlanManager::_updateFlightPlan() quint64 end = _flightEndTime.toUTC().toMSecsSinceEpoch(); */ - params.flight_plan.start_time = Clock::universal_time() + Minutes{5}; - params.flight_plan.end_time = Clock::universal_time() + Hours{2}; + params.flight_plan.start_time = Clock::universal_time() + Minutes{5}; + params.flight_plan.end_time = Clock::universal_time() + Hours{2}; //-- Rules /* AirMapRulesetsManager* pRulesMgr = dynamic_cast(qgcApp()->toolbox()->airspaceManager()->ruleSets()); @@ -334,7 +390,7 @@ AirMapFlightPlanManager::_updateFlightPlan() if (!isAlive.lock()) return; if (_state != State::FlightUpdate) return; if (result) { - _state = State::Idle; + _state = State::FlightPlanPolling; qCDebug(AirMapManagerLog) << "Flight plan updated:" << _flightPlan; _pollBriefing(); } else { @@ -370,19 +426,16 @@ rules_sort(QObject* a, QObject* b) void AirMapFlightPlanManager::_pollBriefing() { - if (_state != State::Idle) { - qCWarning(AirMapManagerLog) << "AirMapFlightPlanManager::_pollBriefing: not idle" << (int)_state; - _pollTimer.start(500); + if (_state != State::FlightPlanPolling && _state != State::FlightPolling) { return; } - _state = State::FlightPolling; FlightPlans::RenderBriefing::Parameters params; params.authorization = _shared.loginToken().toStdString(); - params.id = _flightPlan.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 (_state != State::FlightPlanPolling && _state != State::FlightPolling) return; if (result) { const FlightPlan::Briefing& briefing = result.value(); qCDebug(AirMapManagerLog) << "Flight polling/briefing response"; @@ -486,11 +539,10 @@ AirMapFlightPlanManager::_pollBriefing() } emit flightPermitStatusChanged(); _state = State::Idle; - } else if(pending) { - //-- Wait until we send the next polling request - _state = State::Idle; + } else { + //-- Pending. Try again. _pollTimer.setSingleShot(true); - _pollTimer.start(2000); + _pollTimer.start(1000); } } else { _state = State::Idle; @@ -549,7 +601,7 @@ AirMapFlightPlanManager::_missionChanged() _createFlightPlan(); } else { //-- Plan is being modified - //_updateFlightPlan(); + _updateFlightPlan(); } } } diff --git a/src/Airmap/AirMapFlightPlanManager.h b/src/Airmap/AirMapFlightPlanManager.h index 3b85925fa..74ca5da88 100644 --- a/src/Airmap/AirMapFlightPlanManager.h +++ b/src/Airmap/AirMapFlightPlanManager.h @@ -26,18 +26,19 @@ class AirMapFlightPlanManager : public AirspaceFlightPlanProvider, public Lifeti { Q_OBJECT public: - AirMapFlightPlanManager (AirMapSharedState& shared, QObject *parent = nullptr); - ~AirMapFlightPlanManager (); + AirMapFlightPlanManager (AirMapSharedState& shared, QObject *parent = nullptr); + ~AirMapFlightPlanManager (); PermitStatus flightPermitStatus () const override { return _flightPermitStatus; } - QString flightID () { return _flightPlan; } QDateTime flightStartTime () const override { return _flightStartTime; } QDateTime flightEndTime () const override { return _flightEndTime; } bool valid () override { return _valid; } QmlObjectListModel* advisories () override { return &_advisories; } QmlObjectListModel* ruleSets () override { return &_rulesets; } QGCGeoBoundingCube* missionArea () override { return &_flight.bc; } - AirspaceAdvisoryProvider::AdvisoryColor airspaceColor () override { return _airspaceColor; } + + AirspaceAdvisoryProvider::AdvisoryColor + airspaceColor () override { return _airspaceColor; } QmlObjectListModel* rulesViolation () override { return &_rulesViolation; } QmlObjectListModel* rulesInfo () override { return &_rulesInfo; } @@ -45,23 +46,25 @@ public: QmlObjectListModel* rulesFollowing () override { return &_rulesFollowing; } QmlObjectListModel* briefFeatures () override { return &_briefFeatures; } - void startFlightPlanning (PlanMasterController* planController) override; - void setFlightStartTime (QDateTime start) override; - void setFlightEndTime (QDateTime end) override; + void updateFlightPlan () override; + void startFlightPlanning (PlanMasterController* planController) override; + void setFlightStartTime (QDateTime start) override; + void setFlightEndTime (QDateTime end) override; + void submitFlightPlan () override; signals: - void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); + void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); private slots: - void _pollBriefing (); - void _missionChanged (); + void _pollBriefing (); + void _missionChanged (); private: - void _uploadFlightPlan (); - void _updateFlightPlan (); - void _createFlightPlan (); - void _deleteFlightPlan (); - bool _collectFlightDtata (); + void _uploadFlightPlan (); + void _updateFlightPlan (); + void _createFlightPlan (); + void _deleteFlightPlan (); + bool _collectFlightDtata (); private: enum class State { @@ -69,8 +72,10 @@ private: GetPilotID, FlightUpload, FlightUpdate, + FlightPlanPolling, + FlightDelete, + FlightSubmit, FlightPolling, - FlightDelete }; struct Flight { @@ -90,6 +95,7 @@ private: AirMapSharedState& _shared; QTimer _pollTimer; ///< timer to poll for approval check QString _flightPlan; ///< Current flight plan + QString _flightId; ///< Current flight ID, not necessarily accepted yet QString _pilotID; ///< Pilot ID in the form "auth0|abc123" PlanMasterController* _planController = nullptr; bool _valid = false; diff --git a/src/Airmap/AirMapManager.cc b/src/Airmap/AirMapManager.cc index 58b24c039..9020d99f2 100644 --- a/src/Airmap/AirMapManager.cc +++ b/src/Airmap/AirMapManager.cc @@ -95,7 +95,7 @@ AirMapManager::_settingsChanged() qCDebug(AirMapManagerLog) << "Creating AirMap client"; auto credentials = Credentials{}; credentials.api_key = _shared.settings().apiKey.toStdString(); - auto configuration = Client::default_staging_configuration(credentials); + auto configuration = Client::default_production_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"; @@ -114,7 +114,7 @@ AirMapManager::_settingsChanged() AirspaceVehicleManager* AirMapManager::instantiateVehicle(const Vehicle& vehicle) { - AirMapVehicleManager* manager = new AirMapVehicleManager(_shared, vehicle, *_toolbox); + AirMapVehicleManager* manager = new AirMapVehicleManager(_shared, vehicle); connect(manager, &AirMapVehicleManager::error, this, &AirMapManager::_error); return manager; } diff --git a/src/Airmap/AirMapRulesetsManager.cc b/src/Airmap/AirMapRulesetsManager.cc index 346013bf6..4d0cfe8d9 100644 --- a/src/Airmap/AirMapRulesetsManager.cc +++ b/src/Airmap/AirMapRulesetsManager.cc @@ -29,7 +29,19 @@ AirMapRuleFeature::AirMapRuleFeature(airmap::RuleSet::Feature feature, QObject* //-- Restore persisted value (if it exists) QSettings settings; settings.beginGroup(kAirMapFeatureGroup); - _value = settings.value(name()); + switch(_feature.type) { + case RuleSet::Feature::Type::boolean: + _value = settings.value(name(), false); + break;; + case RuleSet::Feature::Type::floating_point: + _value = settings.value(name(), 0.0f); + break;; + case RuleSet::Feature::Type::string: + _value = settings.value(name(), QString()); + break;; + default: + break; + } settings.endGroup(); } diff --git a/src/Airmap/AirMapTelemetry.cc b/src/Airmap/AirMapTelemetry.cc index eeb416253..0005d57b2 100644 --- a/src/Airmap/AirMapTelemetry.cc +++ b/src/Airmap/AirMapTelemetry.cc @@ -17,12 +17,15 @@ using namespace airmap; +//----------------------------------------------------------------------------- AirMapTelemetry::AirMapTelemetry(AirMapSharedState& shared) -: _shared(shared) + : _shared(shared) { } -void AirMapTelemetry::vehicleMavlinkMessageReceived(const mavlink_message_t& message) +//----------------------------------------------------------------------------- +void +AirMapTelemetry::vehicleMessageReceived(const mavlink_message_t& message) { switch (message.msgid) { case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: @@ -32,23 +35,24 @@ void AirMapTelemetry::vehicleMavlinkMessageReceived(const mavlink_message_t& mes _handleGPSRawInt(message); break; } - } -bool AirMapTelemetry::isTelemetryStreaming() const +//----------------------------------------------------------------------------- +bool +AirMapTelemetry::isTelemetryStreaming() { return _state == State::Streaming; } -void AirMapTelemetry::_handleGPSRawInt(const mavlink_message_t& message) +//----------------------------------------------------------------------------- +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 { @@ -56,15 +60,15 @@ void AirMapTelemetry::_handleGPSRawInt(const mavlink_message_t& message) } } -void AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message) +//----------------------------------------------------------------------------- +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, @@ -84,21 +88,19 @@ void AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message) flight.id = _flightID.toStdString(); _shared.client()->telemetry().submit_updates(flight, _key, {Telemetry::Update{position}, Telemetry::Update{speed}}); - } -void AirMapTelemetry::startTelemetryStream(const QString& flightID) +//----------------------------------------------------------------------------- +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; - + _state = State::StartCommunication; + _flightID = flightID; Flights::StartFlightCommunications::Parameters params; params.authorization = _shared.loginToken().toStdString(); params.id = _flightID.toStdString(); @@ -106,7 +108,6 @@ void AirMapTelemetry::startTelemetryStream(const QString& flightID) _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; @@ -119,13 +120,14 @@ void AirMapTelemetry::startTelemetryStream(const QString& flightID) }); } -void AirMapTelemetry::stopTelemetryStream() +//----------------------------------------------------------------------------- +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(); @@ -135,7 +137,6 @@ void AirMapTelemetry::stopTelemetryStream() Q_UNUSED(result); if (!isAlive.lock()) return; if (_state != State::EndCommunication) return; - _key = ""; _state = State::Idle; }); diff --git a/src/Airmap/AirMapTelemetry.h b/src/Airmap/AirMapTelemetry.h index 58a2e8159..8b67ae039 100644 --- a/src/Airmap/AirMapTelemetry.h +++ b/src/Airmap/AirMapTelemetry.h @@ -16,33 +16,28 @@ #include -/// class to send telemetry data to AirMap +/// Class to send telemetry data to AirMap class AirMapTelemetry : public QObject, public LifetimeChecker { Q_OBJECT public: - AirMapTelemetry(AirMapSharedState& shared); - virtual ~AirMapTelemetry() = default; + AirMapTelemetry (AirMapSharedState& shared); + virtual ~AirMapTelemetry () = default; - /** - * Setup the connection to start sending telemetry - */ - void startTelemetryStream(const QString& flightID); - - void stopTelemetryStream(); - - bool isTelemetryStreaming() const; + void startTelemetryStream (const QString& flightID); + void stopTelemetryStream (); + bool isTelemetryStreaming (); signals: - void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); + void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); public slots: - void vehicleMavlinkMessageReceived(const mavlink_message_t& message); + void vehicleMessageReceived (const mavlink_message_t& message); private: - void _handleGlobalPositionInt(const mavlink_message_t& message); - void _handleGPSRawInt(const mavlink_message_t& message); + void _handleGlobalPositionInt (const mavlink_message_t& message); + void _handleGPSRawInt (const mavlink_message_t& message); enum class State { Idle, @@ -52,11 +47,9 @@ private: }; State _state = State::Idle; - AirMapSharedState& _shared; - std::string _key; ///< key for AES encryption (16 bytes) + std::string _key; ///< key for AES encryption (16 bytes) QString _flightID; - float _lastHdop = 1.f; }; diff --git a/src/Airmap/AirMapTrafficMonitor.cc b/src/Airmap/AirMapTrafficMonitor.cc index 9fec2972b..56d96acbb 100644 --- a/src/Airmap/AirMapTrafficMonitor.cc +++ b/src/Airmap/AirMapTrafficMonitor.cc @@ -12,11 +12,19 @@ using namespace airmap; +//----------------------------------------------------------------------------- +AirMapTrafficMonitor::AirMapTrafficMonitor(AirMapSharedState& shared) + : _shared(shared) +{ +} + +//----------------------------------------------------------------------------- AirMapTrafficMonitor::~AirMapTrafficMonitor() { stop(); } +//----------------------------------------------------------------------------- void AirMapTrafficMonitor::startConnection(const QString& flightID) { @@ -39,6 +47,7 @@ AirMapTrafficMonitor::startConnection(const QString& flightID) _shared.client()->traffic().monitor(params, handler); } +//----------------------------------------------------------------------------- void AirMapTrafficMonitor::_update(Traffic::Update::Type type, const std::vector& update) { @@ -53,6 +62,7 @@ AirMapTrafficMonitor::_update(Traffic::Update::Type type, const std::vector& update); + void _update (airmap::Traffic::Update::Type type, const std::vector& update); private: QString _flightID; diff --git a/src/Airmap/AirMapVehicleManager.cc b/src/Airmap/AirMapVehicleManager.cc index 6d2ea78ad..bd3a2d0c3 100644 --- a/src/Airmap/AirMapVehicleManager.cc +++ b/src/Airmap/AirMapVehicleManager.cc @@ -12,81 +12,58 @@ #include "Vehicle.h" -AirMapVehicleManager::AirMapVehicleManager(AirMapSharedState& shared, const Vehicle& vehicle, QGCToolbox& toolbox) +//----------------------------------------------------------------------------- +AirMapVehicleManager::AirMapVehicleManager(AirMapSharedState& shared, const Vehicle& vehicle) : AirspaceVehicleManager(vehicle) , _shared(shared) , _flightManager(shared) , _telemetry(shared) , _trafficMonitor(shared) - , _toolbox(toolbox) { - connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged, this, &AirMapVehicleManager::flightPermitStatusChanged); - connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged, this, &AirMapVehicleManager::_flightPermitStatusChanged); connect(&_flightManager, &AirMapFlightManager::error, this, &AirMapVehicleManager::error); connect(&_telemetry, &AirMapTelemetry::error, this, &AirMapVehicleManager::error); connect(&_trafficMonitor, &AirMapTrafficMonitor::error, this, &AirMapVehicleManager::error); connect(&_trafficMonitor, &AirMapTrafficMonitor::trafficUpdate, this, &AirspaceVehicleManager::trafficUpdate); } -void -AirMapVehicleManager::createFlight(const QList& missionItems) -{ - if (!_shared.client()) { - qCDebug(AirMapManagerLog) << "No AirMap client instance. Will not create a flight"; - return; - } - //_flightManager.createFlight(missionItems); -} - -AirspaceFlightPlanProvider::PermitStatus -AirMapVehicleManager::flightPermitStatus() const -{ - return _flightManager.flightPermitStatus(); -} - +//----------------------------------------------------------------------------- void AirMapVehicleManager::startTelemetryStream() { - if (_flightManager.flightID() != "") { + if (!_flightManager.flightID().isEmpty()) { _telemetry.startTelemetryStream(_flightManager.flightID()); } } +//----------------------------------------------------------------------------- void AirMapVehicleManager::stopTelemetryStream() { _telemetry.stopTelemetryStream(); } +//----------------------------------------------------------------------------- bool -AirMapVehicleManager::isTelemetryStreaming() const +AirMapVehicleManager::isTelemetryStreaming() { return _telemetry.isTelemetryStreaming(); } +//----------------------------------------------------------------------------- void AirMapVehicleManager::endFlight() { - _flightManager.endFlight(); + if (!_flightManager.flightID().isEmpty()) { + _flightManager.endFlight(_flightManager.flightID()); + } _trafficMonitor.stop(); } +//----------------------------------------------------------------------------- void AirMapVehicleManager::vehicleMavlinkMessageReceived(const mavlink_message_t& message) { if (isTelemetryStreaming()) { - _telemetry.vehicleMavlinkMessageReceived(message); + _telemetry.vehicleMessageReceived(message); } } - -void -AirMapVehicleManager::_flightPermitStatusChanged() -{ - // activate traffic alerts - 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 a3debeff5..87a962f96 100644 --- a/src/Airmap/AirMapVehicleManager.h +++ b/src/Airmap/AirMapVehicleManager.h @@ -16,24 +16,18 @@ #include "AirMapTelemetry.h" #include "AirMapTrafficMonitor.h" -#include "QGCToolbox.h" - /// AirMap per vehicle management class. class AirMapVehicleManager : public AirspaceVehicleManager { Q_OBJECT public: - AirMapVehicleManager (AirMapSharedState& shared, const Vehicle& vehicle, QGCToolbox& toolbox); + AirMapVehicleManager (AirMapSharedState& shared, const Vehicle& vehicle); ~AirMapVehicleManager () = default; - - void createFlight (const QList& missionItems) override; void startTelemetryStream () override; void stopTelemetryStream () override; - bool isTelemetryStreaming () const override; - - AirspaceFlightPlanProvider::PermitStatus flightPermitStatus() const override; + bool isTelemetryStreaming () override; signals: void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails); @@ -44,13 +38,9 @@ public slots: 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; }; diff --git a/src/Airmap/AirMapWeatherInfoManager.cc b/src/Airmap/AirMapWeatherInfoManager.cc index 8c2c51af7..766f4f10d 100644 --- a/src/Airmap/AirMapWeatherInfoManager.cc +++ b/src/Airmap/AirMapWeatherInfoManager.cc @@ -65,7 +65,8 @@ AirMapWeatherInfoManager::_requestWeatherUpdate(const QGeoCoordinate& coordinate qCDebug(AirMapManagerLog) << "Weather Info: " << _valid << "Icon:" << QString::fromStdString(_weather.icon) << "Condition:" << QString::fromStdString(_weather.condition) << "Temp:" << _weather.temperature; } else { _valid = false; - qCDebug(AirMapManagerLog) << "Request Weather Failed"; + QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : ""); + qCDebug(AirMapManagerLog) << "Request Weather Failed" << QString::fromStdString(result.error().message()) << description; } emit weatherChanged(); }); diff --git a/src/Airmap/AirspaceControl.qml b/src/Airmap/AirspaceControl.qml index 26c5fc843..762f6e79f 100644 --- a/src/Airmap/AirspaceControl.qml +++ b/src/Airmap/AirspaceControl.qml @@ -331,7 +331,7 @@ Item { } Flickable { clip: true - height: ScreenTools.defaultFontPixelHeight * 6 + height: ScreenTools.defaultFontPixelHeight * 8 contentHeight: advisoryCol.height flickableDirection: Flickable.VerticalFlick anchors.left: parent.left @@ -366,6 +366,7 @@ Item { heightFactor: 0.3333 showBorder: true width: ScreenTools.defaultFontPixelWidth * 16 + visible: _flightPermit !== AirspaceFlightPlanProvider.PermitNone anchors.horizontalCenter: parent.horizontalCenter onClicked: { rootLoader.sourceComponent = planView ? flightDetails : flightBrief diff --git a/src/Airmap/FlightBrief.qml b/src/Airmap/FlightBrief.qml index 6ae37f235..b6e47bb22 100644 --- a/src/Airmap/FlightBrief.qml +++ b/src/Airmap/FlightBrief.qml @@ -62,24 +62,27 @@ Item { anchors.left: parent.left anchors.verticalCenter: parent.verticalCenter QGCLabel { - text: qsTr("Federal Aviation Administration") + text: qsTr("No Authorization Required") visible: _flightPermit !== AirspaceFlightPlanProvider.PermitNone + anchors.horizontalCenter: parent.horizontalCenter } + /* QGCLabel { text: qsTr("Automatic authorization to fly in controlled airspace") visible: _flightPermit !== AirspaceFlightPlanProvider.PermitNone font.pointSize: ScreenTools.smallFontPointSize } + */ Rectangle { anchors.right: parent.right anchors.left: parent.left height: label.height + (ScreenTools.defaultFontPixelHeight * 0.5) color: { - if(_flightPermit == AirspaceFlightPlanProvider.PermitPending) + if(_flightPermit === AirspaceFlightPlanProvider.PermitPending) return _colorOrange - if(_flightPermit == AirspaceFlightPlanProvider.PermitAccepted) + if(_flightPermit === AirspaceFlightPlanProvider.PermitAccepted) return _colorGreen - if(_flightPermit == AirspaceFlightPlanProvider.PermitRejected) + if(_flightPermit === AirspaceFlightPlanProvider.PermitRejected) return _colorRed return _colorGray } @@ -164,9 +167,7 @@ Item { visible: planView width: ScreenTools.defaultFontPixelWidth * 12 onClicked: { - //-- TODO: Update Plan - mainWindow.enableToolbar() - rootLoader.sourceComponent = null + QGroundControl.airspaceManager.flightPlan.updateFlightPlan() } } QGCButton { @@ -174,7 +175,7 @@ Item { backRadius: 4 heightFactor: 0.3333 showBorder: true - enabled: _flightPermit !== AirspaceFlightPlanProvider.PermitNone + enabled: _flightPermit === AirspaceFlightPlanProvider.PermitAccepted width: ScreenTools.defaultFontPixelWidth * 12 visible: planView onClicked: { diff --git a/src/AirspaceManagement/AirspaceFlightPlanProvider.h b/src/AirspaceManagement/AirspaceFlightPlanProvider.h index ffc11fb6b..4a2a73250 100644 --- a/src/AirspaceManagement/AirspaceFlightPlanProvider.h +++ b/src/AirspaceManagement/AirspaceFlightPlanProvider.h @@ -55,6 +55,8 @@ public: Q_PROPERTY(QmlObjectListModel* rulesFollowing READ rulesFollowing NOTIFY rulesChanged) Q_PROPERTY(QmlObjectListModel* briefFeatures READ briefFeatures NOTIFY rulesChanged) + Q_INVOKABLE virtual void updateFlightPlan () = 0; + virtual PermitStatus flightPermitStatus () const { return PermitNone; } virtual QDateTime flightStartTime () const = 0; virtual QDateTime flightEndTime () const = 0; @@ -73,6 +75,8 @@ public: virtual void setFlightStartTime (QDateTime start) = 0; virtual void setFlightEndTime (QDateTime end) = 0; virtual void startFlightPlanning (PlanMasterController* planController) = 0; + //-- TODO: This will submit the current flight plan in memory. + virtual void submitFlightPlan () = 0; signals: void flightPermitStatusChanged (); diff --git a/src/AirspaceManagement/AirspaceVehicleManager.cc b/src/AirspaceManagement/AirspaceVehicleManager.cc index 5379ef26b..2e92c7cf8 100644 --- a/src/AirspaceManagement/AirspaceVehicleManager.cc +++ b/src/AirspaceManagement/AirspaceVehicleManager.cc @@ -9,6 +9,7 @@ #include "AirspaceManager.h" +#include "AirspaceVehicleManager.h" #include "Vehicle.h" #include "MissionItem.h" diff --git a/src/AirspaceManagement/AirspaceVehicleManager.h b/src/AirspaceManagement/AirspaceVehicleManager.h index aeeae3863..9e8bea279 100644 --- a/src/AirspaceManagement/AirspaceVehicleManager.h +++ b/src/AirspaceManagement/AirspaceVehicleManager.h @@ -16,7 +16,6 @@ #include #include -class MissionItem; class Vehicle; //----------------------------------------------------------------------------- @@ -31,23 +30,12 @@ public: AirspaceVehicleManager (const Vehicle& vehicle); virtual ~AirspaceVehicleManager () = 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 AirspaceFlightPlanProvider::PermitStatus flightPermitStatus() const = 0; - /** * Setup the connection and start sending telemetry */ virtual void startTelemetryStream () = 0; virtual void stopTelemetryStream () = 0; - virtual bool isTelemetryStreaming () const = 0; + virtual bool isTelemetryStreaming () = 0; public slots: virtual void endFlight () = 0; diff --git a/src/MissionManager/PlanMasterController.cc b/src/MissionManager/PlanMasterController.cc index 7d6db3f03..e568a57af 100644 --- a/src/MissionManager/PlanMasterController.cc +++ b/src/MissionManager/PlanMasterController.cc @@ -15,6 +15,9 @@ #include "JsonHelper.h" #include "MissionManager.h" #include "KML.h" +#if defined(QGC_AIRMAP_ENABLED) +#include "AirspaceFlightPlanProvider.h" +#endif #include #include diff --git a/src/QmlControls/QGCGeoBoundingCube.cc b/src/QmlControls/QGCGeoBoundingCube.cc index f62aa5a09..bc80e2283 100644 --- a/src/QmlControls/QGCGeoBoundingCube.cc +++ b/src/QmlControls/QGCGeoBoundingCube.cc @@ -60,6 +60,18 @@ QGCGeoBoundingCube::polygon2D() const return coords; } +//----------------------------------------------------------------------------- +bool +QGCGeoBoundingCube::operator ==(const QList& coords) const +{ + QList c = polygon2D(); + if(c.size() != coords.size()) return false; + for(int i = 0; i < c.size(); i++) { + if(c[i] != coords[i]) return false; + } + return true; +} + //----------------------------------------------------------------------------- double QGCGeoBoundingCube::width() const diff --git a/src/QmlControls/QGCGeoBoundingCube.h b/src/QmlControls/QGCGeoBoundingCube.h index 10f1ead54..3a672b57e 100644 --- a/src/QmlControls/QGCGeoBoundingCube.h +++ b/src/QmlControls/QGCGeoBoundingCube.h @@ -48,6 +48,8 @@ public: return pointNW == other.pointNW && pointSE == other.pointSE; } + bool operator ==(const QList& coords) const; + inline bool operator !=(const QGCGeoBoundingCube& other) { return !(*this == other); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 13fd5ab74..31bbf20ea 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -38,6 +38,10 @@ #include "QGCCameraManager.h" #include "VideoReceiver.h" #include "VideoManager.h" +#if defined(QGC_AIRMAP_ENABLED) +#include "AirspaceVehicleManager.h" +#endif + QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") #define UPDATE_TIMER 50 diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 4bf19a3a1..e21ac9dcf 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -20,10 +20,6 @@ #include "MAVLinkProtocol.h" #include "UASMessageHandler.h" #include "SettingsFact.h" -#if defined(QGC_AIRMAP_ENABLED) -#include "AirspaceManager.h" -#include "AirspaceVehicleManager.h" -#endif class UAS; class UASInterface; @@ -39,6 +35,9 @@ class UASMessage; class SettingsManager; class ADSBVehicle; class QGCCameraManager; +#if defined(QGC_AIRMAP_ENABLED) +class AirspaceVehicleManager; +#endif Q_DECLARE_LOGGING_CATEGORY(VehicleLog) @@ -758,10 +757,6 @@ public: /// Vehicle is about to be deleted void prepareDelete(); -#if defined(QGC_AIRMAP_ENABLED) - AirspaceVehicleManager* airspaceManager() const { return _airspaceVehicleManager; } -#endif - signals: void allLinksInactive(Vehicle* vehicle); void coordinateChanged(QGeoCoordinate coordinate); diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 457948e57..467db61a7 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -134,8 +134,8 @@ MainWindow::MainWindow() _ui.setupUi(this); // Make sure tool bar elements all fit before changing minimum width - setMinimumWidth(1008); - setMinimumHeight(520); + setMinimumWidth(1024); + setMinimumHeight(620); configureWindowName(); // Setup central widget with a layout to hold the views -- 2.22.0