Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_OpenDroneID: fix operator location per specification

This commit is contained in:
bugobliterator 2024-06-14 10:56:55 +10:00
parent 247f5e5446
commit 2bb86ddd21
1 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_OpenDroneID/AP_OpenDroneID.cpp
View File

@ -768,6 +768,15 @@ void AP_OpenDroneID::handle_msg(mavlink_channel_t chan, const mavlink_message_t
case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM:
mavlink_msg_open_drone_id_system_decode(&msg, &pkt_system);
last_system_ms = AP_HAL::millis();
if (pkt_system.operator_latitude == 0 && pkt_system.operator_longitude == 0) {
// set the operator location to the takeoff location if it is not set,
// Specification ASTM F3411-22 5.4.5.19
pkt_system.operator_latitude = _takeoff_location.lat;
pkt_system.operator_longitude = _takeoff_location.lng;
pkt_system.operator_altitude_geo = _takeoff_location.alt;
pkt_system.operator_location_type &= ~0x3;
pkt_system.operator_location_type |= MAV_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF;
}
break;
case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE: {
mavlink_open_drone_id_system_update_t pkt_system_update;
@ -776,6 +785,15 @@ void AP_OpenDroneID::handle_msg(mavlink_channel_t chan, const mavlink_message_t
pkt_system.operator_longitude = pkt_system_update.operator_longitude;
pkt_system.operator_altitude_geo = pkt_system_update.operator_altitude_geo;
pkt_system.timestamp = pkt_system_update.timestamp;
if (pkt_system.operator_latitude == 0 && pkt_system.operator_longitude == 0) {
// set the operator location to the takeoff location if it is not set,
// Specification ASTM F3411-22 5.4.5.19
pkt_system.operator_latitude = _takeoff_location.lat;
pkt_system.operator_longitude = _takeoff_location.lng;
pkt_system.operator_altitude_geo = _takeoff_location.alt;
pkt_system.operator_location_type &= ~0x3;
pkt_system.operator_location_type |= MAV_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF;
}
last_system_update_ms = AP_HAL::millis();
if (last_system_ms != 0) {
// we can only mark system as updated if we have the other