From 2b2c506a95cbe1e419857059b4dd66e58f798f15 Mon Sep 17 00:00:00 2001 From: msl-dev <97445659+msli-dev@users.noreply.github.com> Date: Tue, 9 Jun 2026 05:26:12 +0800 Subject: [PATCH] fix(dronecan): forward MAVLink OpenDroneID Basic ID (#27274) --- msg/CMakeLists.txt | 1 + msg/OpenDroneIdBasicId.msg | 5 +++ src/drivers/uavcan/remoteid.cpp | 43 ++++++++++++++++++------ src/drivers/uavcan/remoteid.hpp | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 25 ++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 3 ++ 6 files changed, 68 insertions(+), 11 deletions(-) create mode 100644 msg/OpenDroneIdBasicId.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index af55a8474fe..77c3f75e57d 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -152,6 +152,7 @@ set(msg_files OffboardControlMode.msg OnboardComputerStatus.msg OpenDroneIdArmStatus.msg + OpenDroneIdBasicId.msg OpenDroneIdOperatorId.msg OpenDroneIdSelfId.msg OpenDroneIdSystem.msg diff --git a/msg/OpenDroneIdBasicId.msg b/msg/OpenDroneIdBasicId.msg new file mode 100644 index 00000000000..4811fd27ce9 --- /dev/null +++ b/msg/OpenDroneIdBasicId.msg @@ -0,0 +1,5 @@ +uint64 timestamp +uint8[20] id_or_mac # Only used for drone ID data received from other UAs, no null termination, null filled if shorter +uint8 id_type # MAV_ODID_ID_TYPE: indicates the format for the uas_id field +uint8 ua_type # MAV_ODID_UA_TYPE: indicates the type of UA (Unmanned Aircraft) +uint8[20] uas_id # UAS (Unmanned Aircraft System) ID following the format specified by id_type, no null termination, null filled if shorter diff --git a/src/drivers/uavcan/remoteid.cpp b/src/drivers/uavcan/remoteid.cpp index a2f373b767b..4e76e71134a 100644 --- a/src/drivers/uavcan/remoteid.cpp +++ b/src/drivers/uavcan/remoteid.cpp @@ -80,20 +80,41 @@ void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &) void UavcanRemoteIDController::send_basic_id() { - dronecan::remoteid::BasicID basic_id {}; - // basic_id.id_or_mac // supposedly only used for drone ID data from other UAs - basic_id.id_type = dronecan::remoteid::BasicID::ODID_ID_TYPE_SERIAL_NUMBER; - basic_id.ua_type = static_cast(open_drone_id_translations::odidTypeForMavType( - _vehicle_status.get().system_type)); + dronecan::remoteid::BasicID msg {}; - // uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type - // TODO: MAV_ODID_ID_TYPE_SERIAL_NUMBER needs to be ANSI/CTA-2063 format + // msg.id_or_mac // supposedly only used for drone ID data from other UAs + if (_open_drone_id_basic_id.advertised()) { + open_drone_id_basic_id_s basic_id {}; - char uas_id[20] = {}; - board_get_px4_guid_formated((char *)(uas_id), sizeof(uas_id)); - basic_id.uas_id = uas_id; + if (_open_drone_id_basic_id.copy(&basic_id)) { + msg.id_type = basic_id.id_type; + msg.ua_type = basic_id.ua_type; - _uavcan_pub_remoteid_basicid.broadcast(basic_id); + using UasIdField = decltype(msg.uas_id); + static_assert(sizeof(basic_id.uas_id) == UasIdField::MaxSize, "OpenDroneID Basic ID uas_id size mismatch"); + + // uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type + for (unsigned i = 0; i < UasIdField::MaxSize; ++i) { + msg.uas_id.push_back(basic_id.uas_id[i]); + } + + } + + } else { + msg.id_type = dronecan::remoteid::BasicID::ODID_ID_TYPE_SERIAL_NUMBER; + msg.ua_type = static_cast(open_drone_id_translations::odidTypeForMavType(_vehicle_status.get().system_type)); + + // uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type + // TODO: MAV_ODID_ID_TYPE_SERIAL_NUMBER needs to be ANSI/CTA-2063 format + char uas_id[20] {}; + board_get_px4_guid_formated(uas_id, sizeof(uas_id)); + + for (unsigned i = 0; i < sizeof(uas_id); ++i) { + msg.uas_id.push_back(uas_id[i]); + } + } + + _uavcan_pub_remoteid_basicid.broadcast(msg); } void UavcanRemoteIDController::send_location() diff --git a/src/drivers/uavcan/remoteid.hpp b/src/drivers/uavcan/remoteid.hpp index 1d1fcaa83ce..3c2916bd066 100644 --- a/src/drivers/uavcan/remoteid.hpp +++ b/src/drivers/uavcan/remoteid.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -89,6 +90,7 @@ private: uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + uORB::Subscription _open_drone_id_basic_id{ORB_ID(open_drone_id_basic_id)}; uORB::Subscription _open_drone_id_operator_id{ORB_ID(open_drone_id_operator_id)}; uORB::Subscription _open_drone_id_self_id{ORB_ID(open_drone_id_self_id)}; uORB::Subscription _open_drone_id_system{ORB_ID(open_drone_id_system)}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2dd78b153e8..9d271f25145 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -276,6 +276,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_statustext(msg); break; + case MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID: + handle_message_open_drone_id_basic_id(msg); + break; + case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID: handle_message_open_drone_id_operator_id(msg); break; @@ -3536,6 +3540,27 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t _gimbal_device_attitude_status_pub.publish(gimbal_attitude_status); } +void MavlinkReceiver::handle_message_open_drone_id_basic_id(mavlink_message_t *msg) +{ + mavlink_open_drone_id_basic_id_t odid_module {}; + mavlink_msg_open_drone_id_basic_id_decode(msg, &odid_module); + + if (odid_module.target_system != mavlink_system.sysid || + (odid_module.target_component != mavlink_system.compid && odid_module.target_component != MAV_COMP_ID_ALL)) { + return; + } + + open_drone_id_basic_id_s odid_basic_id {}; + + odid_basic_id.timestamp = hrt_absolute_time(); + memcpy(odid_basic_id.id_or_mac, odid_module.id_or_mac, sizeof(odid_basic_id.id_or_mac)); + odid_basic_id.id_type = odid_module.id_type; + odid_basic_id.ua_type = odid_module.ua_type; + memcpy(odid_basic_id.uas_id, odid_module.uas_id, sizeof(odid_basic_id.uas_id)); + + _open_drone_id_basic_id_pub.publish(odid_basic_id); +} + void MavlinkReceiver::handle_message_open_drone_id_operator_id( mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 62d7c6aa5bd..644f1afe871 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -91,6 +91,7 @@ #include #include #include +#include #include #include #include @@ -206,6 +207,7 @@ private: void handle_message_obstacle_distance(mavlink_message_t *msg); void handle_message_odometry(mavlink_message_t *msg); void handle_message_onboard_computer_status(mavlink_message_t *msg); + void handle_message_open_drone_id_basic_id(mavlink_message_t *msg); void handle_message_open_drone_id_operator_id(mavlink_message_t *msg); void handle_message_open_drone_id_self_id(mavlink_message_t *msg); void handle_message_open_drone_id_system(mavlink_message_t *msg); @@ -355,6 +357,7 @@ private: uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; uORB::Publication _velocity_limits_pub{ORB_ID(velocity_limits)}; + uORB::Publication _open_drone_id_basic_id_pub{ORB_ID(open_drone_id_basic_id)}; uORB::Publication _open_drone_id_operator_id_pub{ORB_ID(open_drone_id_operator_id)}; uORB::Publication _open_drone_id_self_id_pub{ORB_ID(open_drone_id_self_id)}; uORB::Publication _open_drone_id_system_pub{ORB_ID(open_drone_id_system)};