From cd88c1e6ee365af3681bd1573fc38fab18f744df Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Tue, 17 Jan 2023 21:27:42 +0100 Subject: [PATCH] Remote ID: create remote ID manager and settings --- qgroundcontrol.pro | 4 + src/Settings/RemoteID.SettingsGroup.json | 169 ++++++++++++ src/Settings/RemoteIDSettings.cc | 40 +++ src/Settings/RemoteIDSettings.h | 42 +++ src/Settings/SettingsManager.cc | 2 + src/Settings/SettingsManager.h | 4 + src/Vehicle/RemoteIDManager.cc | 430 +++++++++++++++++++++++++++++++ src/Vehicle/RemoteIDManager.h | 128 +++++++++ 8 files changed, 819 insertions(+) create mode 100644 src/Settings/RemoteID.SettingsGroup.json create mode 100644 src/Settings/RemoteIDSettings.cc create mode 100644 src/Settings/RemoteIDSettings.h create mode 100644 src/Vehicle/RemoteIDManager.cc create mode 100644 src/Vehicle/RemoteIDManager.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index afe1232..d18f22b 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -677,6 +677,7 @@ HEADERS += \ src/Settings/AppSettings.h \ src/Settings/AutoConnectSettings.h \ src/Settings/BrandImageSettings.h \ + src/Settings/RemoteIDSettings.h \ src/Settings/FirmwareUpgradeSettings.h \ src/Settings/FlightMapSettings.h \ src/Settings/FlyViewSettings.h \ @@ -715,6 +716,7 @@ HEADERS += \ src/Vehicle/MAVLinkLogManager.h \ src/Vehicle/MAVLinkStreamConfig.h \ src/Vehicle/MultiVehicleManager.h \ + src/Vehicle/RemoteIDManager.h \ src/Vehicle/StateMachine.h \ src/Vehicle/SysStatusSensorInfo.h \ src/Vehicle/TerrainFactGroup.h \ @@ -932,6 +934,7 @@ SOURCES += \ src/Settings/AppSettings.cc \ src/Settings/AutoConnectSettings.cc \ src/Settings/BrandImageSettings.cc \ + src/Settings/RemoteIDSettings.cc \ src/Settings/FirmwareUpgradeSettings.cc \ src/Settings/FlightMapSettings.cc \ src/Settings/FlyViewSettings.cc \ @@ -970,6 +973,7 @@ SOURCES += \ src/Vehicle/MAVLinkLogManager.cc \ src/Vehicle/MAVLinkStreamConfig.cc \ src/Vehicle/MultiVehicleManager.cc \ + src/Vehicle/RemoteIDManager.cc \ src/Vehicle/StateMachine.cc \ src/Vehicle/SysStatusSensorInfo.cc \ src/Vehicle/TerrainFactGroup.cc \ diff --git a/src/Settings/RemoteID.SettingsGroup.json b/src/Settings/RemoteID.SettingsGroup.json new file mode 100644 index 0000000..4e23486 --- /dev/null +++ b/src/Settings/RemoteID.SettingsGroup.json @@ -0,0 +1,169 @@ +{ + "version": 1, + "fileType": "FactMetaData", + "QGC.MetaData.Facts": +[ +{ + "name": "enable", + "shortDesc": "Show Drone ID settings page when enabled", + "type": "bool", + "default": false +}, +{ + "name": "operatorID", + "shortDesc": "Operator ID", + "longDesc": "Operator ID. Maximum 20 characters.", + "type": "string", + "default": "" +}, +{ + "name": "operatorIDType", + "shortDesc": "Operator ID type", + "type": "uint8", + "enumStrings": "CAA", + "enumValues": "0", + "default": 0 +}, +{ + "name": "sendOperatorID", + "shortDesc": "Send Operator ID", + "longDesc": "When enabled, sends operator ID message", + "type": "bool", + "default": false +}, +{ + "name": "selfIDFree", + "shortDesc": "Self ID", + "longDesc": "Optional plain text for operator to specify operations data (Free Text). Maximum 23 characters.", + "type": "string", + "default": "" +}, +{ + "name": "selfIDEmergency", + "shortDesc": "Self ID", + "longDesc": "Optional plain text for operator to specify operations data (Emergency Text). Maximum 23 characters.", + "type": "string", + "default": "Pilot Emergency Status" +}, +{ + "name": "selfIDExtended", + "shortDesc": "Self ID", + "longDesc": "Optional plain text for operator to specify operations data (Extended Text). Maximum 23 characters.", + "type": "string", + "default": "" +}, +{ + "name": "selfIDType", + "shortDesc": "Self ID type", + "type": "uint8", + "enumStrings": "Free Text,Emergency,Extended Status", + "enumValues": "0,1,2", + "default": 0 +}, +{ + "name": "sendSelfID", + "shortDesc": "Send Self ID", + "longDesc": "When enabled, sends self ID message", + "type": "bool", + "default": false +}, +{ + "name": "basicID", + "shortDesc": "Basic ID", + "type": "string", + "default": "" +}, +{ + "name": "basicIDType", + "shortDesc": "Basic ID Type", + "type": "uint8", + "enumStrings": "None, SerialNumber(ANSI/CTA-2063), CAA, UTM(RFC4122), Specific", + "enumValues": "0,1,2,3,4", + "default": 2 +}, +{ + "name": "basicIDUaType", + "shortDesc": "UA type", + "type": "uint8", + "enumStrings": "Undefined,Airplane/FixedWing,Helicopter/Multirrotor, Gyroplane, VTOL, Ornithopter, Glider, Kite, Free Ballon, Captive Ballon, Airship, Parachute, Rocket, Tethered powered aircraft, Ground Obstacle, Other", + "enumValues": "0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15", + "default": 0 +}, +{ + "name": "sendBasicID", + "shortDesc": "Send Basic ID", + "longDesc": "When enabled, sends basic ID message", + "type": "bool", + "default": false +}, +{ + "name": "region", + "shortDesc": "Region of operation", + "longDesc": "The region of operation the mission will take place in", + "type": "uint8", + "enumStrings": "FAA,EU", + "enumValues": "0,1", + "default": 0 +}, +{ + "name": "locationType", + "shortDesc": "Location Type", + "longDesc": "Operator location Type", + "type": "uint8", + "enumStrings": "Takeoff(Not Supported),Live GNNS, Fixed", + "enumValues": "0,1,2", + "default": 1 +}, +{ + "name": "latitudeFixed", + "shortDesc": "Latitude Fixed", + "longDesc": "Fixed latitude to send on SYSTEM message", + "type": "double", + "decimalPlaces":7, + "default": 0 +}, +{ + "name": "longitudeFixed", + "shortDesc": "Longitude Fixed", + "longDesc": "Fixed Longitude to send on SYSTEM message", + "type": "double", + "decimalPlaces":7, + "default": 0 +}, +{ + "name": "altitudeFixed", + "shortDesc": "Altitude Fixed", + "longDesc": "Fixed Altitude to send on SYSTEM message", + "type": "double", + "decimalPlaces":7, + "default": 0 +}, +{ + "name": "classificationType", + "shortDesc": "Classification Type", + "longDesc": "Classification Type of UA", + "type": "uint8", + "enumStrings": "Undefined,EU", + "enumValues": "0,1", + "default": 0 +}, +{ + "name": "categoryEU", + "shortDesc": "Category EU", + "longDesc": "Category of the UAS in the EU region", + "type": "uint8", + "enumStrings": "Undeclared,Open, Specific, Certified", + "enumValues": "0,1,2,3", + "default": 0 +}, +{ + "name": "classEU", + "shortDesc": "Class EU", + "longDesc": "Class of the UAS in the EU region", + "type": "uint8", + "enumStrings": "Undeclared,Class 0, Class 1, Class 2, Class 3, Class 4, Class 5, Class 6", + "enumValues": "0,1,2,3,4,5,6,7", + "default": 0 +} +] +} \ No newline at end of file diff --git a/src/Settings/RemoteIDSettings.cc b/src/Settings/RemoteIDSettings.cc new file mode 100644 index 0000000..ef405b7 --- /dev/null +++ b/src/Settings/RemoteIDSettings.cc @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * (c) 2009-2022 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "RemoteIDSettings.h" + +#include +#include + +DECLARE_SETTINGGROUP(RemoteID, "RemoteID") +{ + qmlRegisterUncreatableType("QGroundControl.SettingsManager", 1, 0, "RemoteIDSettings", "Reference only"); \ +} + +DECLARE_SETTINGSFACT(RemoteIDSettings, enable) +DECLARE_SETTINGSFACT(RemoteIDSettings, operatorID) +DECLARE_SETTINGSFACT(RemoteIDSettings, operatorIDType) +DECLARE_SETTINGSFACT(RemoteIDSettings, sendOperatorID) +DECLARE_SETTINGSFACT(RemoteIDSettings, selfIDFree) +DECLARE_SETTINGSFACT(RemoteIDSettings, selfIDEmergency) +DECLARE_SETTINGSFACT(RemoteIDSettings, selfIDExtended) +DECLARE_SETTINGSFACT(RemoteIDSettings, selfIDType) +DECLARE_SETTINGSFACT(RemoteIDSettings, sendSelfID) +DECLARE_SETTINGSFACT(RemoteIDSettings, basicID) +DECLARE_SETTINGSFACT(RemoteIDSettings, basicIDType) +DECLARE_SETTINGSFACT(RemoteIDSettings, basicIDUaType) +DECLARE_SETTINGSFACT(RemoteIDSettings, sendBasicID) +DECLARE_SETTINGSFACT(RemoteIDSettings, region) +DECLARE_SETTINGSFACT(RemoteIDSettings, locationType) +DECLARE_SETTINGSFACT(RemoteIDSettings, latitudeFixed) +DECLARE_SETTINGSFACT(RemoteIDSettings, longitudeFixed) +DECLARE_SETTINGSFACT(RemoteIDSettings, altitudeFixed) +DECLARE_SETTINGSFACT(RemoteIDSettings, classificationType) +DECLARE_SETTINGSFACT(RemoteIDSettings, categoryEU) +DECLARE_SETTINGSFACT(RemoteIDSettings, classEU) \ No newline at end of file diff --git a/src/Settings/RemoteIDSettings.h b/src/Settings/RemoteIDSettings.h new file mode 100644 index 0000000..28e8d32 --- /dev/null +++ b/src/Settings/RemoteIDSettings.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * (c) 2009-2022 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "SettingsGroup.h" + +class RemoteIDSettings : public SettingsGroup +{ + Q_OBJECT +public: + RemoteIDSettings(QObject* parent = nullptr); + DEFINE_SETTING_NAME_GROUP() + + DEFINE_SETTINGFACT(enable) + DEFINE_SETTINGFACT(operatorID) + DEFINE_SETTINGFACT(operatorIDType) + DEFINE_SETTINGFACT(sendOperatorID) + DEFINE_SETTINGFACT(selfIDFree) + DEFINE_SETTINGFACT(selfIDEmergency) + DEFINE_SETTINGFACT(selfIDExtended) + DEFINE_SETTINGFACT(selfIDType) + DEFINE_SETTINGFACT(sendSelfID) + DEFINE_SETTINGFACT(basicID) + DEFINE_SETTINGFACT(basicIDType) + DEFINE_SETTINGFACT(basicIDUaType) + DEFINE_SETTINGFACT(sendBasicID) + DEFINE_SETTINGFACT(region) + DEFINE_SETTINGFACT(locationType) + DEFINE_SETTINGFACT(latitudeFixed) + DEFINE_SETTINGFACT(longitudeFixed) + DEFINE_SETTINGFACT(altitudeFixed) + DEFINE_SETTINGFACT(classificationType) + DEFINE_SETTINGFACT(categoryEU) + DEFINE_SETTINGFACT(classEU) +}; \ No newline at end of file diff --git a/src/Settings/SettingsManager.cc b/src/Settings/SettingsManager.cc index e0a1a4a..d75870d 100644 --- a/src/Settings/SettingsManager.cc +++ b/src/Settings/SettingsManager.cc @@ -32,6 +32,7 @@ SettingsManager::SettingsManager(QGCApplication* app, QGCToolbox* toolbox) #if !defined(NO_ARDUPILOT_DIALECT) , _apmMavlinkStreamRateSettings (nullptr) #endif + , _remoteIDSettings (nullptr) { } @@ -60,4 +61,5 @@ void SettingsManager::setToolbox(QGCToolbox *toolbox) #if defined(QGC_AIRMAP_ENABLED) _airMapSettings = new AirMapSettings (this); #endif + _remoteIDSettings = new RemoteIDSettings (this); } diff --git a/src/Settings/SettingsManager.h b/src/Settings/SettingsManager.h index dcaba86..4ac9a2f 100644 --- a/src/Settings/SettingsManager.h +++ b/src/Settings/SettingsManager.h @@ -31,6 +31,7 @@ #include "AirMapSettings.h" #endif #include +#include "RemoteIDSettings.h" /// Provides access to all app settings class SettingsManager : public QGCTool @@ -58,6 +59,7 @@ public: #if !defined(NO_ARDUPILOT_DIALECT) Q_PROPERTY(QObject* apmMavlinkStreamRateSettings READ apmMavlinkStreamRateSettings CONSTANT) #endif + Q_PROPERTY(QObject* remoteIDSettings READ remoteIDSettings CONSTANT) // Override from QGCTool virtual void setToolbox(QGCToolbox *toolbox); @@ -79,6 +81,7 @@ public: #if !defined(NO_ARDUPILOT_DIALECT) APMMavlinkStreamRateSettings* apmMavlinkStreamRateSettings(void) { return _apmMavlinkStreamRateSettings; } #endif + RemoteIDSettings* remoteIDSettings (void) { return _remoteIDSettings; } private: #if defined(QGC_AIRMAP_ENABLED) AirMapSettings* _airMapSettings; @@ -98,6 +101,7 @@ private: #if !defined(NO_ARDUPILOT_DIALECT) APMMavlinkStreamRateSettings* _apmMavlinkStreamRateSettings; #endif + RemoteIDSettings* _remoteIDSettings; }; #endif diff --git a/src/Vehicle/RemoteIDManager.cc b/src/Vehicle/RemoteIDManager.cc new file mode 100644 index 0000000..c0e9b7e --- /dev/null +++ b/src/Vehicle/RemoteIDManager.cc @@ -0,0 +1,430 @@ +/**************************************************************************** + * + * (c) 2009-2022 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "RemoteIDManager.h" +#include "QGCApplication.h" +#include "SettingsManager.h" +#include "RemoteIDSettings.h" +#include "QGCQGeoCoordinate.h" +#include "PositionManager.h" + +#include + +QGC_LOGGING_CATEGORY(RemoteIDManagerLog, "RemoteIDManagerLog") + +#define AREA_COUNT 1 +#define AREA_RADIUS 0 +#define SENDING_RATE_MSEC 1000 +#define ALLOWED_GPS_DELAY 5000 +#define RID_TIMEOUT 2500 // Messages should be arriving at 1 Hz, so we set a 2 second timeout + +const uint8_t* RemoteIDManager::_id_or_mac_unknown = {NULL}; + +RemoteIDManager::RemoteIDManager(Vehicle* vehicle) + : QObject (vehicle) + , _mavlink (nullptr) + , _vehicle (vehicle) + , _settings (nullptr) + , _armStatusGood (false) + , _commsGood (false) + , _gcsGPSGood (false) + , _basicIDGood (true) + , _GCSBasicIDValid (false) + , _operatorIDGood (false) + , _emergencyDeclared (false) + , _targetSystem (0) // By default 0 means broadcast + , _targetComponent (0) // By default 0 means broadcast + , _enforceSendingSelfID (false) +{ + _mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + _settings = qgcApp()->toolbox()->settingsManager()->remoteIDSettings(); + _positionManager = qgcApp()->toolbox()->qgcPositionManager(); + + // Timer to track a healthy RID device. When expired we let the operator know + _odidTimeoutTimer.setSingleShot(true); + _odidTimeoutTimer.setInterval(RID_TIMEOUT); + connect(&_odidTimeoutTimer, &QTimer::timeout, this, &RemoteIDManager::_odidTimeout); + + // Timer to send messages at a constant rate + _sendMessagesTimer.setInterval(SENDING_RATE_MSEC); + connect(&_sendMessagesTimer, &QTimer::timeout, this, &RemoteIDManager::_sendMessages); + + // GCS GPS position updates to track the health of the GPS data + connect(_positionManager, &QGCPositionManager::positionInfoUpdated, this, &RemoteIDManager::_updateLastGCSPositionInfo); + + // Check changes in basic id settings as long as they are modified + connect(_settings->basicID(), &Fact::rawValueChanged, this, &RemoteIDManager::_checkGCSBasicID); + connect(_settings->basicIDType(), &Fact::rawValueChanged, this, &RemoteIDManager::_checkGCSBasicID); + connect(_settings->basicIDUaType(), &Fact::rawValueChanged, this, &RemoteIDManager::_checkGCSBasicID); + + // Assign vehicle sysid and compid. GCS must target these messages to autopilot, and autopilot will redirect them to RID device + _targetSystem = _vehicle->id(); + _targetComponent = _vehicle->compId(); +} + +void RemoteIDManager::mavlinkMessageReceived(mavlink_message_t& message ) +{ + switch (message.msgid) { + // So far we are only listening to this one, as heartbeat won't be sent if connected by CAN + case MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS: + _handleArmStatus(message); + default: + break; + } +} + +// This slot will be called if we stop receiving heartbeats for more than RID_TIMEOUT seconds +void RemoteIDManager::_odidTimeout() +{ + _commsGood = false; + _sendMessagesTimer.stop(); // We stop sending messages if the communication with the RID device is down + emit commsGoodChanged(); + qCDebug(RemoteIDManagerLog) << "We stopped receiving heartbeat from RID device."; +} + +// Parsing of the ARM_STATUS message comming from the RID device +void RemoteIDManager::_handleArmStatus(mavlink_message_t& message) +{ + // Compid must be ODID_TXRX_X + if ( (message.compid < MAV_COMP_ID_ODID_TXRX_1) || (message.compid > MAV_COMP_ID_ODID_TXRX_3) ) { + // or same as autopilot, in the case of Ardupilot and CAN RID modules + if (message.compid != MAV_COMP_ID_AUTOPILOT1) { + return; + } + } + + // Sanity check, only get messages from same sysid + if (_vehicle->id() != message.sysid) { + return; + } + + // We set the targetsystem + if (_targetSystem != message.sysid) { + _targetSystem = message.sysid; + qCDebug(RemoteIDManagerLog) << "Subscribing to ODID messages coming from system " << _targetSystem; + } + + if (!_commsGood) { + _commsGood = true; + _sendMessagesTimer.start(); // Start sending our messages + _checkGCSBasicID(); // Check if basicID is good to send + checkOperatorID(); // Check if OperatorID is good in case we want to send it from start because of the settings + emit commsGoodChanged(); + qCDebug(RemoteIDManagerLog) << "Receiving ODID_ARM_STATUS from RID device"; + } + + // Restart the timeout + _odidTimeoutTimer.start(); + + // CompId and sysId are correct, we can proceed + mavlink_open_drone_id_arm_status_t armStatus; + mavlink_msg_open_drone_id_arm_status_decode(&message, &armStatus); + + if (armStatus.status == MAV_ODID_GOOD_TO_ARM && !_armStatusGood) { + // If good to arm, even if basic ID is not set on GCS, it was set by remoteID parameters, so GCS one would be optional in this case + if (!_basicIDGood) { + _basicIDGood = true; + emit basicIDGoodChanged(); + } + _armStatusGood = true; + emit armStatusGoodChanged(); + qCDebug(RemoteIDManagerLog) << "Arm status GOOD TO ARM."; + } + + if (armStatus.status == MAV_ODID_PRE_ARM_FAIL_GENERIC) { + _armStatusGood = false; + _armStatusError = QString::fromLocal8Bit(armStatus.error); + // Check if the error is because of missing basic id + if (armStatus.error == QString("missing basic_id message")) { + _basicIDGood = false; + qCDebug(RemoteIDManagerLog) << "Arm status error, basic_id is not set in RID device nor in GCS!"; + emit basicIDGoodChanged(); + } + emit armStatusGoodChanged(); + emit armStatusErrorChanged(); + qCDebug(RemoteIDManagerLog) << "Arm status error:" << _armStatusError; + } +} + +// Function that sends messages periodically +void RemoteIDManager::_sendMessages() +{ + // We only send RemoteID messages if we have it enabled in General settings + if (!_settings->enable()->rawValue().toBool()) { + return; + } + + // We always try to send System + _sendSystem(); + + // only send it if the information is correct and the tickbox in settings is set + if (_GCSBasicIDValid && _settings->sendBasicID()->rawValue().toBool()) { + _sendBasicID(); + } + + // We only send selfID if the pilot wants it or in case of a declared emergency. If an emergency is cleared + // we also keep sending the message, to be sure the non emergency state makes it up to the vehicle + if (_settings->sendSelfID()->rawValue().toBool() || _emergencyDeclared || _enforceSendingSelfID) { + _sendSelfIDMsg(); + } + + // We only send the OperatorID if the pilot wants it or if the region we have set is europe. + // To be able to send it, it needs to be filled correclty + if ((_settings->sendOperatorID()->rawValue().toBool() || (_settings->region()->rawValue().toInt() == Region::EU)) && _operatorIDGood) { + _sendOperatorID(); + } + +} + +void RemoteIDManager::_sendSelfIDMsg() +{ + WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); + SharedLinkInterfacePtr sharedLink = weakLink.lock(); + + if (sharedLink) { + mavlink_message_t msg; + + mavlink_msg_open_drone_id_self_id_pack_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + sharedLink->mavlinkChannel(), + &msg, + _targetSystem, + _targetComponent, + _id_or_mac_unknown, + _emergencyDeclared ? 1 : _settings->selfIDType()->rawValue().toInt(), // If emergency is delcared we send directly a 1 (1 = EMERGENCY) + _getSelfIDDescription()); // Depending on the type of SelfID we send a different description + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } +} + +// We need to return the correct description for the self ID type we have selected +const char* RemoteIDManager::_getSelfIDDescription() +{ + QByteArray bytesFree = (_settings->selfIDFree()->rawValue().toString()).toLocal8Bit(); + QByteArray bytesEmergency = (_settings->selfIDEmergency()->rawValue().toString()).toLocal8Bit(); + QByteArray bytesExtended = (_settings->selfIDExtended()->rawValue().toString()).toLocal8Bit(); + + const char* descriptionToSend; + + if (_emergencyDeclared) { + // If emergency is declared we dont care about the settings and we send emergency directly + descriptionToSend = bytesEmergency.data(); + } else { + switch (_settings->selfIDType()->rawValue().toInt()) { + case 0: + descriptionToSend = bytesFree.data(); + break; + case 1: + descriptionToSend = bytesEmergency.data(); + break; + case 2: + descriptionToSend = bytesExtended.data(); + break; + default: + descriptionToSend = bytesEmergency.data(); + } + } + + return descriptionToSend; +} + +void RemoteIDManager::_sendOperatorID() +{ + WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); + SharedLinkInterfacePtr sharedLink = weakLink.lock(); + + if (sharedLink) { + mavlink_message_t msg; + + QByteArray bytesOperatorID = (_settings->operatorID()->rawValue().toString()).toLocal8Bit(); + const char* descriptionToSend = bytesOperatorID.data(); + + mavlink_msg_open_drone_id_operator_id_pack_chan( + _mavlink->getSystemId(), + _mavlink->getComponentId(), + sharedLink->mavlinkChannel(), + &msg, + _targetSystem, + _targetComponent, + _id_or_mac_unknown, + _settings->operatorIDType()->rawValue().toInt(), + descriptionToSend); + + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } +} + +void RemoteIDManager::_sendSystem() +{ + QGeoCoordinate gcsPosition; + QGeoPositionInfo geoPositionInfo; + // Location types: + // 0 -> TAKEOFF (not supported yet) + // 1 -> LIVE GNNS + // 2 -> FIXED + if (_settings->locationType()->rawValue().toUInt() == LocationTypes::FIXED) { + // For FIXED location, we first check that the values are valid. Then we populate our position + if (_settings->latitudeFixed()->rawValue().toFloat() >= -90 && _settings->latitudeFixed()->rawValue().toFloat() <= 90 && _settings->longitudeFixed()->rawValue().toFloat() >= -180 && _settings->longitudeFixed()->rawValue().toFloat() <= 180) { + gcsPosition = QGeoCoordinate(_settings->latitudeFixed()->rawValue().toFloat(), _settings->longitudeFixed()->rawValue().toFloat(), _settings->altitudeFixed()->rawValue().toFloat()); + geoPositionInfo = QGeoPositionInfo(gcsPosition, QDateTime::currentDateTime().currentDateTimeUtc()); + if (!_gcsGPSGood) { + _gcsGPSGood = true; + emit gcsGPSGoodChanged(); + } + } else { + gcsPosition = QGeoCoordinate(0,0,0); + geoPositionInfo = QGeoPositionInfo(gcsPosition, QDateTime::currentDateTime().currentDateTimeUtc()); + if (_gcsGPSGood) { + _gcsGPSGood = false; + emit gcsGPSGoodChanged(); + qCDebug(RemoteIDManagerLog) << "The provided coordinates for FIXED position are invalid."; + } + } + } else { + // For Live GNSS we take QGC GPS data + gcsPosition = _positionManager->gcsPosition(); + geoPositionInfo = _positionManager->geoPositionInfo(); + + // GPS position needs to be valid before checking other stuff + if (geoPositionInfo.isValid()) { + // If we dont have altitude for FAA then the GPS data is no good + if ((_settings->region()->rawValue().toInt() == Region::FAA) && !(gcsPosition.altitude() >= 0) && _gcsGPSGood) { + _gcsGPSGood = false; + emit gcsGPSGoodChanged(); + qCDebug(RemoteIDManagerLog) << "GCS GPS data error (no altitude): Altitude data is mandatory for GCS GPS data in FAA regions."; + return; + } + + // If the GPS data is older than ALLOWED_GPS_DELAY we cannot use this data + if (_lastGeoPositionTimeStamp.msecsTo(QDateTime::currentDateTime().currentDateTimeUtc()) > ALLOWED_GPS_DELAY) { + if (_gcsGPSGood) { + _gcsGPSGood = false; + emit gcsGPSGoodChanged(); + qCDebug(RemoteIDManagerLog) << "GCS GPS data is older than 5 seconds"; + } + } else { + if (!_gcsGPSGood) { + _gcsGPSGood = true; + emit gcsGPSGoodChanged(); + } + } + } else { + _gcsGPSGood = false; + emit gcsGPSGoodChanged(); + qCDebug(RemoteIDManagerLog) << "GCS GPS data is not valid."; + } + + } + + WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); + SharedLinkInterfacePtr sharedLink = weakLink.lock(); + + if (sharedLink) { + mavlink_message_t msg; + + mavlink_msg_open_drone_id_system_pack_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + sharedLink->mavlinkChannel(), + &msg, + _targetSystem, + _targetComponent, + _id_or_mac_unknown, + _settings->locationType()->rawValue().toUInt(), + _settings->classificationType()->rawValue().toUInt(), + geoPositionInfo.isValid() ? ( gcsPosition.latitude() * 1.0e7 ) : 0, // If position not valid, send a 0 + geoPositionInfo.isValid() ? ( gcsPosition.longitude() * 1.0e7 ) : 0, // If position not valid, send a 0 + AREA_COUNT, + AREA_RADIUS, + -1000.0f, + -1000.0f, + _settings->categoryEU()->rawValue().toUInt(), + _settings->classEU()->rawValue().toUInt(), + geoPositionInfo.isValid() ? gcsPosition.altitude() : 0, // If position not valid, send a 0 + _timestamp2019()), // Time stamp needs to be since 00:00:00 1/1/2019 + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } +} + +// Returns seconds elapsed since 00:00:00 1/1/2019 +uint32_t RemoteIDManager::_timestamp2019() +{ + uint32_t secsSinceEpoch2019 = 1546300800; // Secs elapsed since epoch to 1-1-2019 + + return ((QDateTime::currentDateTime().currentSecsSinceEpoch()) - secsSinceEpoch2019); +} + +void RemoteIDManager::_sendBasicID() +{ + WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); + SharedLinkInterfacePtr sharedLink = weakLink.lock(); + + if (sharedLink) { + mavlink_message_t msg; + + QString basicIDTemp = _settings->basicID()->rawValue().toString(); + QByteArray ba = basicIDTemp.toLocal8Bit(); + // To make sure the buffer is large enough to fit the message. It will add padding bytes if smaller, or exclude the extra ones if bigger + ba.resize(MAVLINK_MSG_OPEN_DRONE_ID_BASIC_ID_FIELD_UAS_ID_LEN); + + mavlink_msg_open_drone_id_basic_id_pack_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + sharedLink->mavlinkChannel(), + &msg, + _targetSystem, + _targetComponent, + _id_or_mac_unknown, + _settings->basicIDType()->rawValue().toUInt(), + _settings->basicIDUaType()->rawValue().toUInt(), + reinterpret_cast(ba.constData())), + + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } +} + +void RemoteIDManager::_checkGCSBasicID() +{ + QString basicID = _settings->basicID()->rawValue().toString(); + + if (!basicID.isEmpty() && (_settings->basicIDType()->rawValue().toInt() >= 0) && (_settings->basicIDUaType()->rawValue().toInt() >= 0)) { + _GCSBasicIDValid = true; + } else { + _GCSBasicIDValid = false; + } +} + +void RemoteIDManager::checkOperatorID() +{ + QString operatorID = _settings->operatorID()->rawValue().toString(); + + if (!operatorID.isEmpty() && (_settings->operatorIDType()->rawValue().toInt() >= 0)) { + _operatorIDGood = true; + } else { + _operatorIDGood = false; + } + emit operatorIDGoodChanged(); +} + +void RemoteIDManager::setEmergency(bool declare) +{ + _emergencyDeclared = declare; + emit emergencyDeclaredChanged(); + // Wether we are starting an emergency or cancelling it, we need to enforce sending + // this message. Otherwise, if non optimal connection quality, vehicle RID device + // could remain in the wrong state. It is clarified to the user in remoteidsettings.qml + _enforceSendingSelfID = true; + + qCDebug(RemoteIDManagerLog) << ( declare ? "Emergency declared." : "Emergency cleared."); +} + +void RemoteIDManager::_updateLastGCSPositionInfo(QGeoPositionInfo update) +{ + if (update.isValid()) { + _lastGeoPositionTimeStamp = update.timestamp().toUTC(); + } +} \ No newline at end of file diff --git a/src/Vehicle/RemoteIDManager.h b/src/Vehicle/RemoteIDManager.h new file mode 100644 index 0000000..d256e92 --- /dev/null +++ b/src/Vehicle/RemoteIDManager.h @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * (c) 2009-2022 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +#include "QGCLoggingCategory.h" +#include "QGCMAVLink.h" +#include "Vehicle.h" + +Q_DECLARE_LOGGING_CATEGORY(RemoteIDManagerLog) + +class RemoteIDSettings; +class QGCPositionManager; + +// Supporting Opend Dron ID protocol +class RemoteIDManager : public QObject +{ + Q_OBJECT + +public: + RemoteIDManager(Vehicle* vehicle); + + Q_PROPERTY (bool armStatusGood READ armStatusGood NOTIFY armStatusGoodChanged) + Q_PROPERTY (QString armStatusError READ armStatusError NOTIFY armStatusErrorChanged) + Q_PROPERTY (bool commsGood READ commsGood NOTIFY commsGoodChanged) + Q_PROPERTY (bool gcsGPSGood READ gcsGPSGood NOTIFY gcsGPSGoodChanged) + Q_PROPERTY (bool basicIDGood READ basicIDGood NOTIFY basicIDGoodChanged) + Q_PROPERTY (bool emergencyDeclared READ emergencyDeclared NOTIFY emergencyDeclaredChanged) + Q_PROPERTY (bool operatorIDGood READ operatorIDGood NOTIFY operatorIDGoodChanged) + + + // Check that the information filled by the pilot operatorID is good + Q_INVOKABLE void checkOperatorID(); + + // Declare emergency + Q_INVOKABLE void setEmergency(bool declare); + + bool armStatusGood (void) const { return _armStatusGood; } + QString armStatusError (void) const { return _armStatusError; } + bool commsGood (void) const { return _commsGood; } + bool gcsGPSGood (void) const { return _gcsGPSGood; } + bool basicIDGood (void) const { return _basicIDGood; } + bool emergencyDeclared (void) const { return _emergencyDeclared;} + bool operatorIDGood (void) const { return _operatorIDGood; } + + void mavlinkMessageReceived (mavlink_message_t& message); + + enum LocationTypes { + TAKEOFF, + LiveGNSS, + FIXED + }; + + enum Region { + FAA, + EU + }; + +signals: + void armStatusGoodChanged(); + void armStatusErrorChanged(); + void commsGoodChanged(); + void gcsGPSGoodChanged(); + void basicIDGoodChanged(); + void emergencyDeclaredChanged(); + void operatorIDGoodChanged(); + +private slots: + void _odidTimeout(); + void _sendMessages(); + void _updateLastGCSPositionInfo(QGeoPositionInfo update); + void _checkGCSBasicID(); + +private: + void _handleArmStatus(mavlink_message_t& message); + + // Self ID + void _sendSelfIDMsg (); + const char* _getSelfIDDescription(); + + // Operator ID + void _sendOperatorID (); + + // System + void _sendSystem(); + uint32_t _timestamp2019(); + + // Basic ID + void _sendBasicID(); + + MAVLinkProtocol* _mavlink; + Vehicle* _vehicle; + RemoteIDSettings* _settings; + QGCPositionManager* _positionManager; + + // Flags ODID + bool _armStatusGood; + QString _armStatusError; + bool _commsGood; + bool _gcsGPSGood; + bool _basicIDGood; + bool _GCSBasicIDValid; + bool _operatorIDGood; + + bool _emergencyDeclared; + QDateTime _lastGeoPositionTimeStamp; + int _targetSystem; + int _targetComponent; + + // After emergency cleared, this makes sure the non emergency selfID message makes it to the vehicle + bool _enforceSendingSelfID; + + static const uint8_t* _id_or_mac_unknown; + + // Timers + QTimer _odidTimeoutTimer; + QTimer _sendMessagesTimer; +}; \ No newline at end of file