Cyphal: divide EscClient into 2 publishers, so setpoint and readiness are 2 different ports now

src/drivers/cyphal/Actuators/EscClient.hpp

@@ -63,16 +63,14 @@ using std::isfinite;
 #include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
 #include <reg/udral/service/common/Readiness_0_1.h>
 
-/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status
+class ReadinessPublisher : public UavcanPublisher
-class UavcanEscController : public UavcanPublisher
 {
 public:
-	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
 
-	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
+	ReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr) :
-		UavcanPublisher(handle, pmgr, "udral.", "esc") { };
+		UavcanPublisher(handle, pmgr, "udral.", "readiness") { };
 
-	~UavcanEscController() {};
+	~ReadinessPublisher() {};
 
 	void update() override
 	{
@@ -95,11 +93,86 @@ public:
 		if (hrt_absolute_time() > _previous_pub_time + READINESS_PUBLISH_PERIOD) {
 			publish_readiness();
 		}
+	}
+
+	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
+	hrt_abstime _previous_pub_time = 0;
+
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+	actuator_armed_s _armed {};
+
+	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
+	uint64_t _actuator_test_timestamp{0};
+
+	CanardTransferID _arming_transfer_id;
+
+	void publish_readiness()
+	{
+		const hrt_abstime now = hrt_absolute_time();
+		_previous_pub_time = now;
+
+		size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+
+		// Only publish if we have a valid publication ID set
+		if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) {
+			return;
+		}
+
+		reg_udral_service_common_Readiness_0_1 msg_arming {};
+
+		if (_armed.armed || _actuator_test_timestamp + 100_ms > now) {
+			msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
+
+		} else if (_armed.prearmed) {
+			msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY;
+
+		} else {
+			msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP;
+		}
+
+		uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+		CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id));
+		const CanardTransferMetadata transfer_metadata = {
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindMessage,
+			.port_id        = arming_pid,                // This is the subject-ID.
+			.remote_node_id = CANARD_NODE_ID_UNSET,      // Messages cannot be unicast, so use UNSET.
+			.transfer_id    = _arming_transfer_id,
+		};
+
+		int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
+				&payload_size);
+
+		if (result == 0) {
+			// set the data ready in the buffer and chop if needed
+			++_arming_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+						       &transfer_metadata,
+						       payload_size,
+						       &arming_payload_buffer);
+		}
+	};
+};
+
+/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status
+class UavcanEscController : public UavcanPublisher
+{
+public:
+	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
+
+	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
+		UavcanPublisher(handle, pmgr, "udral.", "esc") { };
+
+	~UavcanEscController() {};
+
+	void update() override
+	{
 	};
 
 	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
 	{
-		if (_port_id > 0) {
+		if (_port_id > 0 && _port_id != CANARD_PORT_ID_UNSET) {
 			reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
 			size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
 
@@ -148,64 +221,5 @@ private:
 	 */
 	void esc_status_sub_cb(const CanardRxTransfer &msg);
 
-	void publish_readiness()
-	{
-		const hrt_abstime now = hrt_absolute_time();
-		_previous_pub_time = now;
-
-		size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
-
-		// Only publish if we have a valid publication ID set
-		if (_port_id == 0) {
-			return;
-		}
-
-		reg_udral_service_common_Readiness_0_1 msg_arming {};
-
-		if (_armed.armed || _actuator_test_timestamp + 100_ms > now) {
-			msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
-
-		} else if (_armed.prearmed) {
-			msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY;
-
-		} else {
-			msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP;
-		}
-
-		uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
-
-		CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
-		const CanardTransferMetadata transfer_metadata = {
-			.priority       = CanardPriorityNominal,
-			.transfer_kind  = CanardTransferKindMessage,
-			.port_id        = arming_pid,                // This is the subject-ID.
-			.remote_node_id = CANARD_NODE_ID_UNSET,      // Messages cannot be unicast, so use UNSET.
-			.transfer_id    = _arming_transfer_id,
-		};
-
-		int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
-				&payload_size);
-
-		if (result == 0) {
-			// set the data ready in the buffer and chop if needed
-			++_arming_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
-			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
-						       &transfer_metadata,
-						       payload_size,
-						       &arming_payload_buffer);
-		}
-	};
-
 	uint8_t _rotor_count {0};
-
-	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
-	hrt_abstime _previous_pub_time = 0;
-
-	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
-	actuator_armed_s _armed {};
-
-	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
-	uint64_t _actuator_test_timestamp{0};
-
-	CanardTransferID _arming_transfer_id;
 };

src/drivers/cyphal/ParamManager.hpp

@@ -116,8 +116,9 @@ public:
 private:
 
 
-	const UavcanParamBinder _uavcan_params[13] {
+	const UavcanParamBinder _uavcan_params[14] {
 		{"uavcan.pub.udral.esc.0.id",                	"UCAN1_ESC_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.udral.readiness.0.id",            	"UCAN1_READ_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.servo.0.id",              	"UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.gps.0.id",                	"UCAN1_GPS_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.actuator_outputs.0.id",   	"UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},

src/drivers/cyphal/PublicationManager.hpp

@@ -67,7 +67,7 @@
 /* Preprocessor calculation of publisher count */
 
 #define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \
-	CONFIG_CYPHAL_ESC_CONTROLLER + \
+	2 * CONFIG_CYPHAL_ESC_CONTROLLER + \
 	CONFIG_CYPHAL_READINESS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
@@ -133,6 +133,14 @@ private:
 			"udral.esc",
 			0
 		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
+			{
+				return new ReadinessPublisher(handle, pmgr);
+			},
+			"udral.readiness",
+			0
+		},
 #endif
 #if CONFIG_CYPHAL_READINESS_PUBLISHER
 		{

src/drivers/cyphal/parameters.c

@@ -162,6 +162,15 @@ PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1);
  */
 PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1);
 
+/**
+ * Cyphal ESC readiness port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_READ_PUB, -1);
+
 /**
  * Cyphal GPS publication port ID.
  *