Browse Source
Moved flight planning away from "Flight Management" to its own class. Planning does not need a connected vehicle.QGC4.4
Gus Grubba
7 years ago
19 changed files with 442 additions and 89 deletions
@ -0,0 +1,244 @@
@@ -0,0 +1,244 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "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<LifetimeChecker> 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<LifetimeChecker> 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<AirMapRulesetsManager*>(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<LifetimeChecker> 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
|
||||
|
||||
|
||||
|
||||
} |
||||
@ -0,0 +1,74 @@
@@ -0,0 +1,74 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2017 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include "LifetimeChecker.h" |
||||
#include "AirMapSharedState.h" |
||||
#include "AirspaceFlightPlanProvider.h" |
||||
|
||||
#include <QObject> |
||||
#include <QTimer> |
||||
#include <QList> |
||||
#include <QGeoCoordinate> |
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
/// 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<QGeoCoordinate> 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; |
||||
|
||||
}; |
||||
|
||||
@ -0,0 +1,15 @@
@@ -0,0 +1,15 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2017 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "AirspaceFlightPlanProvider.h" |
||||
|
||||
AirspaceFlightPlanProvider::AirspaceFlightPlanProvider(QObject *parent) |
||||
: QObject(parent) |
||||
{ |
||||
} |
||||
@ -0,0 +1,47 @@
@@ -0,0 +1,47 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2017 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
/**
|
||||
* @file AirspaceFlightPlanProvider.h |
||||
* Create and maintain a flight plan |
||||
*/ |
||||
|
||||
#include <QObject> |
||||
|
||||
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 (); |
||||
}; |
||||
Loading…
Reference in new issue