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 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* (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 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* (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 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* (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 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* (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