Commit 6b47e107 authored by Gus Grubba's avatar Gus Grubba
Browse files

WIP (not done and not working)

Break apart string of classes into their own files as all in one file became unwieldy
Expose Airspace data in a QML ready format
Split global Airspace from AirMap (Only now I figured out the reason for the pure virtual root classes)
Refactored a bunch of things to make it consistent and/or obvious
parent f3253d42
......@@ -1058,6 +1058,30 @@ SOURCES += \
# AirMap
contains (DEFINES, QGC_AIRMAP_ENABLED) {
#-- These should be always enabled but not yet
INCLUDEPATH += \
src/AirspaceManagement
HEADERS += \
src/AirspaceManagement/AirspaceController.h \
src/AirspaceManagement/AirspaceManagement.h \
src/AirspaceManagement/AirspaceRestriction.h \
src/AirspaceManagement/AirspaceRestrictionProvider.h \
src/AirspaceManagement/AirspaceRulesetsProvider.h \
src/AirspaceManagement/AirspaceWeatherInfoProvider.h \
src/AirspaceManagement/AirspaceVehicleManager.h \
SOURCES += \
src/AirspaceManagement/AirspaceController.cc \
src/AirspaceManagement/AirspaceManagement.cc \
src/AirspaceManagement/AirspaceRestriction.cc \
src/AirspaceManagement/AirspaceRestrictionProvider.cc \
src/AirspaceManagement/AirspaceRulesetsProvider.cc \
src/AirspaceManagement/AirspaceWeatherInfoProvider.cc \
src/AirspaceManagement/AirspaceVehicleManager.cc \
#-- This is the AirMap implementation of the above
RESOURCES += \
src/Airmap/airmap.qrc
......@@ -1065,16 +1089,32 @@ contains (DEFINES, QGC_AIRMAP_ENABLED) {
src/Airmap
HEADERS += \
src/Airmap/AirspaceController.h \
src/Airmap/AirMapManager.h \
src/Airmap/AirspaceManagement.h \
src/Airmap/AirMapSettings.h
src/Airmap/AirMapSettings.h \
src/Airmap/AirmapWeatherInformation.h \
src/Airmap/AirMapRestrictionManager.h \
src/Airmap/AirMapRulesetsManager.h \
src/Airmap/AirMapSharedState.h \
src/Airmap/AirMapFlightManager.h \
src/Airmap/AirMapTelemetry.h \
src/Airmap/AirMapTrafficMonitor.h \
src/Airmap/AirMapVehicleManager.h \
SOURCES += \
src/Airmap/AirMapManager.cc \
src/Airmap/AirspaceManagement.cc \
src/Airmap/AirspaceController.cc \
src/Airmap/AirMapSettings.cc
src/Airmap/AirMapSettings.cc \
src/Airmap/AirmapWeatherInformation.cc \
src/Airmap/AirMapRestrictionManager.cc \
src/Airmap/AirMapRulesetsManager.cc \
src/Airmap/AirMapSharedState.cc \
src/Airmap/AirMapFlightManager.cc \
src/Airmap/AirMapTelemetry.cc \
src/Airmap/AirMapTrafficMonitor.cc \
src/Airmap/AirMapVehicleManager.cc \
} else {
RESOURCES += \
src/Airmap/dummy/airmap_dummy.qrc
......
#include "AirMapFlightManager.h"
AirMapFlightManager::AirMapFlightManager(AirMapSharedState& shared)
: _shared(shared)
{
connect(&_pollTimer, &QTimer::timeout, this, &AirMapFlightManager::_pollBriefing);
}
void AirMapFlightManager::createFlight(const QList<MissionItem*>& 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<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;
_uploadFlight();
} else {
_flightPermitStatus = AirspaceAuthorization::PermitUnknown;
emit flightPermitStatusChanged();
_state = State::Idle;
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Failed to create Flight Plan",
QString::fromStdString(result.error().message()), description);
}
});
});
} else {
_uploadFlight();
}
_flightPermitStatus = AirspaceAuthorization::PermitPending;
emit flightPermitStatusChanged();
}
void AirMapFlightManager::_endFirstFlight()
{
// it could be that AirMap still has an open pending flight, but we don't know the flight ID.
// As there can only be one, we query the flights that end in the future, and close it if there's one.
_state = State::EndFirstFlight;
Flights::Search::Parameters params;
params.pilot_id = _pilotID.toStdString();
params.end_after = Clock::universal_time() - Hours{1};
std::weak_ptr<LifetimeChecker> 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<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?
params.rulesets = { // TODO: which ones to use?
"che_drone_rules",
"che_notam",
"che_airmap_rules",
"che_nature_preserve"
};
// geometry: LineString
Geometry::LineString lineString;
for (const auto& qcoord : _flight.coords) {
Geometry::Coordinate coord;
coord.latitude = qcoord.latitude();
coord.longitude = qcoord.longitude();
lineString.coordinates.push_back(coord);
}
params.geometry = Geometry(lineString);
params.authorization = login_token.toStdString();
_flight.coords.clear();
_shared.client()->flight_plans().create_by_polygon(params, [this, isAlive](const FlightPlans::Create::Result& result) {
if (!isAlive.lock()) return;
if (_state != State::FlightUpload) return;
if (result) {
_pendingFlightPlan = QString::fromStdString(result.value().id);
qCDebug(AirMapManagerLog) << "Flight Plan created:"<<_pendingFlightPlan;
_checkForValidBriefing();
} else {
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Flight Plan creation failed",
QString::fromStdString(result.error().message()), description);
}
});
});
}
void AirMapFlightManager::_checkForValidBriefing()
{
_state = State::FlightBrief;
FlightPlans::RenderBriefing::Parameters params;
params.authorization = _shared.loginToken().toStdString();
params.id = _pendingFlightPlan.toStdString();
std::weak_ptr<LifetimeChecker> isAlive(_instance);
_shared.client()->flight_plans().render_briefing(params, [this, isAlive](const FlightPlans::RenderBriefing::Result& result) {
if (!isAlive.lock()) return;
if (_state != State::FlightBrief) return;
if (result) {
bool allValid = true;
for (const auto& validation : result.value().evaluation.validations) {
if (validation.status != Evaluation::Validation::Status::valid) {
emit error(QString("%1 registration identifier is invalid: %2").arg(
QString::fromStdString(validation.authority.name)).arg(QString::fromStdString(validation.message)), "", "");
allValid = false;
}
}
if (allValid) {
_submitPendingFlightPlan();
} else {
_flightPermitStatus = AirspaceAuthorization::PermitRejected;
emit flightPermitStatusChanged();
_state = State::Idle;
}
} else {
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Brief Request failed",
QString::fromStdString(result.error().message()), description);
_state = State::Idle;
}
});
}
void AirMapFlightManager::_submitPendingFlightPlan()
{
_state = State::FlightSubmit;
FlightPlans::Submit::Parameters params;
params.authorization = _shared.loginToken().toStdString();
params.id = _pendingFlightPlan.toStdString();
std::weak_ptr<LifetimeChecker> isAlive(_instance);
_shared.client()->flight_plans().submit(params, [this, isAlive](const FlightPlans::Submit::Result& result) {
if (!isAlive.lock()) return;
if (_state != State::FlightSubmit) return;
if (result) {
_pendingFlightId = QString::fromStdString(result.value().flight_id.get());
_state = State::FlightPolling;
_pollBriefing();
} else {
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Failed to submit Flight Plan",
QString::fromStdString(result.error().message()), description);
_state = State::Idle;
}
});
}
void AirMapFlightManager::_pollBriefing()
{
if (_state != State::FlightPolling) {
qCWarning(AirMapManagerLog) << "AirMapFlightManager::_pollBriefing: not in polling state";
return;
}
FlightPlans::RenderBriefing::Parameters params;
params.authorization = _shared.loginToken().toStdString();
params.id = _pendingFlightPlan.toStdString();
std::weak_ptr<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 = AirspaceAuthorization::PermitRejected;
} else {
_flightPermitStatus = AirspaceAuthorization::PermitAccepted;
}
_currentFlightId = _pendingFlightId;
_pendingFlightPlan = "";
emit flightPermitStatusChanged();
_state = State::Idle;
} else {
// wait until we send the next polling request
_pollTimer.setSingleShot(true);
_pollTimer.start(2000);
}
} else {
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Brief Request failed",
QString::fromStdString(result.error().message()), description);
_state = State::Idle;
}
});
}
void AirMapFlightManager::endFlight()
{
if (_currentFlightId.length() == 0) {
return;
}
if (_state != State::Idle) {
qCWarning(AirMapManagerLog) << "AirMapFlightManager::endFlight: State not idle";
return;
}
_endFlight(_currentFlightId);
_flightPermitStatus = AirspaceAuthorization::PermitUnknown;
emit flightPermitStatusChanged();
}
void AirMapFlightManager::_endFlight(const QString& flightID)
{
qCDebug(AirMapManagerLog) << "ending flight" << flightID;
_state = State::FlightEnd;
Q_ASSERT(_shared.loginToken() != ""); // Since we have a flight ID, we need to be logged in
Flights::EndFlight::Parameters params;
params.authorization = _shared.loginToken().toStdString();
params.id = flightID.toStdString();
std::weak_ptr<LifetimeChecker> isAlive(_instance);
_shared.client()->flights().end_flight(params, [this, isAlive](const Flights::EndFlight::Result& result) {
if (!isAlive.lock()) return;
if (_state != State::FlightEnd) return;
_state = State::Idle;
_pendingFlightId = "";
_pendingFlightPlan = "";
_currentFlightId = "";
if (result) {
if (!_flight.coords.empty()) {
_uploadFlight();
}
} else {
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Failed to end Flight",
QString::fromStdString(result.error().message()), description);
}
});
}
/****************************************************************************
*
* (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 <QObject>
#include <QTimer>
#include "AirMapManager.h"
//-----------------------------------------------------------------------------
/// class to upload a flight
class AirMapFlightManager : public QObject, public LifetimeChecker
{
Q_OBJECT
public:
AirMapFlightManager(AirMapSharedState& shared);
/// Send flight path to AirMap
void createFlight(const QList<MissionItem*>& missionItems);
AirspaceAuthorization::PermitStatus flightPermitStatus() const { return _flightPermitStatus; }
const QString& flightID() const { return _currentFlightId; }
public slots:
void endFlight();
signals:
void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
void flightPermitStatusChanged();
private slots:
void _pollBriefing();
private:
/**
* upload flight stored in _flight
*/
void _uploadFlight();
/**
* query the active flights and end the first one (because only a single flight can be active at a time).
*/
void _endFirstFlight();
/**
* implementation of endFlight()
*/
void _endFlight(const QString& flightID);
/**
* check if the briefing response is valid and call _submitPendingFlightPlan() if it is.
*/
void _checkForValidBriefing();
void _submitPendingFlightPlan();
enum class State {
Idle,
GetPilotID,
FlightUpload,
FlightBrief,
FlightSubmit,
FlightPolling, // poll & check for approval
FlightEnd,
EndFirstFlight, // get a list of open flights & end the first one (because there can only be 1 active at a time)
};
struct Flight {
QList<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;
QString _currentFlightId; ///< Flight ID, empty if there is none
QString _pendingFlightId; ///< current flight ID, not necessarily accepted yet (once accepted, it's equal to _currentFlightId)
QString _pendingFlightPlan; ///< current flight plan, waiting to be submitted
AirspaceAuthorization::PermitStatus _flightPermitStatus = AirspaceAuthorization::PermitUnknown;
QString _pilotID; ///< Pilot ID in the form "auth0|abc123"
bool _noFlightCreatedYet = true;
QTimer _pollTimer; ///< timer to poll for approval check
};
This diff is collapsed.
......@@ -11,27 +11,16 @@
#include "QGCToolbox.h"
#include "QGCLoggingCategory.h"
#include "QmlObjectListModel.h"
#include "MissionItem.h"
#include "MultiVehicleManager.h"
#include "AirspaceManagement.h"
#include <QGeoCoordinate>
#include <QList>
#include <QQueue>
#include <QTimer>
#include <cstdint>
#include <functional>
#include <memory>
#include <airmap/qt/client.h>
#include <airmap/qt/logger.h>
#include <airmap/qt/types.h>
#include <airmap/traffic.h>
Q_DECLARE_LOGGING_CATEGORY(AirMapManagerLog)
class AirMapSharedState;
//-----------------------------------------------------------------------------
/**
* @class LifetimeChecker
* Base class which helps to check if an object instance still exists.
......@@ -48,311 +37,11 @@ protected:
std::shared_ptr<LifetimeChecker> _instance;
};
//-----------------------------------------------------------------------------
/**
* @class AirMapSharedState
* contains state & settings that need to be shared (such as login)
* @class AirMapManager
* AirMap implementation of AirspaceManager
*/
class AirMapSharedState : public QObject
{
Q_OBJECT
public:
struct Settings {
QString apiKey;
// login credentials
QString clientID;
QString userName; ///< use anonymous login if empty
QString password;
};
void setSettings(const Settings& settings);
const Settings& settings() const { return _settings; }
void setClient(airmap::qt::Client* client) { _client = client; }
/**
* Get the current client instance. It can be NULL. If not NULL, it implies
* there's an API key set.
*/
airmap::qt::Client* client() const { return _client; }
bool hasAPIKey() const { return _settings.apiKey != ""; }
bool isLoggedIn() const { return _loginToken != ""; }
using Callback = std::function<void(const QString& /* login_token */)>;
/**
* Do a request that requires user login: if not yet logged in, the request is queued and
* processed after successful login, otherwise it's executed directly.
*/
void doRequestWithLogin(const Callback& callback);
void login();
void logout();
const QString& loginToken() const { return _loginToken; }
signals:
void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
private:
void _processPendingRequests();
bool _isLoginInProgress = false;
QString _loginToken; ///< login token: empty when not logged in
airmap::qt::Client* _client = nullptr;
Settings _settings;
QQueue<Callback> _pendingRequests; ///< pending requests that are processed after a successful login
};
/// class to download polygons from AirMap
class AirMapRestrictionManager : public AirspaceRestrictionProvider, public LifetimeChecker
{
Q_OBJECT
public:
AirMapRestrictionManager(AirMapSharedState& shared);
void setROI(const QGeoCoordinate& center, double radiusMeters) override;
signals:
void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
private:
static void _addPolygonToList(const airmap::Geometry::Polygon& polygon, QList<PolygonAirspaceRestriction*>& list);
enum class State {
Idle,
RetrieveItems,
};
State _state = State::Idle;
AirMapSharedState& _shared;
};
/// class to download rulesets from AirMap
class AirMapRulesetsManager : public AirspaceRulesetsProvider, public LifetimeChecker
{
Q_OBJECT
public:
AirMapRulesetsManager (AirMapSharedState& shared);
void setROI (const QGeoCoordinate& center) override;
signals:
void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
private:
enum class State {
Idle,
RetrieveItems,
};
State _state = State::Idle;
AirMapSharedState& _shared;
};
/// class to upload a flight
class AirMapFlightManager : public QObject, public LifetimeChecker
{
Q_OBJECT
public:
AirMapFlightManager(AirMapSharedState& shared);
/// Send flight path to AirMap
void createFlight(const QList<MissionItem*>& missionItems);
AirspaceAuthorization::PermitStatus flightPermitStatus() const { return _flightPermitStatus; }
const QString& flightID() const { return _currentFlightId; }
public slots:
void endFlight();
signals:
void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
void flightPermitStatusChanged();
private slots:
void _pollBriefing();
private:
/**
* upload flight stored in _flight
*/
void _uploadFlight();
/**
* query the active flights and end the first one (because only a single flight can be active at a time).
*/
void _endFirstFlight();
/**
* implementation of endFlight()
*/
void _endFlight(const QString& flightID);
/**
* check if the briefing response is valid and call _submitPendingFlightPlan() if it is.
*/
void _checkForValidBriefing();
void _submitPendingFlightPlan();
enum class State {
Idle,
GetPilotID,
FlightUpload,
FlightBrief,
FlightSubmit,
FlightPolling, // poll & check for approval
FlightEnd,
EndFirstFlight, // get a list of open flights & end the first one (because there can only be 1 active at a time)
};
struct Flight {
QList<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;
QString _currentFlightId; ///< Flight ID, empty if there is none
QString _pendingFlightId; ///< current flight ID, not necessarily accepted yet (once accepted, it's equal to _currentFlightId)
QString _pendingFlightPlan; ///< current flight plan, waiting to be submitted
AirspaceAuthorization::PermitStatus _flightPermitStatus = AirspaceAuthorization::PermitUnknown;
QString _pilotID; ///< Pilot ID in the form "auth0|abc123"
bool _noFlightCreatedYet = true;
QTimer _pollTimer; ///< timer to poll for approval check
};
/// class to send telemetry data to AirMap
class AirMapTelemetry : public QObject, public LifetimeChecker
{
Q_OBJECT
public:
AirMapTelemetry(AirMapSharedState& shared);
virtual ~AirMapTelemetry() = default;
/**
* Setup the connection to start sending telemetry
*/
void startTelemetryStream(const QString& flightID);
void stopTelemetryStream();
bool isTelemetryStreaming() const;
signals:
void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
public slots:
void vehicleMavlinkMessageReceived(const mavlink_message_t& message);
private:
void _handleGlobalPositionInt(const mavlink_message_t& message);
void _handleGPSRawInt(const mavlink_message_t& message);
enum class State {
Idle,
StartCommunication,
EndCommunication,
Streaming,
};
State _state = State::Idle;
AirMapSharedState& _shared;
std::string _key; ///< key for AES encryption (16 bytes)
QString _flightID;
float _lastHdop = 1.f;
};
class AirMapTrafficMonitor : public QObject, public LifetimeChecker
{
Q_OBJECT
public:
AirMapTrafficMonitor(AirMapSharedState& shared)
: _shared(shared)
{
}
virtual ~AirMapTrafficMonitor();
void startConnection(const QString& flightID);
void stop();
signals:
void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
void trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading);
private:
void _update(airmap::Traffic::Update::Type type, const std::vector<airmap::Traffic::Update>& update);
private:
QString _flightID;
AirMapSharedState& _shared;
std::shared_ptr<airmap::Traffic::Monitor> _monitor;
std::shared_ptr<airmap::Traffic::Monitor::Subscriber> _subscriber;
};
/// AirMap per vehicle management class.
class AirMapManagerPerVehicle : public AirspaceManagerPerVehicle
{
Q_OBJECT
public:
AirMapManagerPerVehicle(AirMapSharedState& shared, const Vehicle& vehicle, QGCToolbox& toolbox);
virtual ~AirMapManagerPerVehicle() = default;
void createFlight(const QList<MissionItem*>& missionItems) override;
AirspaceAuthorization::PermitStatus flightPermitStatus() const override;
void startTelemetryStream() override;
void stopTelemetryStream() override;
bool isTelemetryStreaming() const override;
signals:
void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
public slots:
void endFlight() override;
protected slots:
virtual void vehicleMavlinkMessageReceived(const mavlink_message_t& message) override;
private slots:
void _flightPermitStatusChanged();
private:
AirMapSharedState& _shared;
AirMapFlightManager _flightManager;
AirMapTelemetry _telemetry;
AirMapTrafficMonitor _trafficMonitor;
QGCToolbox& _toolbox;
};
class AirMapManager : public AirspaceManager
{
......@@ -362,26 +51,23 @@ public:
AirMapManager(QGCApplication* app, QGCToolbox* toolbox);
virtual ~AirMapManager();
void setToolbox(QGCToolbox* toolbox) override;
void setToolbox (QGCToolbox* toolbox) override;
AirspaceManagerPerVehicle* instantiateVehicle (const Vehicle& vehicle) override;
AirspaceRestrictionProvider* instantiateRestrictionProvider () override;
AirspaceRulesetsProvider* instantiateRulesetsProvider () override;
AirspaceVehicleManager* instantiateVehicle (const Vehicle& vehicle) override;
AirspaceRestrictionProvider* instantiateRestrictionProvider () override;
AirspaceRulesetsProvider* instantiateRulesetsProvider () override;
AirspaceWeatherInfoProvider* instatiateAirspaceWeatherInfoProvider () override;
QString name() const override { return "AirMap"; }
void requestWeatherUpdate(const QGeoCoordinate& coordinate) override;
QString name () const override { return "AirMap"; }
private slots:
void _error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
void _error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
void _settingsChanged ();
void _settingsChanged();
private:
AirMapSharedState _shared;
std::shared_ptr<airmap::qt::Logger> _logger;
std::shared_ptr<airmap::qt::DispatchingLogger> _dispatchingLogger;
AirMapSharedState _shared;
std::shared_ptr<airmap::qt::Logger> _logger;
std::shared_ptr<airmap::qt::DispatchingLogger> _dispatchingLogger;
};
#include "AirMapRestrictionManager.h"
AirMapRestrictionManager::AirMapRestrictionManager(AirMapSharedState& shared)
: _shared(shared)
{
}
void
AirMapRestrictionManager::setROI(const QGeoCoordinate& center, double radiusMeters)
{
if (!_shared.client()) {
qCDebug(AirMapManagerLog) << "No AirMap client instance. Not updating Airspace";
return;
}
if (_state != State::Idle) {
qCWarning(AirMapManagerLog) << "AirMapRestrictionManager::updateROI: state not idle";
return;
}
qCDebug(AirMapManagerLog) << "setting ROI";
_polygonList.clear();
_circleList.clear();
_state = State::RetrieveItems;
Airspaces::Search::Parameters params;
params.geometry = Geometry::point(center.latitude(), center.longitude());
params.buffer = radiusMeters;
params.full = true;
std::weak_ptr<LifetimeChecker> isAlive(_instance);
_shared.client()->airspaces().search(params,
[this, isAlive](const Airspaces::Search::Result& result) {
if (!isAlive.lock()) return;
if (_state != State::RetrieveItems) return;
if (result) {
const std::vector<Airspace>& airspaces = result.value();
qCDebug(AirMapManagerLog)<<"Successful search. Items:" << airspaces.size();
for (const auto& airspace : airspaces) {
const Geometry& geometry = airspace.geometry();
switch(geometry.type()) {
case Geometry::Type::polygon: {
const Geometry::Polygon& polygon = geometry.details_for_polygon();
_addPolygonToList(polygon, _polygonList);
}
break;
case Geometry::Type::multi_polygon: {
const Geometry::MultiPolygon& multiPolygon = geometry.details_for_multi_polygon();
for (const auto& polygon : multiPolygon) {
_addPolygonToList(polygon, _polygonList);
}
}
break;
case Geometry::Type::point: {
const Geometry::Point& point = geometry.details_for_point();
_circleList.append(new AirspaceCircularRestriction(QGeoCoordinate(point.latitude, point.longitude), 0.));
// TODO: radius???
}
break;
default:
qWarning() << "unsupported geometry type: "<<(int)geometry.type();
break;
}
}
} else {
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Failed to retrieve Geofences",
QString::fromStdString(result.error().message()), description);
}
emit requestDone(true);
_state = State::Idle;
});
}
void
AirMapRestrictionManager::_addPolygonToList(const airmap::Geometry::Polygon& polygon, QList<AirspacePolygonRestriction*>& list)
{
QVariantList polygonArray;
for (const auto& vertex : polygon.outer_ring.coordinates) {
QGeoCoordinate coord;
if (vertex.altitude) {
coord = QGeoCoordinate(vertex.latitude, vertex.longitude, vertex.altitude.get());
} else {
coord = QGeoCoordinate(vertex.latitude, vertex.longitude);
}
polygonArray.append(QVariant::fromValue(coord));
}
list.append(new AirspacePolygonRestriction(polygonArray));
if (polygon.inner_rings.size() > 0) {
// no need to support those (they are rare, and in most cases, there's a more restrictive polygon filling the hole)
qCDebug(AirMapManagerLog) << "Polygon with holes. Size: "<<polygon.inner_rings.size();
}
}
/****************************************************************************
*
* (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 <QObject>
#include "AirspaceRestrictionProvider.h"
/**
* @file AirMapRestrictionManager.h
* Class to download polygons from AirMap
*/
class AirMapRestrictionManager : public AirspaceRestrictionProvider, public LifetimeChecker
{
Q_OBJECT
public:
AirMapRestrictionManager (AirMapSharedState& shared);
void setROI (const QGeoCoordinate& center, double radiusMeters) override;
signals:
void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
private:
static void _addPolygonToList (const airmap::Geometry::Polygon& polygon, QList<AirspacePolygonRestriction*>& list);
enum class State {
Idle,
RetrieveItems,
};
State _state = State::Idle;
AirMapSharedState& _shared;
};
#include "AirMapRulesetsManager.h"
//-----------------------------------------------------------------------------
AirMapRulesetsManager::AirMapRulesetsManager(AirMapSharedState& shared)
: _shared(shared)
{
}
//-----------------------------------------------------------------------------
void AirMapRulesetsManager::setROI(const QGeoCoordinate& center)
{
if (!_shared.client()) {
qCDebug(AirMapManagerLog) << "No AirMap client instance. Not updating Airspace";
return;
}
if (_state != State::Idle) {
qCWarning(AirMapManagerLog) << "AirMapRestrictionManager::updateROI: state not idle";
return;
}
qCDebug(AirMapManagerLog) << "Setting ROI for Rulesets";
_state = State::RetrieveItems;
RuleSets::Search::Parameters params;
params.geometry = Geometry::point(center.latitude(), center.longitude());
std::weak_ptr<LifetimeChecker> isAlive(_instance);
_shared.client()->rulesets().search(params,
[this, isAlive](const RuleSets::Search::Result& result) {
if (!isAlive.lock()) return;
if (_state != State::RetrieveItems) return;
if (result) {
const std::vector<RuleSet>& rulesets = result.value();
qCDebug(AirMapManagerLog)<<"Successful rulesets search. Items:" << rulesets.size();
for (const auto& ruleset : rulesets) {
qDebug() << "------------------------------------------";
qDebug() << "Jurisdiction:" << ruleset.jurisdiction.name.data() << (int)ruleset.jurisdiction.region;
qDebug() << "Name: " << ruleset.name.data();
qDebug() << "Short Name: " << ruleset.short_name.data();
qDebug() << "Description: " << ruleset.description.data();
qDebug() << "Is default: " << ruleset.is_default;
qDebug() << "Applicable to these airspace types:";
for (const auto& airspaceType : ruleset.airspace_types) {
qDebug() << airspaceType.data();
}
qDebug() << "Rules:";
for (const auto& rule : ruleset.rules) {
qDebug() << " --------------------------------------";
qDebug() << " " << rule.short_text.data();
qDebug() << " " << rule.description.data();
qDebug() << " " << rule.display_order;
qDebug() << " " << (int)rule.status;
qDebug() << " Features:";
for (const auto& feature : rule.features) {
qDebug() << " " << feature.name.data();
qDebug() << " " << feature.description.data();
qDebug() << " " << (int)feature.status;
}
}
}
} else {
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Failed to retrieve RuleSets",
QString::fromStdString(result.error().message()), description);
}
emit requestDone(true);
_state = State::Idle;
});
}
/****************************************************************************
*
* (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 AirMapRulesetsManager.h
* Class to download rulesets from AirMap
*/
#include <QObject>
#include "AirMapManager.h"
#include "AirspaceRulesetsProvider.h"
#include "AirMapSharedState.h"
class AirMapRulesetsManager : public AirspaceRulesetsProvider, public LifetimeChecker
{
Q_OBJECT
public:
AirMapRulesetsManager (AirMapSharedState& shared);
void setROI (const QGeoCoordinate& center) override;
signals:
void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
private:
enum class State {
Idle,
RetrieveItems,
};
State _state = State::Idle;
AirMapSharedState& _shared;
};
#include "AirMapSharedState.h"
void
AirMapSharedState::setSettings(const Settings& settings)
{
logout();
_settings = settings;
}
void
AirMapSharedState::doRequestWithLogin(const Callback& callback)
{
if (isLoggedIn()) {
callback(_loginToken);
} else {
login();
_pendingRequests.enqueue(callback);
}
}
void
AirMapSharedState::login()
{
if (isLoggedIn() || _isLoginInProgress) {
return;
}
_isLoginInProgress = true;
if (_settings.userName == "") { //use anonymous login
Authenticator::AuthenticateAnonymously::Params params;
params.id = "";
_client->authenticator().authenticate_anonymously(params,
[this](const Authenticator::AuthenticateAnonymously::Result& result) {
if (!_isLoginInProgress) { // was logout() called in the meanwhile?
return;
}
if (result) {
qCDebug(AirMapManagerLog)<<"Successfully authenticated with AirMap: id="<< result.value().id.c_str();
_loginToken = QString::fromStdString(result.value().id);
_processPendingRequests();
} else {
_pendingRequests.clear();
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Failed to authenticate with AirMap",
QString::fromStdString(result.error().message()), description);
}
});
} else {
Authenticator::AuthenticateWithPassword::Params params;
params.oauth.username = _settings.userName.toStdString();
params.oauth.password = _settings.password.toStdString();
params.oauth.client_id = _settings.clientID.toStdString();
params.oauth.device_id = "QGroundControl";
_client->authenticator().authenticate_with_password(params,
[this](const Authenticator::AuthenticateWithPassword::Result& result) {
if (!_isLoginInProgress) { // was logout() called in the meanwhile?
return;
}
if (result) {
qCDebug(AirMapManagerLog)<<"Successfully authenticated with AirMap: id="<< result.value().id.c_str()<<", access="
<<result.value().access.c_str();
_loginToken = QString::fromStdString(result.value().id);
_processPendingRequests();
} else {
_pendingRequests.clear();
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Failed to authenticate with AirMap",
QString::fromStdString(result.error().message()), description);
}
});
}
}
void
AirMapSharedState::_processPendingRequests()
{
while (!_pendingRequests.isEmpty()) {
_pendingRequests.dequeue()(_loginToken);
}
}
void
AirMapSharedState::logout()
{
_isLoginInProgress = false; // cancel if we're currently trying to login
if (!isLoggedIn()) {
return;
}
_loginToken = "";
_pendingRequests.clear();
}
/****************************************************************************
*
* (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 <QObject>
#include <airmap/qt/client.h>
/**
* @class AirMapSharedState
* Contains state & settings that need to be shared (such as login)
*/
class AirMapSharedState : public QObject
{
Q_OBJECT
public:
struct Settings {
QString apiKey;
// login credentials
QString clientID;
QString userName; ///< use anonymous login if empty
QString password;
};
void setSettings (const Settings& settings);
const Settings& settings () const { return _settings; }
void setClient (airmap::qt::Client* client) { _client = client; }
/**
* Get the current client instance. It can be NULL. If not NULL, it implies
* there's an API key set.
*/
airmap::qt::Client* client () const { return _client; }
bool hasAPIKey () const { return _settings.apiKey != ""; }
bool isLoggedIn () const { return _loginToken != ""; }
using Callback = std::function<void(const QString& /* login_token */)>;
/**
* Do a request that requires user login: if not yet logged in, the request is queued and
* processed after successful login, otherwise it's executed directly.
*/
void doRequestWithLogin (const Callback& callback);
void login ();
void logout ();
const QString& loginToken () const { return _loginToken; }
signals:
void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
private:
void _processPendingRequests ();
private:
bool _isLoginInProgress = false;
QString _loginToken; ///< login token: empty when not logged in
airmap::qt::Client* _client = nullptr;
Settings _settings;
QQueue<Callback> _pendingRequests; ///< pending requests that are processed after a successful login
};
/****************************************************************************
*
* (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 "AirMapTelemetry.h"
AirMapTelemetry::AirMapTelemetry(AirMapSharedState& shared)
: _shared(shared)
{
}
void AirMapTelemetry::vehicleMavlinkMessageReceived(const mavlink_message_t& message)
{
switch (message.msgid) {
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
_handleGlobalPositionInt(message);
break;
case MAVLINK_MSG_ID_GPS_RAW_INT:
_handleGPSRawInt(message);
break;
}
}
bool AirMapTelemetry::isTelemetryStreaming() const
{
return _state == State::Streaming;
}
void AirMapTelemetry::_handleGPSRawInt(const mavlink_message_t& message)
{
if (!isTelemetryStreaming()) {
return;
}
mavlink_gps_raw_int_t gps_raw;
mavlink_msg_gps_raw_int_decode(&message, &gps_raw);
if (gps_raw.eph == UINT16_MAX) {
_lastHdop = 1.f;
} else {
_lastHdop = gps_raw.eph / 100.f;
}
}
void AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message)
{
if (!isTelemetryStreaming()) {
return;
}
mavlink_global_position_int_t globalPosition;
mavlink_msg_global_position_int_decode(&message, &globalPosition);
Telemetry::Position position{
milliseconds_since_epoch(Clock::universal_time()),
(double) globalPosition.lat / 1e7,
(double) globalPosition.lon / 1e7,
(float) globalPosition.alt / 1000.f,
(float) globalPosition.relative_alt / 1000.f,
_lastHdop
};
Telemetry::Speed speed{
milliseconds_since_epoch(Clock::universal_time()),
globalPosition.vx / 100.f,
globalPosition.vy / 100.f,
globalPosition.vz / 100.f
};
Flight flight;
flight.id = _flightID.toStdString();
_shared.client()->telemetry().submit_updates(flight, _key,
{Telemetry::Update{position}, Telemetry::Update{speed}});
}
void AirMapTelemetry::startTelemetryStream(const QString& flightID)
{
if (_state != State::Idle) {
qCWarning(AirMapManagerLog) << "Not starting telemetry: not in idle state:" << (int)_state;
return;
}
qCInfo(AirMapManagerLog) << "Starting Telemetry stream with flightID" << flightID;
_state = State::StartCommunication;
_flightID = flightID;
Flights::StartFlightCommunications::Parameters params;
params.authorization = _shared.loginToken().toStdString();
params.id = _flightID.toStdString();
std::weak_ptr<LifetimeChecker> isAlive(_instance);
_shared.client()->flights().start_flight_communications(params, [this, isAlive](const Flights::StartFlightCommunications::Result& result) {
if (!isAlive.lock()) return;
if (_state != State::StartCommunication) return;
if (result) {
_key = result.value().key;
_state = State::Streaming;
} else {
_state = State::Idle;
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Failed to start telemetry streaming",
QString::fromStdString(result.error().message()), description);
}
});
}
void AirMapTelemetry::stopTelemetryStream()
{
if (_state == State::Idle) {
return;
}
qCInfo(AirMapManagerLog) << "Stopping Telemetry stream with flightID" << _flightID;
_state = State::EndCommunication;
Flights::EndFlightCommunications::Parameters params;
params.authorization = _shared.loginToken().toStdString();
params.id = _flightID.toStdString();
std::weak_ptr<LifetimeChecker> isAlive(_instance);
_shared.client()->flights().end_flight_communications(params, [this, isAlive](const Flights::EndFlightCommunications::Result& result) {
Q_UNUSED(result);
if (!isAlive.lock()) return;
if (_state != State::EndCommunication) return;
_key = "";
_state = State::Idle;
});
}
/****************************************************************************
*
* (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.
*
****************************************************************************/
#pragma once
#include <QObject>
#include "AirMapManager.h"
/// class to send telemetry data to AirMap
class AirMapTelemetry : public QObject, public LifetimeChecker
{
Q_OBJECT
public:
AirMapTelemetry(AirMapSharedState& shared);
virtual ~AirMapTelemetry() = default;
/**
* Setup the connection to start sending telemetry
*/
void startTelemetryStream(const QString& flightID);
void stopTelemetryStream();
bool isTelemetryStreaming() const;
signals:
void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
public slots:
void vehicleMavlinkMessageReceived(const mavlink_message_t& message);
private:
void _handleGlobalPositionInt(const mavlink_message_t& message);
void _handleGPSRawInt(const mavlink_message_t& message);
enum class State {
Idle,
StartCommunication,
EndCommunication,
Streaming,
};
State _state = State::Idle;
AirMapSharedState& _shared;
std::string _key; ///< key for AES encryption (16 bytes)
QString _flightID;
float _lastHdop = 1.f;
};
#include "AirMapTrafficMonitor.h"
AirMapTrafficMonitor::~AirMapTrafficMonitor()
{
stop();
}
void
AirMapTrafficMonitor::startConnection(const QString& flightID)
{
_flightID = flightID;
std::weak_ptr<LifetimeChecker> isAlive(_instance);
auto handler = [this, isAlive](const Traffic::Monitor::Result& result) {
if (!isAlive.lock()) return;
if (result) {
_monitor = result.value();
_subscriber = std::make_shared<Traffic::Monitor::FunctionalSubscriber>(
std::bind(&AirMapTrafficMonitor::_update, this, std::placeholders::_1, std::placeholders::_2));
_monitor->subscribe(_subscriber);
} else {
QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
emit error("Failed to start Traffic Monitoring",
QString::fromStdString(result.error().message()), description);
}
};
Traffic::Monitor::Params params{flightID.toStdString(), _shared.loginToken().toStdString()};
_shared.client()->traffic().monitor(params, handler);
}
void
AirMapTrafficMonitor::_update(Traffic::Update::Type type, const std::vector<Traffic::Update>& update)
{
qCDebug(AirMapManagerLog) << "Traffic update with" << update.size() << "elements";
if (type != Traffic::Update::Type::situational_awareness)
return; // currently we're only interested in situational awareness
for (const auto& traffic : update) {
QString traffic_id = QString::fromStdString(traffic.id);
QString vehicle_id = QString::fromStdString(traffic.aircraft_id);
emit trafficUpdate(traffic_id, vehicle_id, QGeoCoordinate(traffic.latitude, traffic.longitude, traffic.altitude),
traffic.heading);
}
}
void
AirMapTrafficMonitor::stop()
{
if (_monitor) {
_monitor->unsubscribe(_subscriber);
_subscriber.reset();
_monitor.reset();
}
}
/****************************************************************************
*
* (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 <QObject>
/**
* @class AirMapTrafficMonitor
*
*/
class AirMapTrafficMonitor : public QObject, public LifetimeChecker
{
Q_OBJECT
public:
AirMapTrafficMonitor(AirMapSharedState& shared)
: _shared(shared)
{
}
virtual ~AirMapTrafficMonitor();
void startConnection(const QString& flightID);
void stop();
signals:
void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
void trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading);
private:
void _update(airmap::Traffic::Update::Type type, const std::vector<airmap::Traffic::Update>& update);
private:
QString _flightID;
AirMapSharedState& _shared;
std::shared_ptr<airmap::Traffic::Monitor> _monitor;
std::shared_ptr<airmap::Traffic::Monitor::Subscriber> _subscriber;
};
#include "AirMapVehicleManager.h"
#include "AirMapFlightManager.h"
#include "AirMapTelemetry.h"
#include "AirMapTrafficMonitor.h"
AirMapVehicleManager::AirMapVehicleManager(AirMapSharedState& shared, const Vehicle& vehicle, QGCToolbox& toolbox)
: 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<MissionItem*>& missionItems)
{
if (!_shared.client()) {
qCDebug(AirMapManagerLog) << "No AirMap client instance. Will not create a flight";
return;
}
_flightManager.createFlight(missionItems);
}
AirspaceAuthorization::PermitStatus
AirMapVehicleManager::flightPermitStatus() const
{
return _flightManager.flightPermitStatus();
}
void
AirMapVehicleManager::startTelemetryStream()
{
if (_flightManager.flightID() != "") {
_telemetry.startTelemetryStream(_flightManager.flightID());
}
}
void
AirMapVehicleManager::stopTelemetryStream()
{
_telemetry.stopTelemetryStream();
}
bool
AirMapVehicleManager::isTelemetryStreaming() const
{
return _telemetry.isTelemetryStreaming();
}
void
AirMapVehicleManager::endFlight()
{
_flightManager.endFlight();
_trafficMonitor.stop();
}
void
AirMapVehicleManager::vehicleMavlinkMessageReceived(const mavlink_message_t& message)
{
if (isTelemetryStreaming()) {
_telemetry.vehicleMavlinkMessageReceived(message);
}
}
void
AirMapVehicleManager::_flightPermitStatusChanged()
{
// activate traffic alerts
if (_flightManager.flightPermitStatus() == AirspaceAuthorization::PermitAccepted) {
qCDebug(AirMapManagerLog) << "Subscribing to Traffic Alerts";
// since we already created the flight, we know that we have a valid login token
_trafficMonitor.startConnection(_flightManager.flightID());
}
}
/****************************************************************************
*
* (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 <QObject>
#include "AirspaceManagement.h"
#include "AirspaceVehicleManager.h"
/// AirMap per vehicle management class.
class AirMapSharedState;
class AirMapFlightManager;
class AirMapTelemetry;
class AirMapTrafficMonitor;
class AirMapVehicleManager : public AirspaceVehicleManager
{
Q_OBJECT
public:
AirMapVehicleManager (AirMapSharedState& shared, const Vehicle& vehicle, QGCToolbox& toolbox);
~AirMapVehicleManager () = default;
void createFlight (const QList<MissionItem*>& missionItems) override;
void startTelemetryStream () override;
void stopTelemetryStream () override;
bool isTelemetryStreaming () const override;
AirspaceAuthorization::PermitStatus flightPermitStatus() const override;
signals:
void error (const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
public slots:
void endFlight () override;
protected slots:
virtual void vehicleMavlinkMessageReceived(const mavlink_message_t& message) override;
private slots:
void _flightPermitStatusChanged();
private:
AirMapSharedState& _shared;
AirMapFlightManager _flightManager;
AirMapTelemetry _telemetry;
AirMapTrafficMonitor _trafficMonitor;
QGCToolbox& _toolbox;
};
#include "AirmapWeatherInformation.h"
AirMapWeatherInformation::AirMapWeatherInformation(AirMapSharedState& shared, QObject *parent)
: QObject(parent)
, _valid(false)
, _windHeading(0)
, _windSpeed(0)
, _windGusting(0)
, _temperature(0)
, _humidity(0.0f)
, _visibility(0)
, _precipitation(0)
{
}
void
AirMapWeatherInformation::setROI(QGeoCoordinate center)
{
//-- If first time or we've moved more than WEATHER_UPDATE_DISTANCE, ask for weather updates.
if(!_lastRoiCenter.isValid() || _lastRoiCenter.distanceTo(center) > WEATHER_UPDATE_DISTANCE) {
_lastRoiCenter = center;
_requestWeatherUpdate(center);
_weatherTime.start();
} else {
//-- Check weather once every WEATHER_UPDATE_TIME
if(_weatherTime.elapsed() > WEATHER_UPDATE_TIME) {
_requestWeatherUpdate(center);
_weatherTime.start();
}
}
}
void
AirMapWeatherInformation::_requestWeatherUpdate(const QGeoCoordinate& coordinate)
{
if (!_shared.client()) {
qCDebug(AirMapManagerLog) << "No AirMap client instance. Not updating Weather information";
_valid = false;
emit weatherChanged();
return;
}
Status::GetStatus::Parameters params;
params.longitude = coordinate.longitude();
params.latitude = coordinate.latitude();
params.weather = true;
_shared.client()->status().get_status_by_point(params, [this, coordinate](const Status::GetStatus::Result& result) {
if (result) {
const Status::Weather& weather = result.value().weather;
AirMapWeatherInformation weatherUpdateInfo;
_valid = true;
_condition = QString::fromStdString(weather.condition);
_icon = QStringLiteral("qrc:/airmapweather/") + QString::fromStdString(weather.icon) + QStringLiteral(".svg");
_windHeading = weather.wind.heading;
_windSpeed = weather.wind.speed;
_windGusting = weather.wind.gusting;
_temperature = weather.temperature;
_humidity = weather.humidity;
_visibility = weather.visibility;
_precipitation = weather.precipitation;
} else {
_valid = false;
}
emit weatherChanged();
});
}
/****************************************************************************
*
* (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 <QObject>
#include <QGeoCoordinate>
#include "AirspaceWeatherInfoProvider.h"
/**
* @file AirMapWeatherInformation.h
* Weather information provided by AirMap.
*/
class AirMapWeatherInformation : public AirspaceWeatherInfoProvider, public LifetimeChecker
{
Q_OBJECT
friend class AirMapManager;
public:
AirMapWeatherInformation(AirMapSharedState &shared, QObject *parent = nullptr);
bool valid () override { return _valid; }
QString condition () override { return _condition; }
QString icon () override { return _icon; }
quint32 windHeading () override { return _windHeading; }
quint32 windSpeed () override { return _windSpeed; }
quint32 windGusting () override { return _windGusting; }
qint32 temperature () override { return _temperature; }
float humidity () override { return _humidity; }
quint32 visibility () override { return _visibility; }
quint32 precipitation () override { return _precipitation; }
void setROI (const QGeoCoordinate& center) override;
private:
void _requestWeatherUpdate (const QGeoCoordinate& coordinate);
private:
bool _valid;
QString _condition;
QString _icon;
quint32 _windHeading;
quint32 _windSpeed;
quint32 _windGusting;
qint32 _temperature;
float _humidity;
quint32 _visibility;
quint32 _precipitation;
//-- Don't check the weather every time the user moves the map
QGeoCoordinate _lastRoiCenter;
QTime _weatherTime;
};
......@@ -19,8 +19,6 @@
AirspaceController::AirspaceController(QObject* parent)
: QObject(parent)
, _manager(qgcApp()->toolbox()->airspaceManager())
, _weatherTemp(0)
, _hasWeather(false)
{
connect(_manager, &AirspaceManager::weatherUpdate, this, &AirspaceController::_weatherUpdate);
}
......@@ -28,25 +26,4 @@ AirspaceController::AirspaceController(QObject* parent)
void AirspaceController::setROI(QGeoCoordinate center, double radius)
{
_manager->setROI(center, radius);
//-- If first time or we've moved more than WEATHER_UPDATE_DISTANCE, ask for weather updates.
if(!_lastRoiCenter.isValid() || _lastRoiCenter.distanceTo(center) > WEATHER_UPDATE_DISTANCE) {
_lastRoiCenter = center;
_manager->requestWeatherUpdate(center);
_weatherTime.start();
} else {
//-- Check weather once every WEATHER_UPDATE_TIME
if(_weatherTime.elapsed() > WEATHER_UPDATE_TIME) {
_manager->requestWeatherUpdate(center);
_weatherTime.start();
}
}
}
void AirspaceController::_weatherUpdate(bool success, QGeoCoordinate, WeatherInformation weather)
{
qCDebug(AirMapManagerLog)<<"Weather Info:"<< success << weather.condition << weather.temperature;
_hasWeather = success;
_weatherIcon = QStringLiteral("qrc:/airmapweather/") + weather.condition + QStringLiteral(".svg");
_weatherTemp = weather.temperature;
emit weatherChanged();
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment