remoteid: add SelfID message

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;