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

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
......
This diff is collapsed.
/****************************************************************************
*
* (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.
This diff is collapsed.
#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);