Commit 44a05ab6 authored by Beat Küng's avatar Beat Küng

AirMapManager: change backend to use airmapd

You need to adjust the 'AIRMAPD_PATH = path_to_airmapd' in
qgroundcontrol.pro to make it compile.

And sorry for the huge commit :/
parent d17f10d2
...@@ -4,9 +4,3 @@ ...@@ -4,9 +4,3 @@
[submodule "libs/mavlink/include/mavlink/v2.0"] [submodule "libs/mavlink/include/mavlink/v2.0"]
path = libs/mavlink/include/mavlink/v2.0 path = libs/mavlink/include/mavlink/v2.0
url = https://github.com/mavlink/c_library_v2.git url = https://github.com/mavlink/c_library_v2.git
[submodule "libs/thirdParty/tiny-AES128-C"]
path = libs/thirdParty/tiny-AES128-C
url = https://github.com/bkueng/tiny-AES128-C.git
[submodule "libs/thirdParty/qmqtt"]
path = libs/thirdParty/qmqtt
url = https://github.com/emqtt/qmqtt.git
...@@ -131,10 +131,6 @@ WindowsBuild { ...@@ -131,10 +131,6 @@ WindowsBuild {
QMAKE_TARGET_PRODUCT = "$${QGC_APP_NAME}" QMAKE_TARGET_PRODUCT = "$${QGC_APP_NAME}"
} }
include($$PWD/libs/thirdParty/qmqtt/src/mqtt/mqtt.pri)
HEADERS += $$PUBLIC_HEADERS $$PRIVATE_HEADERS
# #
# Plugin configuration # Plugin configuration
# #
...@@ -592,7 +588,9 @@ HEADERS += \ ...@@ -592,7 +588,9 @@ HEADERS += \
src/uas/UASInterface.h \ src/uas/UASInterface.h \
src/uas/UASMessageHandler.h \ src/uas/UASMessageHandler.h \
src/AnalyzeView/LogDownloadController.h \ src/AnalyzeView/LogDownloadController.h \
libs/thirdParty/tiny-AES128-C/aes.h \
AIRMAPD_PATH = path_to_airmapd
# Protobuf (AirMap) # Protobuf (AirMap)
# This should be optional. As is, QGC now requires protobuf to be installed. # This should be optional. As is, QGC now requires protobuf to be installed.
...@@ -602,10 +600,19 @@ MacBuild { ...@@ -602,10 +600,19 @@ MacBuild {
LIBS += \ LIBS += \
-L/usr/local/opt/protobuf/lib -L/usr/local/opt/protobuf/lib
} }
LIBS += -lprotobuf LIBS += -lprotobuf
PROTOS = src/protobuf/airmap_telemetry.proto
include(src/protobuf/proto_compile.pri) # airmapd
INCLUDEPATH += $${AIRMAPD_PATH}/include
LIBS += -L$${AIRMAPD_PATH}/build/src/airmap -lairmap-qt \
-lairmap-mavlink \
-lairmap-client -lssl -lcrypto \
-L$${AIRMAPD_PATH}/build/vendor/fmt/fmt -lfmt \
-lboost_system -lboost_date_time -lboost_filesystem -lboost_program_options \
-lboost_unit_test_framework \
-L$${AIRMAPD_PATH}/build/vendor/xdg -lxdg \
-L$${AIRMAPD_PATH}/build/vendor/uri/src -lnetwork-uri \
AndroidBuild { AndroidBuild {
HEADERS += \ HEADERS += \
...@@ -797,7 +804,6 @@ SOURCES += \ ...@@ -797,7 +804,6 @@ SOURCES += \
src/uas/UAS.cc \ src/uas/UAS.cc \
src/uas/UASMessageHandler.cc \ src/uas/UASMessageHandler.cc \
src/AnalyzeView/LogDownloadController.cc \ src/AnalyzeView/LogDownloadController.cc \
libs/thirdParty/tiny-AES128-C/aes.c \
DebugBuild { DebugBuild {
SOURCES += \ SOURCES += \
...@@ -875,7 +881,6 @@ INCLUDEPATH += \ ...@@ -875,7 +881,6 @@ INCLUDEPATH += \
src/FirmwarePlugin \ src/FirmwarePlugin \
src/Vehicle \ src/Vehicle \
src/VehicleSetup \ src/VehicleSetup \
libs/thirdParty/tiny-AES128-C \
HEADERS+= \ HEADERS+= \
src/AutoPilotPlugins/AutoPilotPlugin.h \ src/AutoPilotPlugins/AutoPilotPlugin.h \
......
...@@ -17,290 +17,126 @@ ...@@ -17,290 +17,126 @@
#include "QGCQGeoCoordinate.h" #include "QGCQGeoCoordinate.h"
#include "QGCApplication.h" #include "QGCApplication.h"
#include <QNetworkAccessManager> #include <airmap/authenticator.h>
#include <QUrlQuery> #include <airmap/airspaces.h>
#include <QJsonDocument> #include <airmap/flight_plans.h>
#include <QJsonArray> #include <airmap/flights.h>
#include <QNetworkProxy> #include <airmap/pilots.h>
#include <QSet> #include <airmap/telemetry.h>
#include <QtEndian>
extern "C" {
#include <aes.h>
}
#include <airmap_telemetry.pb.h>
QGC_LOGGING_CATEGORY(AirMapManagerLog, "AirMapManagerLog") using namespace airmap;
QGC_LOGGING_CATEGORY(AirMapManagerLog, "AirMapManagerLog")
AirMapLogin::AirMapLogin(QNetworkAccessManager& networkManager, const QString& APIKey)
: _networkManager(networkManager), _APIKey(APIKey)
{
}
void AirMapLogin::setCredentials(const QString& clientID, const QString& userName, const QString& password) void AirMapSharedState::setSettings(const Settings& settings)
{ {
logout(); logout();
_clientID = clientID; _settings = settings;
_userName = userName;
_password = password;
}
void AirMapLogin::login()
{
if (isLoggedIn() || _isLoginInProgress) {
return;
}
_isLoginInProgress = true;
QUrlQuery postData;
postData.addQueryItem(QStringLiteral("grant_type"), "password");
postData.addQueryItem(QStringLiteral("client_id"), _clientID);
postData.addQueryItem(QStringLiteral("connection"), "Username-Password-Authentication");
postData.addQueryItem(QStringLiteral("username"), _userName);
postData.addQueryItem(QStringLiteral("password"), _password);
postData.addQueryItem(QStringLiteral("scope"), "openid offline_access");
postData.addQueryItem(QStringLiteral("device"), "qgc");
QUrl url(QStringLiteral("https://sso.airmap.io/oauth/ro"));
_post(url, postData.toString(QUrl::FullyEncoded).toUtf8());
} }
void AirMapLogin::_requestFinished(void) void AirMapSharedState::doRequestWithLogin(const Callback& callback)
{ {
QNetworkReply* reply = qobject_cast<QNetworkReply*>(QObject::sender()); if (isLoggedIn()) {
_isLoginInProgress = false; callback(_loginToken);
} else {
QByteArray responseBytes = reply->readAll(); login();
QJsonParseError parseError; _pendingRequests.enqueue(callback);
QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError);
if (parseError.error != QJsonParseError::NoError) {
QNetworkReply::NetworkError networkError = QNetworkReply::NetworkError::UnknownNetworkError;
emit loginFailure(networkError, "", "");
return;
}
QJsonObject rootObject = responseJson.object();
// When an error occurs we still end up here
if (reply->error() != QNetworkReply::NoError) {
QJsonValue errorDescription = rootObject.value("error_description");
QString serverError = "";
if (errorDescription.isString()) {
serverError = errorDescription.toString();
}
emit loginFailure(reply->error(), reply->errorString(), serverError);
return;
}
_JWTToken = rootObject["id_token"].toString();
if (_JWTToken == "") { // make sure we got a token
QNetworkReply::NetworkError networkError = QNetworkReply::NetworkError::AuthenticationRequiredError;
emit loginFailure(networkError, "", "");
return;
} }
emit loginSuccess();
}
void AirMapLogin::_requestError(QNetworkReply::NetworkError code)
{
Q_UNUSED(code);
// handled in _requestFinished()
} }
void AirMapLogin::_post(QUrl url, const QByteArray& postData) void AirMapSharedState::login()
{ {
QNetworkRequest request(url); if (isLoggedIn() || _isLoginInProgress) {
request.setHeader(QNetworkRequest::ContentTypeHeader, "application/x-www-form-urlencoded");
request.setRawHeader("X-API-Key", _APIKey.toUtf8());
QNetworkProxy tProxy;
tProxy.setType(QNetworkProxy::DefaultProxy);
_networkManager.setProxy(tProxy);
QNetworkReply* networkReply = _networkManager.post(request, postData);
if (!networkReply) {
QNetworkReply::NetworkError networkError = QNetworkReply::NetworkError::UnknownNetworkError;
emit loginFailure(networkError, "", "");
return; return;
} }
_isLoginInProgress = true;
connect(networkReply, &QNetworkReply::finished, this, &AirMapLogin::_requestFinished); if (_settings.userName == "") { //use anonymous login
connect(networkReply, static_cast<void (QNetworkReply::*)(QNetworkReply::NetworkError)>(&QNetworkReply::error), this, &AirMapLogin::_requestError);
}
AirMapNetworking::AirMapNetworking(SharedData& networkingData) Authenticator::AuthenticateAnonymously::Params params;
: _networkingData(networkingData) 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();
try {
std::rethrow_exception(result.error());
} catch (const std::exception& e) {
// TODO: emit signal
emit error("Failed to authenticate with AirMap", e.what(), "");
}
}
});
void AirMapNetworking::post(QUrl url, const QByteArray& postData, bool isJsonData, bool requiresLogin)
{
QNetworkRequest request(url);
if (isJsonData) {
request.setHeader(QNetworkRequest::ContentTypeHeader, "application/json; charset=utf-8");
} else { } else {
request.setHeader(QNetworkRequest::ContentTypeHeader, "application/x-www-form-urlencoded");
}
request.setRawHeader("X-API-Key", _networkingData.airmapAPIKey.toUtf8());
if (requiresLogin) {
if (_networkingData.login.isLoggedIn()) {
request.setRawHeader("Authorization", (QString("Bearer ")+_networkingData.login.JWTToken()).toUtf8());
} else {
connect(&_networkingData.login, &AirMapLogin::loginSuccess, this, &AirMapNetworking::_loginSuccess);
connect(&_networkingData.login, &AirMapLogin::loginFailure, this, &AirMapNetworking::_loginFailure);
_pendingRequest.type = RequestType::POST;
_pendingRequest.url = url;
_pendingRequest.postData = postData;
_pendingRequest.isJsonData = isJsonData;
_pendingRequest.requiresLogin = requiresLogin;
_networkingData.login.login();
return;
}
}
QNetworkProxy tProxy;
tProxy.setType(QNetworkProxy::DefaultProxy);
_networkingData.networkManager.setProxy(tProxy);
QNetworkReply* networkReply = _networkingData.networkManager.post(request, postData);
if (!networkReply) {
QNetworkReply::NetworkError networkError = QNetworkReply::NetworkError::UnknownNetworkError;
emit error(networkError, "", "");
return;
}
connect(networkReply, &QNetworkReply::finished, this, &AirMapNetworking::_requestFinished);
_currentNetworkReply = networkReply;
}
void AirMapNetworking::_loginSuccess()
{
disconnect(&_networkingData.login, &AirMapLogin::loginSuccess, this, &AirMapNetworking::_loginSuccess);
disconnect(&_networkingData.login, &AirMapLogin::loginFailure, this, &AirMapNetworking::_loginFailure);
if (_pendingRequest.type == RequestType::GET) { Authenticator::AuthenticateWithPassword::Params params;
get(_pendingRequest.url, _pendingRequest.requiresLogin); params.oauth.username = _settings.userName.toStdString();
} else if (_pendingRequest.type == RequestType::POST) { params.oauth.password = _settings.password.toStdString();
post(_pendingRequest.url, _pendingRequest.postData, _pendingRequest.isJsonData, _pendingRequest.requiresLogin); params.oauth.client_id = _settings.clientID.toStdString();
} params.oauth.device_id = "QGroundControl";
_pendingRequest.type = RequestType::None; _client->authenticator().authenticate_with_password(params,
} [this](const Authenticator::AuthenticateWithPassword::Result& result) {
void AirMapNetworking::_loginFailure(QNetworkReply::NetworkError networkError, const QString& errorString, const QString& serverErrorMessage) if (!_isLoginInProgress) { // was logout() called in the meanwhile?
{ return;
disconnect(&_networkingData.login, &AirMapLogin::loginSuccess, this, &AirMapNetworking::_loginSuccess); }
disconnect(&_networkingData.login, &AirMapLogin::loginFailure, this, &AirMapNetworking::_loginFailure); if (result) {
emit error(networkError, errorString, serverErrorMessage); qCDebug(AirMapManagerLog)<<"Successfully authenticated with AirMap: id="<< result.value().id.c_str()<<", access="
} <<result.value().access.c_str();
_loginToken = QString::fromStdString(result.value().id);
void AirMapNetworking::abort() _processPendingRequests();
{ } else {
if (_currentNetworkReply) { _pendingRequests.clear();
disconnect(_currentNetworkReply, &QNetworkReply::finished, this, &AirMapNetworking::_requestFinished); try {
_currentNetworkReply->abort(); std::rethrow_exception(result.error());
_currentNetworkReply = nullptr; } catch (const std::exception& e) {
// TODO: proper error handling
emit error("Failed to authenticate with AirMap", e.what(), "");
}
}
});
} }
_pendingRequest.type = RequestType::None;
} }
void AirMapNetworking::get(QUrl url, bool requiresLogin) void AirMapSharedState::_processPendingRequests()
{ {
QNetworkRequest request(url); while (!_pendingRequests.isEmpty()) {
_pendingRequests.dequeue()(_loginToken);
request.setRawHeader("X-API-Key", _networkingData.airmapAPIKey.toUtf8());
if (requiresLogin) {
if (_networkingData.login.isLoggedIn()) {
request.setRawHeader("Authorization", (QString("Bearer ")+_networkingData.login.JWTToken()).toUtf8());
} else {
connect(&_networkingData.login, &AirMapLogin::loginSuccess, this, &AirMapNetworking::_loginSuccess);
connect(&_networkingData.login, &AirMapLogin::loginFailure, this, &AirMapNetworking::_loginFailure);
_pendingRequest.type = RequestType::GET;
_pendingRequest.url = url;
_pendingRequest.requiresLogin = requiresLogin;
_networkingData.login.login();
return;
}
}
QNetworkProxy tProxy;
tProxy.setType(QNetworkProxy::DefaultProxy);
_networkingData.networkManager.setProxy(tProxy);
QNetworkReply* networkReply = _networkingData.networkManager.get(request);
if (!networkReply) {
QNetworkReply::NetworkError networkError = QNetworkReply::NetworkError::UnknownNetworkError;
emit error(networkError, "", "");
return;
} }
connect(networkReply, &QNetworkReply::finished, this, &AirMapNetworking::_requestFinished);
_currentNetworkReply = networkReply;
} }
void AirMapNetworking::_requestFinished(void) void AirMapSharedState::logout()
{ {
QNetworkReply* reply = qobject_cast<QNetworkReply*>(QObject::sender()); _isLoginInProgress = false; // cancel if we're currently trying to login
_currentNetworkReply = nullptr;
// When an error occurs we still end up here
if (reply->error() != QNetworkReply::NoError) {
QByteArray responseBytes = reply->readAll();
QJsonParseError parseError;
QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError);
QJsonObject rootObject = responseJson.object();
qCDebug(AirMapManagerLog) << "Server error:" << rootObject;
QString serverError = "";
if (rootObject.contains("data")) { // eg. in case of a conflict message
serverError = rootObject["data"].toObject()["message"].toString();
} else if (rootObject.contains("error_description")) { // eg. login failure
serverError = rootObject["error_description"].toString();
} else if (rootObject.contains("message")) { // eg. api key failure
serverError = rootObject["message"].toString();
}
emit error(reply->error(), reply->errorString(), serverError);
return;
}
// Check for redirection if (!isLoggedIn()) {
QVariant redirectionTarget = reply->attribute(QNetworkRequest::RedirectionTargetAttribute);
if (!redirectionTarget.isNull()) {
QUrl redirectUrl = reply->url().resolved(redirectionTarget.toUrl());
get(redirectUrl);
reply->deleteLater();
return; return;
} }
_loginToken = "";
_pendingRequests.clear();
QByteArray responseBytes = reply->readAll();
QJsonParseError parseError;
QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError);
if (parseError.error != QJsonParseError::NoError) {
qCWarning(AirMapManagerLog) << "JSON parse error:" << parseError.errorString();
}
emit finished(parseError, responseJson);
} }
AirMapRestrictionManager::AirMapRestrictionManager(AirMapSharedState& shared)
AirMapRestrictionManager::AirMapRestrictionManager(AirMapNetworking::SharedData& sharedData) : _shared(shared)
: _networking(sharedData)
{ {
connect(&_networking, &AirMapNetworking::finished, this, &AirMapRestrictionManager::_parseAirspaceJson); //connect(&_networking, &AirMapNetworking::finished, this, &AirMapRestrictionManager::_parseAirspaceJson);
connect(&_networking, &AirMapNetworking::error, this, &AirMapRestrictionManager::_error); //connect(&_networking, &AirMapNetworking::error, this, &AirMapRestrictionManager::_error);
} }
void AirMapRestrictionManager::setROI(const QGeoCoordinate& center, double radiusMeters) void AirMapRestrictionManager::setROI(const QGeoCoordinate& center, double radiusMeters)
{ {
if (!_networking.hasAPIKey()) { if (!_shared.client()) {
qCDebug(AirMapManagerLog) << "API key not set. Not updating Airspace"; qCDebug(AirMapManagerLog) << "No AirMap client instance. Not updating Airspace";
return; return;
} }
...@@ -308,233 +144,92 @@ void AirMapRestrictionManager::setROI(const QGeoCoordinate& center, double radiu ...@@ -308,233 +144,92 @@ void AirMapRestrictionManager::setROI(const QGeoCoordinate& center, double radiu
qCWarning(AirMapManagerLog) << "AirMapRestrictionManager::updateROI: state not idle"; qCWarning(AirMapManagerLog) << "AirMapRestrictionManager::updateROI: state not idle";
return; return;
} }
qCDebug(AirMapManagerLog) << "setting ROI";
// Build up the polygon for the query
// QJsonObject polygonJson;
//
// polygonJson["type"] = "Polygon";
//
// QGeoCoordinate left = _roiCenter.atDistanceAndAzimuth(_roiRadius, -90);
// QGeoCoordinate right = _roiCenter.atDistanceAndAzimuth(_roiRadius, 90);
// QGeoCoordinate top = _roiCenter.atDistanceAndAzimuth(_roiRadius, 0);
// QGeoCoordinate bottom = _roiCenter.atDistanceAndAzimuth(_roiRadius, 180);
//
// QGeoCoordinate topLeft(top.latitude(), left.longitude());
// QGeoCoordinate topRight(top.latitude(), right.longitude());
// QGeoCoordinate bottomLeft(bottom.latitude(), left.longitude());
// QGeoCoordinate bottomRight(bottom.latitude(), left.longitude());
//
// QJsonValue coordValue;
// QJsonArray rgVertex;
//
// // GeoJson polygons are right handed and include duplicate first and last vertex
// JsonHelper::saveGeoJsonCoordinate(topLeft, false /* writeAltitude */, coordValue);
// rgVertex.append(coordValue);
// JsonHelper::saveGeoJsonCoordinate(bottomLeft, false /* writeAltitude */, coordValue);
// rgVertex.append(coordValue);
// JsonHelper::saveGeoJsonCoordinate(bottomRight, false /* writeAltitude */, coordValue);
// rgVertex.append(coordValue);
// JsonHelper::saveGeoJsonCoordinate(topRight, false /* writeAltitude */, coordValue);
// rgVertex.append(coordValue);
// JsonHelper::saveGeoJsonCoordinate(topLeft, false /* writeAltitude */, coordValue);
// rgVertex.append(coordValue);
//
// QJsonArray rgPolygon;
// rgPolygon.append(rgVertex);
//
// polygonJson["coordinates"] = rgPolygon;
// QJsonDocument polygonJsonDoc(polygonJson);
_polygonList.clear(); _polygonList.clear();
_circleList.clear(); _circleList.clear();
// Build up the http query _state = State::RetrieveItems;
QUrlQuery airspaceQuery; Airspaces::Search::Parameters params;
params.geometry = Geometry::point(center.latitude(), center.longitude());
airspaceQuery.addQueryItem(QStringLiteral("latitude"), QString::number(center.latitude(), 'f', 10)); params.buffer = radiusMeters;
airspaceQuery.addQueryItem(QStringLiteral("longitude"), QString::number(center.longitude(), 'f', 10)); params.full = true;
airspaceQuery.addQueryItem(QStringLiteral("weather"), QStringLiteral("true")); _shared.client()->airspaces().search(params,
airspaceQuery.addQueryItem(QStringLiteral("buffer"), QString::number(radiusMeters, 'f', 0)); [this](const Airspaces::Search::Result& result) {
QUrl airMapAirspaceUrl(QStringLiteral("https://api.airmap.com/status/alpha/point")); if (_state != State::RetrieveItems) return;
airMapAirspaceUrl.setQuery(airspaceQuery);
if (result) {
_state = State::RetrieveList; const std::vector<Airspace>& airspaces = result.value();
_networking.get(airMapAirspaceUrl); qCDebug(AirMapManagerLog)<<"Successful search: " << airspaces.size();
} for (const auto& airspace : airspaces) {
void AirMapRestrictionManager::_parseAirspaceJson(QJsonParseError parseError, QJsonDocument airspaceDoc) const Geometry& geometry = airspace.geometry();
{ switch(geometry.type()) {
Q_UNUSED(parseError); case Geometry::Type::polygon: {
QJsonObject rootObject = airspaceDoc.object(); const Geometry::Polygon& polygon = geometry.details_for_polygon();
QVariantList polygonArray;
switch(_state) { if (polygon.size() == 1) {
case State::RetrieveList: for (const auto& vertex : polygon[0].coordinates) {
{ QGeoCoordinate coord;
QSet<QString> advisorySet; if (vertex.altitude) {
const QJsonArray& advisoriesArray = rootObject["data"].toObject()["advisories"].toArray(); coord = QGeoCoordinate(vertex.latitude, vertex.longitude, vertex.altitude.get());
for (int i=0; i< advisoriesArray.count(); i++) { } else {
const QJsonObject& advisoryObject = advisoriesArray[i].toObject(); coord = QGeoCoordinate(vertex.latitude, vertex.longitude);
QString advisoryId(advisoryObject["id"].toString()); }
qCDebug(AirMapManagerLog) << "Advisory id: " << advisoryId; polygonArray.append(QVariant::fromValue(coord));
advisorySet.insert(advisoryId); }
} _polygonList.append(new PolygonAirspaceRestriction(polygonArray));
for (const auto& advisoryId : advisorySet) {
QUrl url(QStringLiteral("https://api.airmap.com/airspace/v2/")+advisoryId);
_networking.get(url);
}
_numAwaitingItems = advisorySet.size();
if (_numAwaitingItems > 0) {
_state = State::RetrieveItems;
} else {
_state = State::Idle;
}
}
break;
case State::RetrieveItems:
{
const QJsonArray& airspaceArray = rootObject["data"].toArray();
for (int i = 0; i < airspaceArray.count(); i++) {
const QJsonObject& airspaceObject = airspaceArray[i].toObject();
QString airspaceType(airspaceObject["type"].toString());
QString airspaceId(airspaceObject["id"].toString());
QString airspaceName(airspaceObject["name"].toString());
const QJsonObject& airspaceGeometry(airspaceObject["geometry"].toObject());
QString geometryType(airspaceGeometry["type"].toString());
const QJsonObject& airspaceProperties(airspaceObject["properties"].toObject());
bool enabled = airspaceProperties["enabled"].toBool(true);
if (!enabled) {
continue;
}
qCDebug(AirMapManagerLog) << "Airspace ID:" << airspaceId << "name:" << airspaceName << "type:" << airspaceType << "geometry:" << geometryType;
// filter by start and end time
const QJsonValue& effectiveStart(airspaceProperties["effective_start"]);
QDateTime now = QDateTime::currentDateTimeUtc();
if (!effectiveStart.isNull()) {
QDateTime d = QDateTime::fromString(effectiveStart.toString(), Qt::ISODate);
if (d > now.addSecs(3600)) {
qCDebug(AirMapManagerLog) << "effective start filter";
continue;
}
}
const QJsonValue& effectiveEnd(airspaceProperties["effective_end"]);
if (!effectiveEnd.isNull()) {
QDateTime d = QDateTime::fromString(effectiveEnd.toString(), Qt::ISODate);
if (d < now.addSecs(-3600)) {
qCDebug(AirMapManagerLog) << "effective end filter";
continue;
}
}
int authorizationLevel = (int)(airspaceProperties["authorization_level"].toDouble(3.)+0.5);
// 1 == green
if (authorizationLevel <= 1 || authorizationLevel > 3) {
qCDebug(AirMapManagerLog) << "auth level filter";
continue;
}
if (geometryType == "Polygon") {
const QJsonArray& airspaceCoordinates(airspaceGeometry["coordinates"].toArray()[0].toArray()); } else {
QString errorString; // TODO: support that?
QmlObjectListModel list; qWarning() << "Empty polygon, or Polygon with holes. Size: "<<polygon.size();
if (JsonHelper::loadPolygon(airspaceCoordinates, list, this, errorString)) {
QVariantList polygon;
for (int i = 0; i < list.count(); ++i) {
polygon.append(QVariant::fromValue(((QGCQGeoCoordinate*)list[i])->coordinate()));
} }
list.clearAndDeleteContents();
_polygonList.append(new PolygonAirspaceRestriction(polygon));
} else {
//TODO
qWarning() << errorString;
} }
break;
} else if (geometryType == "MultiPolygon") { case Geometry::Type::multi_polygon: {
// TODO: it's possible (?) that polygons contain holes. These need to be rendered properly const Geometry::MultiPolygon& multiPolygon = geometry.details_for_multi_polygon();
const QJsonArray& polygonArray = airspaceGeometry["coordinates"].toArray(); qWarning() << "multi polygon "<<multiPolygon.size();
for (int polygonIdx = 0; polygonIdx < polygonArray.count(); polygonIdx++) { // TODO
const QJsonArray& airspaceCoordinates(polygonArray[polygonIdx].toArray()[0].toArray());
QString errorString;
QmlObjectListModel list;
if (JsonHelper::loadPolygon(airspaceCoordinates, list, this, errorString)) {
QVariantList polygon;
for (int i = 0; i < list.count(); ++i) {
polygon.append(QVariant::fromValue(((QGCQGeoCoordinate*)list[i])->coordinate()));
}
list.clearAndDeleteContents();
_polygonList.append(new PolygonAirspaceRestriction(polygon));
}
} }
break;
} else { case Geometry::Type::point: {
// TODO: are there any circles? const Geometry::Point& point = geometry.details_for_point();
qWarning() << "Unknown geometry type:" << geometryType; _circleList.append(new CircularAirspaceRestriction(QGeoCoordinate(point.latitude, point.longitude), 0.));
// TODO: radius???
}
break;
default:
qWarning() << "unsupported geometry type: "<<(int)geometry.type();
break;
} }
}
if (--_numAwaitingItems == 0) {
_state = State::Idle;
emit requestDone(true);
} }
}
break;
default:
qCDebug(AirMapManagerLog) << "Error: wrong state";
break;
}
// https://api.airmap.com/airspace/v2/search API
// const QJsonArray& airspaceArray = rootObject["data"].toArray();
// for (int i=0; i< airspaceArray.count(); i++) {
// const QJsonObject& airspaceObject = airspaceArray[i].toObject();
// QString airspaceType(airspaceObject["type"].toString());
// qDebug() << airspaceType;
// qDebug() << airspaceObject["name"].toString();
// QGeoCoordinate center(airspaceObject["latitude"].toDouble(), airspaceObject["longitude"].toDouble());
// qDebug() << center;
// if (airspaceType == "airport") {
// _circleList.append(new CircularAirspaceRestriction(center, 5000));
// }
// }
}
void AirMapRestrictionManager::_error(QNetworkReply::NetworkError code, const QString& errorString,
const QString& serverErrorMessage)
{
qCWarning(AirMapManagerLog) << "AirMapRestrictionManager::_error" << code << serverErrorMessage;
if (_state == State::RetrieveItems) { } else {
if (--_numAwaitingItems == 0) { try {
_state = State::Idle; std::rethrow_exception(result.error());
emit requestDone(false); } catch (const std::exception& e) {
// TODO: proper error handling
emit error("Failed to authenticate with AirMap", e.what(), "");
}
} }
} else { emit requestDone(true);
_state = State::Idle; _state = State::Idle;
} });
emit networkError(code, errorString, serverErrorMessage);
} }
AirMapFlightManager::AirMapFlightManager(AirMapNetworking::SharedData& sharedData) AirMapFlightManager::AirMapFlightManager(AirMapSharedState& shared)
: _networking(sharedData) : _shared(shared)
{ {
connect(&_networking, &AirMapNetworking::finished, this, &AirMapFlightManager::_parseJson); connect(&_pollTimer, &QTimer::timeout, this, &AirMapFlightManager::_pollBriefing);
connect(&_networking, &AirMapNetworking::error, this, &AirMapFlightManager::_error);
connect(&_pollTimer, &QTimer::timeout, this, &AirMapFlightManager::_sendBriefingRequest);
} }
void AirMapFlightManager::createFlight(const QList<MissionItem*>& missionItems) void AirMapFlightManager::createFlight(const QList<MissionItem*>& missionItems)
{ {
if (!_networking.getLogin().hasCredentials()) { if (!_shared.client()) {
qCDebug(AirMapManagerLog) << "Login Credentials not set: will not send flight"; qCDebug(AirMapManagerLog) << "No AirMap client instance. Will not create a flight";
return; return;
} }
...@@ -579,9 +274,33 @@ void AirMapFlightManager::createFlight(const QList<MissionItem*>& missionItems) ...@@ -579,9 +274,33 @@ void AirMapFlightManager::createFlight(const QList<MissionItem*>& missionItems)
if (_pilotID == "") { if (_pilotID == "") {
// need to get the pilot id before uploading the flight // need to get the pilot id before uploading the flight
QUrl url(QString("https://api.airmap.com/pilot/v2/profile")); qCDebug(AirMapManagerLog) << "Getting pilot ID";
_networking.get(url, true);
_state = State::GetPilotID; _state = State::GetPilotID;
_shared.doRequestWithLogin([this](const QString& login_token) {
Pilots::Authenticated::Parameters params;
params.authorization = login_token.toStdString();
_shared.client()->pilots().authenticated(params, [this](const Pilots::Authenticated::Result& result) {
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;
try {
std::rethrow_exception(result.error());
} catch (const std::exception& e) {
// TODO: proper error handling
emit error("Failed to create Flight Plan", e.what(), "");
}
}
});
});
} else { } else {
_uploadFlight(); _uploadFlight();
} }
...@@ -590,6 +309,48 @@ void AirMapFlightManager::createFlight(const QList<MissionItem*>& missionItems) ...@@ -590,6 +309,48 @@ void AirMapFlightManager::createFlight(const QList<MissionItem*>& missionItems)
emit flightPermitStatusChanged(); 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};
_shared.client()->flights().search(params, [this](const Flights::Search::Result& result) {
if (_state != State::EndFirstFlight) return;
if (result && result.value().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()[0].id; // pick the first flight (TODO: match the vehicle id)
_shared.client()->flights().end_flight(params, [this](const Flights::EndFlight::Result& result) {
if (_state != State::EndFirstFlight) return;
if (!result) {
try {
std::rethrow_exception(result.error());
} catch (const std::exception& e) {
// TODO: emit signal
emit error("Failed to end first Flight", e.what(), "");
}
}
_state = State::Idle;
_uploadFlight();
});
} else {
_state = State::Idle;
_uploadFlight();
}
});
}
void AirMapFlightManager::_uploadFlight() void AirMapFlightManager::_uploadFlight()
{ {
...@@ -600,235 +361,153 @@ void AirMapFlightManager::_uploadFlight() ...@@ -600,235 +361,153 @@ void AirMapFlightManager::_uploadFlight()
} }
if (_noFlightCreatedYet) { if (_noFlightCreatedYet) {
// it could be that AirMap still has an open pending flight, but we don't know the flight ID. _endFirstFlight();
// As there can only be one, we query the flights that end in the future, and close it if there's one.
QUrlQuery flightsQuery;
flightsQuery.addQueryItem(QStringLiteral("pilot_id"), _pilotID);
QDateTime end_time = QDateTime::currentDateTime().toUTC().addSecs(-60*60);
flightsQuery.addQueryItem(QStringLiteral("end_after"), end_time.toString(Qt::ISODate));
QUrl flightsQueryUrl(QStringLiteral("https://api.airmap.com/flight/v2/"));
flightsQueryUrl.setQuery(flightsQuery);
_networking.get(flightsQueryUrl, true);
_state = State::EndFirstFlight;
_noFlightCreatedYet = false; _noFlightCreatedYet = false;
return; return;
} }
qCDebug(AirMapManagerLog) << "uploading flight"; qCDebug(AirMapManagerLog) << "uploading flight";
QJsonObject root;
QJsonObject geometryObject;
geometryObject.insert("type", "LineString");
QJsonArray coordinatesArray;
for (const auto& coord : _flight.coords) {
QJsonArray coordinate;
coordinate.push_back(coord.longitude());
coordinate.push_back(coord.latitude());
coordinatesArray.push_back(coordinate);
}
geometryObject.insert("coordinates", coordinatesArray);
root.insert("geometry", geometryObject);
root.insert("max_altitude_agl", _flight.maxAltitude);
root.insert("buffer", 2);
QJsonObject flightFeatures;
if (_sitaUavRegistrationId != "") {
flightFeatures.insert("sita_uav_registration_id", _sitaUavRegistrationId);
}
if (_sitaPilotRegistrationId != "") {
flightFeatures.insert("sita_pilot_registration_id", _sitaPilotRegistrationId);
}
root.insert("flight_features", flightFeatures);
root.insert("takeoff_latitude", _flight.takeoffCoord.latitude());
root.insert("takeoff_longitude", _flight.takeoffCoord.longitude());
QJsonArray rulesets;
rulesets.push_back("city_d3qzey_drone_rules");
rulesets.push_back("che_drone_rules");
rulesets.push_back("custom_kz6e55_drone_rules");
rulesets.push_back("che_notam");
rulesets.push_back("che_airmap_rules");
rulesets.push_back("che_nature_preserve");
root.insert("rulesets", rulesets);
root.insert("pilot_id", _pilotID);
QDateTime now = QDateTime::currentDateTime().toUTC();
QDateTime startTime = now.addSecs(5 * 60); // TODO: user configurable?
QDateTime endTime = now.addSecs(2 * 60 * 60);
root.insert("start_time", startTime.toString(Qt::ISODate));
root.insert("end_time", endTime.toString(Qt::ISODate));
_flight.coords.clear();
_state = State::FlightUpload; _state = State::FlightUpload;
QUrl url(QStringLiteral("https://api.airmap.com/flight/v2/plan")); _shared.doRequestWithLogin([this](const QString& login_token) {
qCDebug(AirMapManagerLog) << root; if (_state != State::FlightUpload) return;
_networking.post(url, QJsonDocument(root).toJson(), true, true);
} FlightPlans::Create::Parameters params;
params.max_altitude = _flight.maxAltitude;
void AirMapFlightManager::endFlight() params.buffer = 2.f;
{ params.latitude = _flight.takeoffCoord.latitude();
if (_currentFlightId.length() == 0) { params.longitude = _flight.takeoffCoord.longitude();
return; params.pilot.id = _pilotID.toStdString();
} params.start_time = Clock::universal_time() + Minutes{5};
if (_state != State::Idle) { params.end_time = Clock::universal_time() + Hours{2}; // TODO: user-configurable?
qCWarning(AirMapManagerLog) << "AirMapFlightManager::endFlight: State not idle"; params.rulesets = { // TODO: which ones to use?
return; "che_drone_rules",
} "che_notam",
_endFlight(_currentFlightId); "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);
}
_flightPermitStatus = AirspaceAuthorization::PermitUnknown; params.geometry = Geometry(lineString);
emit flightPermitStatusChanged(); params.authorization = login_token.toStdString();
} _flight.coords.clear();
void AirMapFlightManager::abort() _shared.client()->flight_plans().create_by_polygon(params, [this](const FlightPlans::Create::Result& result) {
{ if (_state != State::FlightUpload) return;
_state = State::Idle;
_networking.abort();
_flightPermitStatus = AirspaceAuthorization::PermitUnknown;
emit flightPermitStatusChanged();
}
void AirMapFlightManager::_endFlight(const QString& flightID) if (result) {
{ _pendingFlightPlan = QString::fromStdString(result.value().id);
qCDebug(AirMapManagerLog) << "ending flight" << flightID; qCDebug(AirMapManagerLog) << "Flight Plan created:"<<_pendingFlightPlan;
_state = State::FlightEnd; _checkForValidBriefing();
QUrl url(QString("https://api.airmap.com/flight/v2/%1/end").arg(flightID)); } else {
// to kill the flight, use: https://api.airmap.com/flight/v2/%1/delete (otherwise same query) // TODO
qCDebug(AirMapManagerLog) << "Flight Plan creation failed";
}
_networking.post(url, QByteArray(), false, true); });
}
void AirMapFlightManager::_sendBriefingRequest() });
{
QUrl url(QString("https://api.airmap.com/flight/v2/plan/%1/briefing").arg(_pendingFlightPlan));
_networking.get(url, true);
} }
void AirMapFlightManager::_parseJson(QJsonParseError parseError, QJsonDocument doc) void AirMapFlightManager::_checkForValidBriefing()
{ {
Q_UNUSED(parseError); _state = State::FlightBrief;
QJsonObject rootObject = doc.object(); FlightPlans::RenderBriefing::Parameters params;
params.authorization = _shared.loginToken().toStdString();
switch(_state) { params.id = _pendingFlightPlan.toStdString();
case State::GetPilotID: _shared.client()->flight_plans().render_briefing(params, [this](const FlightPlans::RenderBriefing::Result& result) {
{ if (_state != State::FlightBrief) return;
QString status = rootObject["status"].toString();
if (status == "success") {
const QJsonObject& dataObject = rootObject["data"].toObject();
_pilotID = dataObject["id"].toString();
qCDebug(AirMapManagerLog) << "Pilot ID:" << _pilotID;
_uploadFlight();
} else {
QNetworkReply::NetworkError networkError = QNetworkReply::NetworkError::UnknownContentError;
emit _error(networkError, "Failed to get the pilot ID", "");
_flightPermitStatus = AirspaceAuthorization::PermitUnknown;
emit flightPermitStatusChanged();
_state = State::Idle;
}
}
break;
case State::FlightUpload:
{
qCDebug(AirMapManagerLog) << "flight uploaded:" << rootObject;
const QJsonObject& dataObject = rootObject["data"].toObject();
_pendingFlightPlan = dataObject["id"].toString();
qCDebug(AirMapManagerLog) << "Got Flight Plan:" << _pendingFlightPlan;
QString status = rootObject["status"].toString();
if (status == "success") {
_sendBriefingRequest();
_state = State::FlightBrief;
} else {
QNetworkReply::NetworkError networkError = QNetworkReply::NetworkError::UnknownContentError;
emit _error(networkError, "Failed to create the flight", "");
_flightPermitStatus = AirspaceAuthorization::PermitUnknown;
emit flightPermitStatusChanged();
_state = State::Idle;
}
}
break;
case State::FlightBrief:
{
qCDebug(AirMapManagerLog) << "flight briefing response:" << rootObject;
// check if the validations are valid if (result) {
const QJsonObject& dataObject = rootObject["data"].toObject();
const QJsonArray& validationsArray = dataObject["validations"].toArray();
bool allValid = true; bool allValid = true;
for (int i = 0; i < validationsArray.count(); i++) { for (const auto& validation : result.value().validations) {
const QJsonObject& validationObject = validationsArray[i].toObject(); if (validation.status != FlightPlan::Briefing::Validation::Status::valid) {
QString authority = validationObject["authority"].toObject()["name"].toString(); emit error(QString("%1 registration identifier is invalid: %2").arg(
QString status = validationObject["status"].toString(); QString::fromStdString(validation.authority.name)).arg(QString::fromStdString(validation.message)), "", "");
QString identifier = validationObject["data"].toString();
if (status != "valid") {
QNetworkReply::NetworkError networkError = QNetworkReply::NetworkError::AuthenticationRequiredError;
emit _error(networkError, QString("%1 registration identifier (%2) is invalid").arg(authority).arg(identifier), "");
allValid = false; allValid = false;
} }
} }
if (allValid) { if (allValid) {
QUrl url(QString("https://api.airmap.com/flight/v2/plan/%1/submit").arg(_pendingFlightPlan)); _submitPendingFlightPlan();
_networking.post(url, QByteArray(), false, true);
_state = State::FlightSubmit;
} else { } else {
_flightPermitStatus = AirspaceAuthorization::PermitRejected; _flightPermitStatus = AirspaceAuthorization::PermitRejected;
emit flightPermitStatusChanged(); emit flightPermitStatusChanged();
_state = State::Idle; _state = State::Idle;
} }
} else {
// TODO
} }
break; });
case State::FlightSubmit: }
{
qCDebug(AirMapManagerLog) << "flight submit response:" << rootObject; void AirMapFlightManager::_submitPendingFlightPlan()
{
QString status = rootObject["status"].toString(); _state = State::FlightSubmit;
if (status == "success") { FlightPlans::Submit::Parameters params;
_sendBriefingRequest(); params.authorization = _shared.loginToken().toStdString();
const QJsonObject& dataObject = rootObject["data"].toObject(); params.id = _pendingFlightPlan.toStdString();
_pendingFlightId = dataObject["flight_id"].toString(); _shared.client()->flight_plans().submit(params, [this](const FlightPlans::Submit::Result& result) {
_state = State::FlightPolling; if (_state != State::FlightSubmit) return;
} else {
QNetworkReply::NetworkError networkError = QNetworkReply::NetworkError::UnknownContentError; if (result) {
emit _error(networkError, "Failed to create the flight", ""); _pendingFlightId = QString::fromStdString(result.value().flight_id.get());
_state = State::Idle; _state = State::FlightPolling;
} _pollBriefing();
} else {
// TODO
} }
break; });
}
case State::FlightPolling: void AirMapFlightManager::_pollBriefing()
{ {
qCDebug(AirMapManagerLog) << "flight polling/briefing response:" << rootObject; if (_state != State::FlightPolling) {
const QJsonObject& dataObject = rootObject["data"].toObject(); qCWarning(AirMapManagerLog) << "AirMapFlightManager::_pollBriefing: not in polling state";
const QJsonArray& authorizationsArray = dataObject["authorizations"].toArray(); return;
}
FlightPlans::RenderBriefing::Parameters params;
params.authorization = _shared.loginToken().toStdString();
params.id = _pendingFlightPlan.toStdString();
_shared.client()->flight_plans().render_briefing(params, [this](const FlightPlans::RenderBriefing::Result& result) {
if (_state != State::FlightPolling) return;
if (result) {
const FlightPlan::Briefing& briefing = result.value();
qCDebug(AirMapManagerLog) << "flight polling/briefing response";
bool rejected = false; bool rejected = false;
bool accepted = false; bool accepted = false;
bool pending = false; bool pending = false;
for (int i = 0; i < authorizationsArray.count(); i++) { for (const auto& authorization : briefing.authorizations) {
const QJsonObject& authorizationObject = authorizationsArray[i].toObject(); switch (authorization.status) {
QString status = authorizationObject["status"].toString(); case FlightPlan::Briefing::Authorization::Status::accepted:
if (status == "accepted") { case FlightPlan::Briefing::Authorization::Status::accepted_upon_submission:
accepted = true; accepted = true;
} else if (status == "rejected") { break;
case FlightPlan::Briefing::Authorization::Status::rejected:
case FlightPlan::Briefing::Authorization::Status::rejected_upon_submission:
rejected = true; rejected = true;
} else if (status == "pending") { break;
case FlightPlan::Briefing::Authorization::Status::pending:
pending = true; pending = true;
break;
} }
} }
if (authorizationsArray.size() == 0) { if (briefing.authorizations.size() == 0) {
// if we don't get any authorizations, we assume it's accepted // if we don't get any authorizations, we assume it's accepted
accepted = true; accepted = true;
} }
...@@ -850,79 +529,64 @@ void AirMapFlightManager::_parseJson(QJsonParseError parseError, QJsonDocument d ...@@ -850,79 +529,64 @@ void AirMapFlightManager::_parseJson(QJsonParseError parseError, QJsonDocument d
_pollTimer.setSingleShot(true); _pollTimer.setSingleShot(true);
_pollTimer.start(2000); _pollTimer.start(2000);
} }
} else {
} // TODO: error handling
break;
case State::FlightEnd:
_pendingFlightId = "";
_pendingFlightPlan = "";
_currentFlightId = "";
_state = State::Idle;
if (!_flight.coords.empty()) {
_uploadFlight();
}
break;
case State::EndFirstFlight:
{
qCDebug(AirMapManagerLog) << "getting flights:" << rootObject;
const QJsonObject& dataObject = rootObject["data"].toObject();
const QJsonArray& resultsArray = dataObject["results"].toArray();
// first flight is the newest
if (resultsArray.count() > 0) {
const QJsonObject& flight = resultsArray[0].toObject();
QString flightID = flight["id"].toString();
_endFlight(flightID);
} else {
_uploadFlight();
}
} }
break; });
default:
qCDebug(AirMapManagerLog) << "Error: wrong state";
break;
}
} }
void AirMapFlightManager::_error(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage) void AirMapFlightManager::endFlight()
{ {
qCWarning(AirMapManagerLog) << "AirMapFlightManager::_error" << code << serverErrorMessage; if (_currentFlightId.length() == 0) {
if (_state == State::FlightUpload || _state == State::GetPilotID) { return;
_flightPermitStatus = AirspaceAuthorization::PermitUnknown;
emit flightPermitStatusChanged();
} }
_state = State::Idle; if (_state != State::Idle) {
emit networkError(code, errorString, serverErrorMessage); qCWarning(AirMapManagerLog) << "AirMapFlightManager::endFlight: State not idle";
} return;
}
_endFlight(_currentFlightId);
AirMapTelemetry::AirMapTelemetry(AirMapNetworking::SharedData& sharedData) _flightPermitStatus = AirspaceAuthorization::PermitUnknown;
: _networking(sharedData) emit flightPermitStatusChanged();
{
connect(&_networking, &AirMapNetworking::finished, this, &AirMapTelemetry::_parseJson);
connect(&_networking, &AirMapNetworking::error, this, &AirMapTelemetry::_error);
} }
AirMapTelemetry::~AirMapTelemetry() void AirMapFlightManager::_endFlight(const QString& flightID)
{ {
if (_socket) { qCDebug(AirMapManagerLog) << "ending flight" << flightID;
delete _socket;
_socket = nullptr; _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();
_shared.client()->flights().end_flight(params, [this](const Flights::EndFlight::Result& result) {
if (_state != State::FlightEnd) return;
_state = State::Idle;
_pendingFlightId = "";
_pendingFlightPlan = "";
_currentFlightId = "";
if (result) {
if (!_flight.coords.empty()) {
_uploadFlight();
}
} else {
try {
std::rethrow_exception(result.error());
} catch (const std::exception& e) {
// TODO: emit signal
emit error("Failed to end Flight", e.what(), "");
}
}
});
} }
void AirMapTelemetry::_udpTelemetryHostLookup(QHostInfo info)
AirMapTelemetry::AirMapTelemetry(AirMapSharedState& shared)
: _shared(shared)
{ {
if (info.error() != QHostInfo::NoError || info.addresses().length() == 0) {
qCWarning(AirMapManagerLog) << "DNS lookup error:" << info.errorString();
QNetworkReply::NetworkError networkError = QNetworkReply::NetworkError::HostNotFoundError;
emit _error(networkError, "DNS lookup failed", "");
_state = State::Idle;
return;
}
qCDebug(AirMapManagerLog) << "Got Hosts for UDP telemetry:" << info.addresses();
_udpHost = info.addresses()[0];
} }
void AirMapTelemetry::vehicleMavlinkMessageReceived(const mavlink_message_t& message) void AirMapTelemetry::vehicleMavlinkMessageReceived(const mavlink_message_t& message)
...@@ -940,7 +604,7 @@ void AirMapTelemetry::vehicleMavlinkMessageReceived(const mavlink_message_t& mes ...@@ -940,7 +604,7 @@ void AirMapTelemetry::vehicleMavlinkMessageReceived(const mavlink_message_t& mes
bool AirMapTelemetry::isTelemetryStreaming() const bool AirMapTelemetry::isTelemetryStreaming() const
{ {
return _state == State::Streaming && !_udpHost.isNull(); return _state == State::Streaming;
} }
void AirMapTelemetry::_handleGPSRawInt(const mavlink_message_t& message) void AirMapTelemetry::_handleGPSRawInt(const mavlink_message_t& message)
...@@ -968,128 +632,26 @@ void AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message) ...@@ -968,128 +632,26 @@ void AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message)
mavlink_global_position_int_t globalPosition; mavlink_global_position_int_t globalPosition;
mavlink_msg_global_position_int_decode(&message, &globalPosition); mavlink_msg_global_position_int_decode(&message, &globalPosition);
qDebug() << "Got position from vehicle:" << globalPosition.lat << "," << globalPosition.lon; 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
};
// documentation: https://developers.airmap.com/docs/sending-telemetry Flight flight;
flight.id = _flightID.toStdString();
_shared.client()->telemetry().submit_updates(flight, _key,
{Telemetry::Update{position}, Telemetry::Update{speed}});
uint8_t output[512]; // assume the whole packet is not longer than this
uint8_t payload[512];
uint32_t payloadLength = 0;
uint32_t packetHeaderLength = 0;
uint8_t* key = (uint8_t*)_key.data();
uint8_t iv[16];
for (size_t i = 0; i < sizeof(iv); ++i) {
iv[i] = (uint8_t)(qrand() & 0xff); // TODO: should use a secure random source
}
// write packet header & set the length
qToBigEndian(_seqNum, output + packetHeaderLength);
packetHeaderLength += sizeof(_seqNum);
++_seqNum;
QByteArray flightID = _flightID.toUtf8();
output[packetHeaderLength++] = flightID.length();
memcpy(output+packetHeaderLength, flightID.data(), flightID.length());
packetHeaderLength += flightID.length();
output[packetHeaderLength++] = 1; //encryption type = 'aes-256-cbc'
memcpy(output+packetHeaderLength, iv, sizeof(iv));
packetHeaderLength += sizeof(iv);
// write payload
// Position message
airmap::telemetry::Position position;
position.set_timestamp(QDateTime::currentMSecsSinceEpoch());
position.set_latitude((double) globalPosition.lat / 1e7);
position.set_longitude((double) globalPosition.lon / 1e7);
position.set_altitude_msl((float) globalPosition.alt / 1000.f);
position.set_altitude_agl((float) globalPosition.relative_alt / 1000.f);
position.set_horizontal_accuracy(_lastHdop);
uint16_t msgID = 1; // Position: 1, Attitude: 2, Speed: 3, Barometer: 4
uint16_t msgLength = position.ByteSize();
qToBigEndian(msgID, payload+payloadLength);
qToBigEndian(msgLength, payload+payloadLength+sizeof(msgID));
payloadLength += sizeof(msgID) + sizeof(msgLength);
position.SerializeToArray(payload+payloadLength, msgLength);
payloadLength += msgLength;
// Speed message
airmap::telemetry::Speed speed;
speed.set_timestamp(QDateTime::currentMSecsSinceEpoch());
speed.set_velocity_x(globalPosition.vx / 100.f);
speed.set_velocity_y(globalPosition.vy / 100.f);
speed.set_velocity_z(globalPosition.vz / 100.f);
msgID = 3; // Position: 1, Attitude: 2, Speed: 3, Barometer: 4
msgLength = speed.ByteSize();
qToBigEndian(msgID, payload+payloadLength);
qToBigEndian(msgLength, payload+payloadLength+sizeof(msgID));
payloadLength += sizeof(msgID) + sizeof(msgLength);
speed.SerializeToArray(payload+payloadLength, msgLength);
payloadLength += msgLength;
// pad to 16 bytes if necessary
int padding = 16 - (payloadLength % 16);
if (padding != 16) {
for (int i = 0; i < padding; ++i) {
payload[payloadLength+i] = (uint8_t)padding;
}
payloadLength += padding;
}
AES_CBC_encrypt_buffer(output + packetHeaderLength, payload, payloadLength, key, iv);
qDebug() << "Telemetry Position: payload len=" << payloadLength << "header len=" << packetHeaderLength << "msglen=" << msgLength;
_socket->writeDatagram((char*)output, packetHeaderLength+payloadLength, _udpHost, _udpPort);
}
void AirMapTelemetry::_parseJson(QJsonParseError parseError, QJsonDocument doc)
{
Q_UNUSED(parseError);
QJsonObject rootObject = doc.object();
switch(_state) {
case State::StartCommunication:
{
const QJsonObject& dataObject = rootObject["data"].toObject();
QString key = dataObject["key"].toString(); // base64 encoded key
if (key.length() == 0) {
qCWarning(AirMapManagerLog) << "Did not get a valid key";
_state = State::Idle;
return;
}
_key = QByteArray::fromBase64(key.toUtf8());
_seqNum = 1;
if (!_socket) {
_socket = new QUdpSocket(this);
}
qDebug() << "AES key=" << key;
_state = State::Streaming;
}
break;
case State::EndCommunication:
if (_socket) {
delete _socket;
_socket = nullptr;
}
_key = "";
_state = State::Idle;
break;
default:
qCDebug(AirMapManagerLog) << "Error: wrong state";
break;
}
}
void AirMapTelemetry::_error(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage)
{
_state = State::Idle;
qCWarning(AirMapManagerLog) << "AirMapTelemetry::_error" << code << serverErrorMessage;
emit networkError(code, errorString, serverErrorMessage);
} }
void AirMapTelemetry::startTelemetryStream(const QString& flightID) void AirMapTelemetry::startTelemetryStream(const QString& flightID)
...@@ -1101,15 +663,28 @@ void AirMapTelemetry::startTelemetryStream(const QString& flightID) ...@@ -1101,15 +663,28 @@ void AirMapTelemetry::startTelemetryStream(const QString& flightID)
qCInfo(AirMapManagerLog) << "Starting Telemetry stream with flightID" << flightID; qCInfo(AirMapManagerLog) << "Starting Telemetry stream with flightID" << flightID;
QUrl url(QString("https://api.airmap.com/flight/v2/%1/start-comm").arg(flightID));
_networking.post(url, QByteArray(), false, true);
_state = State::StartCommunication; _state = State::StartCommunication;
_flightID = flightID; _flightID = flightID;
// do the DNS lookup concurrently Flights::StartFlightCommunications::Parameters params;
QString host = "api.k8s.stage.airmap.com"; params.authorization = _shared.loginToken().toStdString();
QHostInfo::lookupHost(host, this, SLOT(_udpTelemetryHostLookup(QHostInfo))); params.id = _flightID.toStdString();
_shared.client()->flights().start_flight_communications(params, [this](const Flights::StartFlightCommunications::Result& result) {
if (_state != State::StartCommunication) return;
if (result) {
_key = result.value().key;
_state = State::Streaming;
} else {
_state = State::Idle;
try {
std::rethrow_exception(result.error());
} catch (const std::exception& e) {
// TODO: emit signal
emit error("Failed to start telemetry streaming", e.what(), "");
}
}
});
} }
void AirMapTelemetry::stopTelemetryStream() void AirMapTelemetry::stopTelemetryStream()
...@@ -1119,89 +694,89 @@ void AirMapTelemetry::stopTelemetryStream() ...@@ -1119,89 +694,89 @@ void AirMapTelemetry::stopTelemetryStream()
} }
qCInfo(AirMapManagerLog) << "Stopping Telemetry stream with flightID" << _flightID; qCInfo(AirMapManagerLog) << "Stopping Telemetry stream with flightID" << _flightID;
QUrl url(QString("https://api.airmap.com/flight/v2/%1/end-comm").arg(_flightID));
_networking.post(url, QByteArray(), false, true);
_state = State::EndCommunication; _state = State::EndCommunication;
Flights::EndFlightCommunications::Parameters params;
params.authorization = _shared.loginToken().toStdString();
params.id = _flightID.toStdString();
_shared.client()->flights().end_flight_communications(params, [this](const Flights::EndFlightCommunications::Result& result) {
Q_UNUSED(result);
if (_state != State::EndCommunication) return;
_key = "";
_state = State::Idle;
});
} }
void AirMapTrafficAlertClient::startConnection(const QString& flightID, const QString& password) AirMapTrafficMonitor::~AirMapTrafficMonitor()
{ {
_flightID = flightID; stop();
setClientId("qgc-client-id"); // TODO: random?
setUsername(flightID);
setPassword(password);
connectToHost();
} }
void AirMapTrafficAlertClient::onError(const QMQTT::ClientError error) void AirMapTrafficMonitor::startConnection(const QString& flightID)
{ {
// TODO: user-facing warning _flightID = flightID;
qWarning() << "QMQTT error" << (int)error; auto handler = [this](const Traffic::Monitor::Result& result) {
} 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 {
try {
std::rethrow_exception(result.error());
} catch (const std::exception& e) {
// TODO: error
}
}
};
void AirMapTrafficAlertClient::onConnected() Traffic::Monitor::Params params{flightID.toStdString(), _shared.loginToken().toStdString()};
{ _shared.client()->traffic().monitor(params, handler);
// see https://developers.airmap.com/v2.0/docs/traffic-alerts
subscribe("uav/traffic/sa/" + _flightID, 0); // situational awareness topic
} }
void AirMapTrafficAlertClient::onSubscribed(const QString& topic) void AirMapTrafficMonitor::_update(Traffic::Update::Type type, const std::vector<Traffic::Update>& update)
{ {
qCDebug(AirMapManagerLog) << "subscribed" << topic; qCDebug(AirMapManagerLog) << "Traffic update with" << update.size() << "elements";
}
void AirMapTrafficAlertClient::onReceived(const QMQTT::Message& message) if (type != Traffic::Update::Type::situational_awareness)
{ return; // currently we're only interested in situational awareness
qCDebug(AirMapManagerLog) << "traffic publish received:" << QString::fromUtf8(message.payload());
// looks like this:
// "{"traffic":[{"id":"OAW380-1501565193-airline-0081","direction":-41.20024491976423,"altitude":"5675",
// "latitude":"47.50335","longitude":"8.40912","recorded_time":"1501774066","ground_speed_kts":"199",
// "true_heading":"241","properties":{"aircraft_id":"OAW380"},"timestamp":1501774079064}]}"
QJsonParseError parseError;
QJsonDocument responseJson = QJsonDocument::fromJson(message.payload(), &parseError);
if (parseError.error != QJsonParseError::NoError) {
qWarning() << "QMQTT JSON parsing error" << parseError.errorString();
return;
}
QJsonObject rootObject = responseJson.object();
const QJsonArray& trafficArray = rootObject["traffic"].toArray();
for (int i = 0; i < trafficArray.count(); i++) {
const QJsonObject& trafficObject = trafficArray[i].toObject();
QString traffic_id = trafficObject["id"].toString();
float altitude = trafficObject["altitude"].toString().toFloat() * 0.3048f; // feet to meters
double lat = trafficObject["latitude"].toString().toDouble();
double lon = trafficObject["longitude"].toString().toDouble();
float heading = trafficObject["true_heading"].toString().toDouble(); // in deg
QString vehicle_id = trafficObject["properties"].toObject()["aircraft_id"].toString();
emit trafficUpdate(traffic_id, vehicle_id, QGeoCoordinate(lat, lon, altitude), heading);
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();
}
}
AirMapManagerPerVehicle::AirMapManagerPerVehicle(AirMapNetworking::SharedData& sharedData, const Vehicle& vehicle, AirMapManagerPerVehicle::AirMapManagerPerVehicle(AirMapSharedState& shared, const Vehicle& vehicle,
QGCToolbox& toolbox) QGCToolbox& toolbox)
: AirspaceManagerPerVehicle(vehicle), _networking(sharedData), : AirspaceManagerPerVehicle(vehicle), _shared(shared),
_flightManager(sharedData), _telemetry(sharedData), _trafficAlerts("mqtt-prod.airmap.io", 8883), _flightManager(shared), _telemetry(shared), _trafficMonitor(shared),
_toolbox(toolbox) _toolbox(toolbox)
{ {
connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged, connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged,
this, &AirMapManagerPerVehicle::flightPermitStatusChanged); this, &AirMapManagerPerVehicle::flightPermitStatusChanged);
connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged, connect(&_flightManager, &AirMapFlightManager::flightPermitStatusChanged,
this, &AirMapManagerPerVehicle::_flightPermitStatusChanged); this, &AirMapManagerPerVehicle::_flightPermitStatusChanged);
connect(&_flightManager, &AirMapFlightManager::networkError, this, &AirMapManagerPerVehicle::networkError); //connect(&_flightManager, &AirMapFlightManager::networkError, this, &AirMapManagerPerVehicle::networkError);
connect(&_telemetry, &AirMapTelemetry::networkError, this, &AirMapManagerPerVehicle::networkError); //connect(&_telemetry, &AirMapTelemetry::networkError, this, &AirMapManagerPerVehicle::networkError);
connect(&_trafficAlerts, &AirMapTrafficAlertClient::trafficUpdate, this, &AirspaceManagerPerVehicle::trafficUpdate); connect(&_trafficMonitor, &AirMapTrafficMonitor::trafficUpdate, this, &AirspaceManagerPerVehicle::trafficUpdate);
settingsChanged();
} }
void AirMapManagerPerVehicle::createFlight(const QList<MissionItem*>& missionItems) void AirMapManagerPerVehicle::createFlight(const QList<MissionItem*>& missionItems)
{ {
if (!_networking.hasAPIKey()) { if (!_shared.client()) {
qCDebug(AirMapManagerLog) << "API key not set. Will not create a flight"; qCDebug(AirMapManagerLog) << "No AirMap client instance. Will not create a flight";
return; return;
} }
_flightManager.createFlight(missionItems); _flightManager.createFlight(missionItems);
...@@ -1232,20 +807,7 @@ bool AirMapManagerPerVehicle::isTelemetryStreaming() const ...@@ -1232,20 +807,7 @@ bool AirMapManagerPerVehicle::isTelemetryStreaming() const
void AirMapManagerPerVehicle::endFlight() void AirMapManagerPerVehicle::endFlight()
{ {
_flightManager.endFlight(); _flightManager.endFlight();
_trafficAlerts.disconnectFromHost(); _trafficMonitor.stop();
}
void AirMapManagerPerVehicle::settingsChanged()
{
AirMapSettings* ap = _toolbox.settingsManager()->airMapSettings();
_flightManager.setSitaPilotRegistrationId(ap->sitaUserReg()->rawValueString());
_flightManager.setSitaUavRegistrationId(ap->sitaUavReg()->rawValueString());
// reset the states
_flightManager.abort();
_flightManager.endFlight();
_telemetry.stopTelemetryStream();
_trafficAlerts.disconnectFromHost();
} }
void AirMapManagerPerVehicle::vehicleMavlinkMessageReceived(const mavlink_message_t& message) void AirMapManagerPerVehicle::vehicleMavlinkMessageReceived(const mavlink_message_t& message)
...@@ -1261,22 +823,34 @@ void AirMapManagerPerVehicle::_flightPermitStatusChanged() ...@@ -1261,22 +823,34 @@ void AirMapManagerPerVehicle::_flightPermitStatusChanged()
if (_flightManager.flightPermitStatus() == AirspaceAuthorization::PermitAccepted) { if (_flightManager.flightPermitStatus() == AirspaceAuthorization::PermitAccepted) {
qCDebug(AirMapManagerLog) << "Subscribing to Traffic Alerts"; qCDebug(AirMapManagerLog) << "Subscribing to Traffic Alerts";
// since we already created the flight, we know that we have a valid login token // since we already created the flight, we know that we have a valid login token
_trafficAlerts.startConnection(_flightManager.flightID(), _networking.JWTLoginToken()); _trafficMonitor.startConnection(_flightManager.flightID());
} }
} }
AirMapManager::AirMapManager(QGCApplication* app, QGCToolbox* toolbox) AirMapManager::AirMapManager(QGCApplication* app, QGCToolbox* toolbox)
: AirspaceManager(app, toolbox) : AirspaceManager(app, toolbox)
{ {
GOOGLE_PROTOBUF_VERIFY_VERSION; _logger = std::make_shared<qt::Logger>();
qt::register_types(); // TODO: still needed?
_logger->logging_category().setEnabled(QtDebugMsg, true);
_logger->logging_category().setEnabled(QtInfoMsg, true);
_logger->logging_category().setEnabled(QtWarningMsg, true);
_dispatchingLogger = std::make_shared<qt::DispatchingLogger>(_logger);
connect(&_shared, &AirMapSharedState::error, this, &AirMapManager::_error);
}
AirMapManager::~AirMapManager()
{
if (_shared.client()) {
delete _shared.client();
}
} }
void AirMapManager::setToolbox(QGCToolbox* toolbox) void AirMapManager::setToolbox(QGCToolbox* toolbox)
{ {
AirspaceManager::setToolbox(toolbox); AirspaceManager::setToolbox(toolbox);
AirMapSettings* ap = toolbox->settingsManager()->airMapSettings(); AirMapSettings* ap = toolbox->settingsManager()->airMapSettings();
_networkingData.airmapAPIKey = ap->apiKey()->rawValueString();
_networkingData.login.setCredentials(ap->clientID()->rawValueString(), ap->userName()->rawValueString(), ap->password()->rawValueString());
connect(ap->apiKey(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); connect(ap->apiKey(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged);
connect(ap->clientID(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); connect(ap->clientID(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged);
...@@ -1285,6 +859,16 @@ void AirMapManager::setToolbox(QGCToolbox* toolbox) ...@@ -1285,6 +859,16 @@ void AirMapManager::setToolbox(QGCToolbox* toolbox)
connect(ap->sitaUserReg(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); connect(ap->sitaUserReg(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged);
connect(ap->sitaUavReg(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged); connect(ap->sitaUavReg(), &Fact::rawValueChanged, this, &AirMapManager::_settingsChanged);
_settingsChanged();
}
void AirMapManager::_error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails)
{
//TODO: console message & UI message
//qgcApp()->showMessage(QString("AirMap error: %1%2").arg(errorString).arg(errorDetails));
qCDebug(AirMapManagerLog) << "Caught error: "<<what<<", "<<airmapdMessage<<", "<<airmapdDetails;
} }
void AirMapManager::_settingsChanged() void AirMapManager::_settingsChanged()
...@@ -1292,40 +876,53 @@ void AirMapManager::_settingsChanged() ...@@ -1292,40 +876,53 @@ void AirMapManager::_settingsChanged()
qCDebug(AirMapManagerLog) << "AirMap settings changed"; qCDebug(AirMapManagerLog) << "AirMap settings changed";
AirMapSettings* ap = _toolbox->settingsManager()->airMapSettings(); AirMapSettings* ap = _toolbox->settingsManager()->airMapSettings();
_networkingData.airmapAPIKey = ap->apiKey()->rawValueString();
_networkingData.login.setCredentials(ap->clientID()->rawValueString(), ap->userName()->rawValueString(), ap->password()->rawValueString());
emit settingsChanged(); AirMapSharedState::Settings settings;
} settings.apiKey = ap->apiKey()->rawValueString();
bool apiKeyChanged = settings.apiKey != _shared.settings().apiKey;
settings.clientID = ap->clientID()->rawValueString();
settings.userName = ap->userName()->rawValueString();
settings.password = ap->password()->rawValueString();
_shared.setSettings(settings);
void AirMapManager::_networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage) // need to re-create the client if the API key changed
{ if (_shared.client() && apiKeyChanged) {
QString errorDetails = ""; delete _shared.client();
if (code == QNetworkReply::NetworkError::ContentOperationNotPermittedError) { _shared.setClient(nullptr);
errorDetails = " (invalid API key?)";
} else if (code == QNetworkReply::NetworkError::AuthenticationRequiredError) {
errorDetails = " (authentication failure)";
} }
if (serverErrorMessage.length() > 0) {
// the errorString is a bit verbose and redundant with serverErrorMessage. So we just show the server error.
qgcApp()->showMessage(QString("AirMap error%1. Response from Server: %2").arg(errorDetails).arg(serverErrorMessage)); if (!_shared.client() && settings.apiKey != "") {
} else { qCDebug(AirMapManagerLog) << "Creating AirMap client";
qgcApp()->showMessage(QString("AirMap error: %1%2").arg(errorString).arg(errorDetails));
auto credentials = Credentials{};
credentials.api_key = _shared.settings().apiKey.toStdString();
auto configuration = Client::default_staging_configuration(credentials);
qt::Client::create(configuration, _dispatchingLogger, this, [this, ap](const qt::Client::CreateResult& result) {
if (result) {
qCDebug(AirMapManagerLog) << "Successfully created airmap::qt::Client instance";
_shared.setClient(result.value());
} else {
qWarning("Failed to create airmap::qt::Client instance");
// TODO: user error message
}
});
} }
} }
AirspaceManagerPerVehicle* AirMapManager::instantiateVehicle(const Vehicle& vehicle) AirspaceManagerPerVehicle* AirMapManager::instantiateVehicle(const Vehicle& vehicle)
{ {
AirMapManagerPerVehicle* manager = new AirMapManagerPerVehicle(_networkingData, vehicle, *_toolbox); AirMapManagerPerVehicle* manager = new AirMapManagerPerVehicle(_shared, vehicle, *_toolbox);
connect(manager, &AirMapManagerPerVehicle::networkError, this, &AirMapManager::_networkError); //connect(manager, &AirMapManagerPerVehicle::networkError, this, &AirMapManager::_networkError);
connect(this, &AirMapManager::settingsChanged, manager, &AirMapManagerPerVehicle::settingsChanged);
return manager; return manager;
} }
AirspaceRestrictionProvider* AirMapManager::instantiateRestrictionProvider() AirspaceRestrictionProvider* AirMapManager::instantiateRestrictionProvider()
{ {
AirMapRestrictionManager* restrictionManager = new AirMapRestrictionManager(_networkingData); AirMapRestrictionManager* restrictionManager = new AirMapRestrictionManager(_shared);
connect(restrictionManager, &AirMapRestrictionManager::networkError, this, &AirMapManager::_networkError); //connect(restrictionManager, &AirMapRestrictionManager::networkError, this, &AirMapManager::_networkError);
connect(restrictionManager, &AirMapRestrictionManager::error, this, &AirMapManager::_error);
return restrictionManager; return restrictionManager;
} }
...@@ -7,8 +7,7 @@ ...@@ -7,8 +7,7 @@
* *
****************************************************************************/ ****************************************************************************/
#ifndef AirMapManager_H #pragma once
#define AirMapManager_H
#include "QGCToolbox.h" #include "QGCToolbox.h"
#include "QGCLoggingCategory.h" #include "QGCLoggingCategory.h"
...@@ -17,146 +16,82 @@ ...@@ -17,146 +16,82 @@
#include "MultiVehicleManager.h" #include "MultiVehicleManager.h"
#include "AirspaceManagement.h" #include "AirspaceManagement.h"
#include <qmqtt.h>
#include <QGeoCoordinate> #include <QGeoCoordinate>
#include <QList> #include <QList>
#include <QNetworkAccessManager> #include <QQueue>
#include <QNetworkReply>
#include <QTimer> #include <QTimer>
#include <QUdpSocket>
#include <QHostInfo>
#include <QHostAddress>
#include <stdint.h>
Q_DECLARE_LOGGING_CATEGORY(AirMapManagerLog)
class AirMapLogin : public QObject
{
Q_OBJECT
public:
/**
* @param networkManager
* @param APIKey AirMap API key: this is stored as a reference, and thus must live as long as this object does
*/
AirMapLogin(QNetworkAccessManager& networkManager, const QString& APIKey);
void setCredentials(const QString& clientID, const QString& userName, const QString& password);
/**
* check if the credentials are set (not necessarily valid)
*/
bool hasCredentials() const { return _userName != "" && _password != ""; }
void login();
void logout() { _JWTToken = ""; }
/** get the JWT token. Empty if user not logged in */
const QString& JWTToken() const { return _JWTToken; }
bool isLoggedIn() const { return _JWTToken != ""; } #include <cstdint>
#include <functional>
#include <memory>
signals: #include <airmap/qt/client.h>
void loginSuccess(); #include <airmap/qt/logger.h>
void loginFailure(QNetworkReply::NetworkError error, const QString& errorString, const QString& serverErrorMessage); #include <airmap/qt/types.h>
#include <airmap/traffic.h>
private slots:
void _requestFinished(void);
void _requestError(QNetworkReply::NetworkError code);
private:
void _post(QUrl url, const QByteArray& postData);
QNetworkAccessManager& _networkManager; Q_DECLARE_LOGGING_CATEGORY(AirMapManagerLog)
bool _isLoginInProgress = false;
QString _JWTToken = ""; ///< JWT login token: empty when not logged in
const QString& _APIKey;
// login credentials
QString _clientID;
QString _userName;
QString _password;
};
/** /**
* @class AirMapNetworking * @class AirMapSharedState
* Handles networking requests (GET & POST), with login if required. * contains state & settings that need to be shared (such as login)
* There can only be one active request per object instance.
*/ */
class AirMapNetworking : public QObject class AirMapSharedState : public QObject
{ {
Q_OBJECT Q_OBJECT
public: public:
struct Settings {
QString apiKey;
struct SharedData { // login credentials
SharedData() : login(networkManager, airmapAPIKey) {} QString clientID;
QString userName; ///< use anonymous login if empty
QNetworkAccessManager networkManager; QString password;
QString airmapAPIKey;
AirMapLogin login;
}; };
AirMapNetworking(SharedData& networkingData); void setSettings(const Settings& settings);
const Settings& settings() const { return _settings; }
/** void setClient(airmap::qt::Client* client) { _client = client; }
* send a GET request
* @param url
* @param requiresLogin set to true if the user needs to be logged in for the request
*/
void get(QUrl url, bool requiresLogin = false);
/** /**
* send a POST request * Get the current client instance. It can be NULL. If not NULL, it implies
* @param url * there's an API key set.
* @param postData
* @param isJsonData if true, content type is set to JSON, form data otherwise
* @param requiresLogin set to true if the user needs to be logged in for the request
*/ */
void post(QUrl url, const QByteArray& postData, bool isJsonData = false, bool requiresLogin = false); airmap::qt::Client* client() const { return _client; }
bool hasAPIKey() const { return _settings.apiKey != ""; }
const QString& JWTLoginToken() const { return _networkingData.login.JWTToken(); } bool isLoggedIn() const { return _loginToken != ""; }
const AirMapLogin& getLogin() const { return _networkingData.login; } using Callback = std::function<void(const QString& /* login_token */)>;
/** /**
* abort the current request (_requestFinished() or _requestError() will not be emitted) * 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 abort(); void doRequestWithLogin(const Callback& callback);
bool hasAPIKey() const { return _networkingData.airmapAPIKey != ""; } void login();
void logout();
const QString& loginToken() const { return _loginToken; }
signals: signals:
/// signal when the request finished (get or post). All requests are assumed to return JSON. void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
void finished(QJsonParseError parseError, QJsonDocument document);
void error(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage);
private slots:
void _loginSuccess();
void _loginFailure(QNetworkReply::NetworkError networkError, const QString& errorString, const QString& serverErrorMessage);
void _requestFinished(void);
private: private:
SharedData& _networkingData; void _processPendingRequests();
enum class RequestType { bool _isLoginInProgress = false;
None, QString _loginToken; ///< login token: empty when not logged in
GET,
POST airmap::qt::Client* _client = nullptr;
};
struct PendingRequest { Settings _settings;
RequestType type = RequestType::None;
QUrl url;
QByteArray postData;
bool isJsonData;
bool requiresLogin;
};
PendingRequest _pendingRequest;
QNetworkReply* _currentNetworkReply = nullptr; QQueue<Callback> _pendingRequests; ///< pending requests that are processed after a successful login
}; };
...@@ -165,26 +100,22 @@ class AirMapRestrictionManager : public AirspaceRestrictionProvider ...@@ -165,26 +100,22 @@ class AirMapRestrictionManager : public AirspaceRestrictionProvider
{ {
Q_OBJECT Q_OBJECT
public: public:
AirMapRestrictionManager(AirMapNetworking::SharedData& sharedData); AirMapRestrictionManager(AirMapSharedState& shared);
void setROI(const QGeoCoordinate& center, double radiusMeters) override; void setROI(const QGeoCoordinate& center, double radiusMeters) override;
signals: signals:
void networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage); void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
private slots:
void _parseAirspaceJson(QJsonParseError parseError, QJsonDocument airspaceDoc);
void _error(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage);
private: private:
enum class State { enum class State {
Idle, Idle,
RetrieveList,
RetrieveItems, RetrieveItems,
}; };
State _state = State::Idle; State _state = State::Idle;
int _numAwaitingItems = 0; AirMapSharedState& _shared;
AirMapNetworking _networking;
}; };
...@@ -193,7 +124,7 @@ class AirMapFlightManager : public QObject ...@@ -193,7 +124,7 @@ class AirMapFlightManager : public QObject
{ {
Q_OBJECT Q_OBJECT
public: public:
AirMapFlightManager(AirMapNetworking::SharedData& sharedData); AirMapFlightManager(AirMapSharedState& shared);
/// Send flight path to AirMap /// Send flight path to AirMap
void createFlight(const QList<MissionItem*>& missionItems); void createFlight(const QList<MissionItem*>& missionItems);
...@@ -202,30 +133,16 @@ public: ...@@ -202,30 +133,16 @@ public:
const QString& flightID() const { return _currentFlightId; } const QString& flightID() const { return _currentFlightId; }
void setSitaUavRegistrationId(const QString& sitaUavRegistrationId) {
_sitaUavRegistrationId = sitaUavRegistrationId;
}
void setSitaPilotRegistrationId(const QString& sitaPilotRegistrationId) {
_sitaPilotRegistrationId = sitaPilotRegistrationId;
}
/**
* abort the current operation
*/
void abort();
public slots: public slots:
void endFlight(); void endFlight();
signals: signals:
void networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage); void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
void flightPermitStatusChanged(); void flightPermitStatusChanged();
private slots: private slots:
void _parseJson(QJsonParseError parseError, QJsonDocument doc); void _pollBriefing();
void _error(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage);
void _sendBriefingRequest();
private: private:
/** /**
...@@ -233,11 +150,23 @@ private: ...@@ -233,11 +150,23 @@ private:
*/ */
void _uploadFlight(); 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() * implementation of endFlight()
*/ */
void _endFlight(const QString& flightID); 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 { enum class State {
Idle, Idle,
GetPilotID, GetPilotID,
...@@ -261,7 +190,7 @@ private: ...@@ -261,7 +190,7 @@ private:
Flight _flight; ///< flight pending to be uploaded Flight _flight; ///< flight pending to be uploaded
State _state = State::Idle; State _state = State::Idle;
AirMapNetworking _networking; AirMapSharedState& _shared;
QString _currentFlightId; ///< Flight ID, empty if there is none 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 _pendingFlightId; ///< current flight ID, not necessarily accepted yet (once accepted, it's equal to _currentFlightId)
QString _pendingFlightPlan; ///< current flight plan, waiting to be submitted QString _pendingFlightPlan; ///< current flight plan, waiting to be submitted
...@@ -269,9 +198,6 @@ private: ...@@ -269,9 +198,6 @@ private:
QString _pilotID; ///< Pilot ID in the form "auth0|abc123" QString _pilotID; ///< Pilot ID in the form "auth0|abc123"
bool _noFlightCreatedYet = true; bool _noFlightCreatedYet = true;
QTimer _pollTimer; ///< timer to poll for approval check QTimer _pollTimer; ///< timer to poll for approval check
QString _sitaUavRegistrationId;
QString _sitaPilotRegistrationId;
}; };
/// class to send telemetry data to AirMap /// class to send telemetry data to AirMap
...@@ -279,8 +205,8 @@ class AirMapTelemetry : public QObject ...@@ -279,8 +205,8 @@ class AirMapTelemetry : public QObject
{ {
Q_OBJECT Q_OBJECT
public: public:
AirMapTelemetry(AirMapNetworking::SharedData& sharedData); AirMapTelemetry(AirMapSharedState& shared);
virtual ~AirMapTelemetry(); virtual ~AirMapTelemetry() = default;
/** /**
* Setup the connection to start sending telemetry * Setup the connection to start sending telemetry
...@@ -292,16 +218,11 @@ public: ...@@ -292,16 +218,11 @@ public:
bool isTelemetryStreaming() const; bool isTelemetryStreaming() const;
signals: signals:
void networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage); void error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
public slots: public slots:
void vehicleMavlinkMessageReceived(const mavlink_message_t& message); void vehicleMavlinkMessageReceived(const mavlink_message_t& message);
private slots:
void _parseJson(QJsonParseError parseError, QJsonDocument doc);
void _error(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage);
void _udpTelemetryHostLookup(QHostInfo info);
private: private:
void _handleGlobalPositionInt(const mavlink_message_t& message); void _handleGlobalPositionInt(const mavlink_message_t& message);
...@@ -316,49 +237,39 @@ private: ...@@ -316,49 +237,39 @@ private:
State _state = State::Idle; State _state = State::Idle;
AirMapNetworking _networking; AirMapSharedState& _shared;
QByteArray _key; ///< key for AES encryption, 16 bytes std::string _key; ///< key for AES encryption (16 bytes)
QString _flightID; QString _flightID;
uint32_t _seqNum = 1;
QUdpSocket* _socket = nullptr;
QHostAddress _udpHost;
static constexpr int _udpPort = 32003;
float _lastHdop = 1.f; float _lastHdop = 1.f;
}; };
class AirMapTrafficAlertClient : public QMQTT::Client class AirMapTrafficMonitor : public QObject
{ {
Q_OBJECT Q_OBJECT
public: public:
AirMapTrafficAlertClient(const QString& host, const quint16 port, QObject* parent = NULL) AirMapTrafficMonitor(AirMapSharedState& shared)
: QMQTT::Client(host, port, QSslConfiguration::defaultConfiguration(), true, parent) : _shared(shared)
{ {
connect(this, &AirMapTrafficAlertClient::connected, this, &AirMapTrafficAlertClient::onConnected);
connect(this, &AirMapTrafficAlertClient::subscribed, this, &AirMapTrafficAlertClient::onSubscribed);
connect(this, &AirMapTrafficAlertClient::received, this, &AirMapTrafficAlertClient::onReceived);
connect(this, &AirMapTrafficAlertClient::error, this, &AirMapTrafficAlertClient::onError);
} }
virtual ~AirMapTrafficAlertClient() = default; virtual ~AirMapTrafficMonitor();
void startConnection(const QString& flightID);
void startConnection(const QString& flightID, const QString& password); void stop();
signals: signals:
void trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading); void trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading);
private slots: private:
void _update(airmap::Traffic::Update::Type type, const std::vector<airmap::Traffic::Update>& update);
void onError(const QMQTT::ClientError error);
void onConnected();
void onSubscribed(const QString& topic);
void onReceived(const QMQTT::Message& message);
private: private:
QString _flightID; QString _flightID;
AirMapSharedState& _shared;
std::shared_ptr<airmap::Traffic::Monitor> _monitor;
std::shared_ptr<airmap::Traffic::Monitor::Subscriber> _subscriber;
}; };
...@@ -368,7 +279,7 @@ class AirMapManagerPerVehicle : public AirspaceManagerPerVehicle ...@@ -368,7 +279,7 @@ class AirMapManagerPerVehicle : public AirspaceManagerPerVehicle
{ {
Q_OBJECT Q_OBJECT
public: public:
AirMapManagerPerVehicle(AirMapNetworking::SharedData& sharedData, const Vehicle& vehicle, QGCToolbox& toolbox); AirMapManagerPerVehicle(AirMapSharedState& shared, const Vehicle& vehicle, QGCToolbox& toolbox);
virtual ~AirMapManagerPerVehicle() = default; virtual ~AirMapManagerPerVehicle() = default;
...@@ -388,18 +299,16 @@ signals: ...@@ -388,18 +299,16 @@ signals:
public slots: public slots:
void endFlight() override; void endFlight() override;
void settingsChanged();
protected slots: protected slots:
virtual void vehicleMavlinkMessageReceived(const mavlink_message_t& message) override; virtual void vehicleMavlinkMessageReceived(const mavlink_message_t& message) override;
private slots: private slots:
void _flightPermitStatusChanged(); void _flightPermitStatusChanged();
private: private:
AirMapNetworking _networking; AirMapSharedState& _shared;
AirMapFlightManager _flightManager; AirMapFlightManager _flightManager;
AirMapTelemetry _telemetry; AirMapTelemetry _telemetry;
AirMapTrafficAlertClient _trafficAlerts; AirMapTrafficMonitor _trafficMonitor;
QGCToolbox& _toolbox; QGCToolbox& _toolbox;
}; };
...@@ -411,7 +320,7 @@ class AirMapManager : public AirspaceManager ...@@ -411,7 +320,7 @@ class AirMapManager : public AirspaceManager
public: public:
AirMapManager(QGCApplication* app, QGCToolbox* toolbox); AirMapManager(QGCApplication* app, QGCToolbox* toolbox);
virtual ~AirMapManager() = default; virtual ~AirMapManager();
void setToolbox(QGCToolbox* toolbox) override; void setToolbox(QGCToolbox* toolbox) override;
...@@ -421,16 +330,16 @@ public: ...@@ -421,16 +330,16 @@ public:
QString name() const override { return "AirMap"; } QString name() const override { return "AirMap"; }
signals:
void settingsChanged();
private slots: private slots:
void _networkError(QNetworkReply::NetworkError code, const QString& errorString, const QString& serverErrorMessage); void _error(const QString& what, const QString& airmapdMessage, const QString& airmapdDetails);
void _settingsChanged(); void _settingsChanged();
private: private:
AirMapNetworking::SharedData _networkingData; AirMapSharedState _shared;
std::shared_ptr<airmap::qt::Logger> _logger;
std::shared_ptr<airmap::qt::DispatchingLogger> _dispatchingLogger;
}; };
#endif
/airmap_telemetry.pb.*
// protoc -I=. --cpp_out=. airmap_telemetry.proto
syntax = "proto2";
package airmap.telemetry;
message Position {
// UNIX time in Milliseconds
required uint64 timestamp = 1;
// The recorded latitude
// Decimal place requirement: 7 decimal places.
required double latitude = 2;
// The recorded longitude
// Decimal place requirement: 7 decimal places.
required double longitude = 3;
//Altitude above mean sea level (ie. GPS), meters
required float altitude_agl = 4;
// Altitude above ground level, meters
required float altitude_msl = 5;
// Horizontal Dilution of Precision, in meters
required float horizontal_accuracy = 6;
}
message Attitude {
// UNIX time in Milliseconds
required uint64 timestamp = 1;
// Yaw angle measured from True North, { 0 <= x < 360 } degrees
required float yaw = 2;
// Pitch angle, { -180 < x <= 180 } degrees
required float pitch = 3;
// Roll angle, { -180 < x <= 180 } degrees
required float roll = 4;
}
message Speed {
// UNIX time in Milliseconds
required uint64 timestamp = 1;
// Aircraft Speed in the x direction in meters per second using the North-East-Down (N-E-D) coordinate system
required float velocity_x = 2;
// Aircraft Speed in the y direction in meters per second using the North-East-Down (N-E-D) coordinate system
required float velocity_y = 3;
// Aircraft Speed in the z direction in meters per second using the North-East-Down (N-E-D) coordinate system
required float velocity_z = 4;
}
message Barometer {
// UNIX time in Milliseconds
required uint64 timestamp = 1;
// Barometric pressure in hPa
required float pressure = 2;
}
# Qt qmake integration with Google Protocol Buffers compiler protoc
#
# To compile protocol buffers with qt qmake, specify PROTOS variable and
# include this file
#
# Example:
# BITSIZE = $$system(getconf LONG_BIT)
# if (contains(BITSIZE, 64)) {
# LIBS += /usr/lib/x86_64-linux-gnu/libprotobuf.so
# }
# if (contains(BITSIZE, 32)) {
# LIBS += /usr/lib/libprotobuf.so
# }
# PROTOS = a.proto b.proto
# include(protobuf.pri)
#
# By default protoc looks for .proto files (including the imported ones) in
# the current directory where protoc is run. If you need to include additional
# paths specify the PROTOPATH variable
message("Generating protocol buffer classes from .proto files.")
INCLUDEPATH += $$PWD
DEPENDPATH += $$PWD
protobuf_decl.name = protobuf headers
protobuf_decl.input = PROTOS
protobuf_decl.output = ${QMAKE_FILE_IN_PATH}/${QMAKE_FILE_BASE}.pb.h
protobuf_decl.commands = protoc --cpp_out=${QMAKE_FILE_IN_PATH} --proto_path=${QMAKE_FILE_IN_PATH} ${QMAKE_FILE_NAME}
protobuf_decl.variable_out = HEADERS
QMAKE_EXTRA_COMPILERS += protobuf_decl
protobuf_impl.name = protobuf sources
protobuf_impl.input = PROTOS
protobuf_impl.output = ${QMAKE_FILE_IN_PATH}/${QMAKE_FILE_BASE}.pb.cc
protobuf_impl.depends = ${QMAKE_FILE_IN_PATH}/${QMAKE_FILE_BASE}.pb.h
protobuf_impl.commands = $$escape_expand(\n)
protobuf_impl.variable_out = SOURCES
QMAKE_EXTRA_COMPILERS += protobuf_impl
Markdown is supported
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