uavcan: add OpenDroneID ArmStatus, operator ID

In order to have operator ID be sent by QGC, we need to forward
ArmStatus from the remote ID module (here on DroneCAN) to MAVLink.

src/drivers/uavcan/remoteid.cpp

@@ -44,7 +44,9 @@ UavcanRemoteIDController::UavcanRemoteIDController(uavcan::INode &node) :
 	_node(node),
 	_uavcan_pub_remoteid_basicid(node),
 	_uavcan_pub_remoteid_location(node),
-	_uavcan_pub_remoteid_system(node)
+	_uavcan_pub_remoteid_system(node),
+	_uavcan_pub_remoteid_operator_id(node),
+	_uavcan_sub_arm_status(node)
 {
 }
 
@@ -53,6 +55,14 @@ int UavcanRemoteIDController::init()
 	// Setup timer and call back function for periodic updates
 	_timer.setCallback(TimerCbBinder(this, &UavcanRemoteIDController::periodic_update));
 	_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ));
+
+	int res = _uavcan_sub_arm_status.start(ArmStatusBinder(this, &UavcanRemoteIDController::arm_status_sub_cb));
+
+	if (res < 0) {
+		PX4_WARN("ArmStatus sub failed %i", res);
+		return res;
+	}
+
 	return 0;
 }
 
@@ -63,6 +73,7 @@ void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &)
 	send_basic_id();
 	send_location();
 	send_system();
+	send_operator_id();
 }
 
 void UavcanRemoteIDController::send_basic_id()
@@ -254,3 +265,37 @@ void UavcanRemoteIDController::send_system()
 		}
 	}
 }
+
+void UavcanRemoteIDController::send_operator_id()
+{
+	open_drone_id_operator_id_s operator_id;
+
+	if (_open_drone_id_operator_id.copy(&operator_id)) {
+
+		dronecan::remoteid::OperatorID msg {};
+
+		for (unsigned i = 0; i < sizeof(operator_id.id_or_mac); ++i) {
+			msg.id_or_mac.push_back(operator_id.id_or_mac[i]);
+		}
+
+		msg.operator_id_type = operator_id.operator_id_type;
+
+		for (unsigned i = 0; i < sizeof(operator_id.operator_id); ++i) {
+			msg.operator_id.push_back(operator_id.operator_id[i]);
+		}
+
+		_uavcan_pub_remoteid_operator_id.broadcast(msg);
+	}
+}
+
+void
+UavcanRemoteIDController::arm_status_sub_cb(const uavcan::ReceivedDataStructure<dronecan::remoteid::ArmStatus> &msg)
+{
+	open_drone_id_arm_status_s arm_status{};
+
+	arm_status.timestamp = hrt_absolute_time();
+	arm_status.status = msg.status;
+	memcpy(arm_status.error, msg.error.c_str(), sizeof(arm_status.error));
+
+	_open_drone_id_arm_status_pub.publish(arm_status);
+}

src/drivers/uavcan/remoteid.hpp

@@ -33,18 +33,23 @@
 
 #pragma once
 
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/home_position.h>
-#include <uORB/Subscription.hpp>
+#include <uORB/topics/open_drone_id_operator_id.h>
+#include <uORB/topics/open_drone_id_arm_status.h>
 
 #include <uavcan/uavcan.hpp>
 #include <dronecan/remoteid/BasicID.hpp>
 #include <dronecan/remoteid/Location.hpp>
 #include <dronecan/remoteid/System.hpp>
+#include <dronecan/remoteid/ArmStatus.hpp>
+#include <dronecan/remoteid/OperatorID.hpp>
 
 #include <px4_platform_common/module_params.h>
 
@@ -68,6 +73,9 @@ private:
 	void send_basic_id();
 	void send_location();
 	void send_system();
+	void send_operator_id();
+
+	void arm_status_sub_cb(const uavcan::ReceivedDataStructure<dronecan::remoteid::ArmStatus> &msg);
 
 	uavcan::INode &_node;
 
@@ -77,8 +85,16 @@ 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_operator_id{ORB_ID(open_drone_id_operator_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::System> _uavcan_pub_remoteid_system;
+	uavcan::Publisher<dronecan::remoteid::OperatorID> _uavcan_pub_remoteid_operator_id;
+
+	using ArmStatusBinder = uavcan::MethodBinder < UavcanRemoteIDController *,
+	      void (UavcanRemoteIDController::*)(const uavcan::ReceivedDataStructure<dronecan::remoteid::ArmStatus> &) >;
+
+	uavcan::Subscriber<dronecan::remoteid::ArmStatus, ArmStatusBinder> _uavcan_sub_arm_status;
 };