From 7d1d3989840fe0219e4d2ec8d8537ec4ad09fddd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 19 Jul 2024 14:12:22 +1200 Subject: [PATCH] remoteid: add SelfID message --- msg/CMakeLists.txt | 1 + msg/OpenDroneIdSelfId.msg | 4 ++++ src/drivers/uavcan/remoteid.cpp | 24 ++++++++++++++++++++++++ src/drivers/uavcan/remoteid.hpp | 5 +++++ src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 2 ++ 6 files changed, 56 insertions(+) create mode 100644 msg/OpenDroneIdSelfId.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 0585df1ac5..f7465fdc00 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -154,6 +154,7 @@ set(msg_files OnboardComputerStatus.msg OpenDroneIdArmStatus.msg OpenDroneIdOperatorId.msg + OpenDroneIdSelfId.msg OrbitStatus.msg OrbTest.msg OrbTestLarge.msg diff --git a/msg/OpenDroneIdSelfId.msg b/msg/OpenDroneIdSelfId.msg new file mode 100644 index 0000000000..1ff699ebf1 --- /dev/null +++ b/msg/OpenDroneIdSelfId.msg @@ -0,0 +1,4 @@ +uint64 timestamp +uint8[20] id_or_mac +uint8 description_type +char[23] description diff --git a/src/drivers/uavcan/remoteid.cpp b/src/drivers/uavcan/remoteid.cpp index 01c68aa944..6f66ba94ce 100644 --- a/src/drivers/uavcan/remoteid.cpp +++ b/src/drivers/uavcan/remoteid.cpp @@ -44,6 +44,7 @@ UavcanRemoteIDController::UavcanRemoteIDController(uavcan::INode &node) : _node(node), _uavcan_pub_remoteid_basicid(node), _uavcan_pub_remoteid_location(node), + _uavcan_pub_remoteid_self_id(node), _uavcan_pub_remoteid_system(node), _uavcan_pub_remoteid_operator_id(node), _uavcan_sub_arm_status(node) @@ -72,6 +73,7 @@ void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &) send_basic_id(); send_location(); + send_self_id(); send_system(); send_operator_id(); } @@ -266,6 +268,28 @@ void UavcanRemoteIDController::send_system() } } +void UavcanRemoteIDController::send_self_id() +{ + open_drone_id_self_id_s self_id; + + if (_open_drone_id_self_id.copy(&self_id)) { + + dronecan::remoteid::SelfID msg {}; + + for (unsigned i = 0; i < sizeof(self_id.id_or_mac); ++i) { + msg.id_or_mac.push_back(self_id.id_or_mac[i]); + } + + msg.description_type = self_id.description_type; + + for (unsigned i = 0; i < sizeof(self_id.description); ++i) { + msg.description.push_back(self_id.description[i]); + } + + _uavcan_pub_remoteid_self_id.broadcast(msg); + } +} + void UavcanRemoteIDController::send_operator_id() { open_drone_id_operator_id_s operator_id; diff --git a/src/drivers/uavcan/remoteid.hpp b/src/drivers/uavcan/remoteid.hpp index d78f365be6..eb9bffd6c0 100644 --- a/src/drivers/uavcan/remoteid.hpp +++ b/src/drivers/uavcan/remoteid.hpp @@ -43,10 +43,12 @@ #include #include #include +#include #include #include #include +#include #include #include #include @@ -72,6 +74,7 @@ private: void send_basic_id(); void send_location(); + void send_self_id(); void send_system(); void send_operator_id(); @@ -86,10 +89,12 @@ private: 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_operator_id{ORB_ID(open_drone_id_operator_id)}; + uORB::Subscription _open_drone_id_self_id{ORB_ID(open_drone_id_self_id)}; uORB::Publication _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)}; uavcan::Publisher _uavcan_pub_remoteid_basicid; uavcan::Publisher _uavcan_pub_remoteid_location; + uavcan::Publisher _uavcan_pub_remoteid_self_id; uavcan::Publisher _uavcan_pub_remoteid_system; uavcan::Publisher _uavcan_pub_remoteid_operator_id; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f54f9c158c..6e13f3c0ea 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -284,6 +284,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_open_drone_id_operator_id(msg); break; + case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID: + handle_message_open_drone_id_self_id(msg); + break; + #if !defined(CONSTRAINED_FLASH) case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: @@ -3105,6 +3109,22 @@ void MavlinkReceiver::handle_message_open_drone_id_operator_id( _open_drone_id_operator_id_pub.publish(odid_operator_id); } +void MavlinkReceiver::handle_message_open_drone_id_self_id(mavlink_message_t *msg) +{ + mavlink_open_drone_id_self_id_t odid_module; + mavlink_msg_open_drone_id_self_id_decode(msg, &odid_module); + + open_drone_id_self_id_s odid_self_id{}; + memset(&odid_self_id, 0, sizeof(odid_self_id)); + + odid_self_id.timestamp = hrt_absolute_time(); + memcpy(odid_self_id.id_or_mac, odid_module.id_or_mac, sizeof(odid_self_id.id_or_mac)); + odid_self_id.description_type = odid_module.description_type; + memcpy(odid_self_id.description, odid_module.description, sizeof(odid_self_id.description)); + + _open_drone_id_self_id_pub.publish(odid_self_id); +} + void MavlinkReceiver::run() { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index acea502841..4fa143475a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -88,6 +88,7 @@ #include #include #include +#include #include #include #include @@ -317,6 +318,7 @@ private: 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_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 _generator_status_pub{ORB_ID(generator_status)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};