Skip to content

Commit

Permalink
AP_OpenDroneID: fix operator location per specification
Browse files Browse the repository at this point in the history
  • Loading branch information
bugobliterator committed Jun 14, 2024
1 parent 247f5e5 commit 2bb86dd
Showing 1 changed file with 18 additions and 0 deletions.
18 changes: 18 additions & 0 deletions libraries/AP_OpenDroneID/AP_OpenDroneID.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand Down

0 comments on commit 2bb86dd

Please sign in to comment.