remoteid: add SelfID message

msg/CMakeLists.txt

@@ -154,6 +154,7 @@ set(msg_files
 	OnboardComputerStatus.msg
 	OpenDroneIdArmStatus.msg
 	OpenDroneIdOperatorId.msg
+	OpenDroneIdSelfId.msg
 	OrbitStatus.msg
 	OrbTest.msg
 	OrbTestLarge.msg

msg/OpenDroneIdSelfId.msg

@@ -0,0 +1,4 @@
+uint64 timestamp
+uint8[20] id_or_mac
+uint8 description_type
+char[23] description

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;

src/drivers/uavcan/remoteid.hpp

@@ -43,10 +43,12 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/open_drone_id_operator_id.h>
 #include <uORB/topics/open_drone_id_arm_status.h>
+#include <uORB/topics/open_drone_id_self_id.h>
 
 #include <uavcan/uavcan.hpp>
 #include <dronecan/remoteid/BasicID.hpp>
 #include <dronecan/remoteid/Location.hpp>
+#include <dronecan/remoteid/SelfID.hpp>
 #include <dronecan/remoteid/System.hpp>
 #include <dronecan/remoteid/ArmStatus.hpp>
 #include <dronecan/remoteid/OperatorID.hpp>
@@ -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_s> _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)};
 
 	uavcan::Publisher<dronecan::remoteid::BasicID> _uavcan_pub_remoteid_basicid;
 	uavcan::Publisher<dronecan::remoteid::Location> _uavcan_pub_remoteid_location;
+	uavcan::Publisher<dronecan::remoteid::SelfID> _uavcan_pub_remoteid_self_id;
 	uavcan::Publisher<dronecan::remoteid::System> _uavcan_pub_remoteid_system;
 	uavcan::Publisher<dronecan::remoteid::OperatorID> _uavcan_pub_remoteid_operator_id;
 

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()
 {

src/modules/mavlink/mavlink_receiver.h

@@ -88,6 +88,7 @@
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/onboard_computer_status.h>
 #include <uORB/topics/open_drone_id_operator_id.h>
+#include <uORB/topics/open_drone_id_self_id.h>
 #include <uORB/topics/ping.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/radio_status.h>
@@ -317,6 +318,7 @@ private:
 	uORB::Publication<onboard_computer_status_s>		_onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
 	uORB::Publication<velocity_limits_s>			_velocity_limits_pub{ORB_ID(velocity_limits)};
 	uORB::Publication<open_drone_id_operator_id_s>		_open_drone_id_operator_id_pub{ORB_ID(open_drone_id_operator_id)};
+	uORB::Publication<open_drone_id_self_id_s>		_open_drone_id_self_id_pub{ORB_ID(open_drone_id_self_id)};
 	uORB::Publication<generator_status_s>			_generator_status_pub{ORB_ID(generator_status)};
 	uORB::Publication<vehicle_attitude_s>			_attitude_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};