davidsastresas
2 years ago
committed by
David Sastre
8 changed files with 819 additions and 0 deletions
@ -0,0 +1,169 @@
@@ -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 |
||||
} |
||||
] |
||||
} |
||||
@ -0,0 +1,40 @@
@@ -0,0 +1,40 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2022 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "RemoteIDSettings.h" |
||||
|
||||
#include <QQmlEngine> |
||||
#include <QtQml> |
||||
|
||||
DECLARE_SETTINGGROUP(RemoteID, "RemoteID") |
||||
{ |
||||
qmlRegisterUncreatableType<RemoteIDSettings>("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) |
||||
@ -0,0 +1,42 @@
@@ -0,0 +1,42 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2022 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include "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) |
||||
}; |
||||
@ -0,0 +1,430 @@
@@ -0,0 +1,430 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2022 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "RemoteIDManager.h" |
||||
#include "QGCApplication.h" |
||||
#include "SettingsManager.h" |
||||
#include "RemoteIDSettings.h" |
||||
#include "QGCQGeoCoordinate.h" |
||||
#include "PositionManager.h" |
||||
|
||||
#include <QDebug> |
||||
|
||||
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<const unsigned char*>(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(); |
||||
} |
||||
} |
||||
@ -0,0 +1,128 @@
@@ -0,0 +1,128 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2022 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <QObject> |
||||
#include <QDateTime> |
||||
#include <QGeoPositionInfo> |
||||
|
||||
#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; |
||||
}; |
||||
Loading…
Reference in new issue