uavcan_v1: Fix bugs in MixingOutput / EscClient

Also add commented-out code for use with PR-16808
(MixingOutput + output_control)
Bench-tested PWM output on a Pixracer via UAVCANv1 ESC commands from a
Pixhawk 4.

src/drivers/uavcan_v1/Actuators/EscClient.hpp

@@ -129,10 +129,19 @@ public:
 		if (_port_id > 0) {
 			reg_drone_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
 
-			for (uint8_t i = 0; i < num_outputs; i++) {
+			for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
-				msg_sp.value[i] = outputs[i];
+				if (i < num_outputs) {
+					msg_sp.value[i] = static_cast<float>(outputs[i]);
+
+				} else {
+					// "unset" values published as NaN
+					msg_sp.value[i] = NAN;
+				}
 			}
 
+			PX4_INFO("Publish %d values %f, %f, %f, %f", num_outputs, (double)msg_sp.value[0], (double)msg_sp.value[1],
+				 (double)msg_sp.value[2], (double)msg_sp.value[3]);
+
 			uint8_t esc_sp_payload_buffer[reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
 
 			CanardTransfer transfer = {

src/drivers/uavcan_v1/ParamManager.hpp

@@ -67,7 +67,7 @@ private:
 		{"uavcan.pub.esc.0.id",   "UCAN1_ESC_PUB"},
 		{"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB"},
 		{"uavcan.pub.gps.0.id",   "UCAN1_GPS_PUB"},
-		{"uavcan.sub.esc.0.id",   "UCAN1_ESC_PID"},
+		{"uavcan.sub.esc.0.id",   "UCAN1_ESC0_PID"},
 		{"uavcan.sub.gps.0.id",   "UCAN1_GPS0_PID"},
 		{"uavcan.sub.gps.1.id",   "UCAN1_GPS1_PID"},
 		{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_PID"},

src/drivers/uavcan_v1/Subscribers/Esc.hpp

@@ -42,8 +42,12 @@
 
 #pragma once
 
+/// For use with PR-16808 once merged
+// #include <uORB/topics/output_control.h>
+
 // DS-15 Specification Messages
 #include <reg/drone/service/actuator/common/sp/Vector8_0_1.h>
+#include <reg/drone/service/common/Readiness_0_1.h>
 
 #include "Subscriber.hpp"
 
@@ -63,15 +67,20 @@ public:
 				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 				  &_canard_sub);
 
-		/** TODO: Add additional ESC-service messages: reg.drone.service.common.Readiness
+		// Subscribe to messages reg.drone.service.common.Readiness.0.1
-		 */
+		canardRxSubscribe(&_canard_instance,
+				  CanardTransferKindMessage,
+				  static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1),
+				  reg_drone_service_common_Readiness_0_1_EXTENT_BYTES_,
+				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+				  &_canard_sub_readiness);
 	};
 
 	void callback(const CanardTransfer &receive) override
 	{
 		// Test with Yakut:
 		// export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)"
-		// yakut pub 22.reg.drone.service.actuator.common.sp.Vector8.0.1 '{value: {1000, 2000, 3000, 4000}, longitude: 2.34, altitude: {meter: 0.5}}'
+		// yakut pub 22.reg.drone.service.actuator.common.sp.Vector8.0.1 '{value: [1000, 2000, 3000, 4000, 0, 0, 0, 0]}'
 		PX4_INFO("EscCallback");
 
 		reg_drone_service_actuator_common_sp_Vector8_0_1 esc {};
@@ -86,7 +95,19 @@ public:
 		PX4_INFO("values[0-3] = {%f, %f, %f, %f}", val1, val2, val3, val4);
 		/// do something with the data
 
-		/// TODO: Publish to output_control_mc
+		/// For use with PR-16808 once merged
+		// output_control_s outputs;
+
+		// for (uint8_t i = 0; i < 8; i++) {
+		// 	outputs.value[i] = 2.f * (esc.value[i] / 8191.f) - 1.f;
+		// }
+
+		// _output_pub.publish(outputs);
 	};
 
+private:
+	/// For use with PR-16808 once merged
+	// uORB::Publication<output_control_s> _output_pub{ORB_ID(output_control_mc)};
+
+	CanardRxSubscription _canard_sub_readiness;
 };

src/drivers/uavcan_v1/Uavcan.cpp

@@ -90,6 +90,8 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
 	for (auto &subscriber : _subscribers) {
 		subscriber->updateParam();
 	}
+
+	_mixing_output.mixingOutput().updateSubscriptions(false, false);
 }
 
 UavcanNode::~UavcanNode()
@@ -245,6 +247,10 @@ void UavcanNode::Run()
 			// Setting the port-id to 0 disables the subscription
 			subscriber->updateParam();
 		}
+
+		_mixing_output.updateParams();
+
+		_mixing_output.mixingOutput().updateSubscriptions(false, false);
 	}
 
 	perf_begin(_cycle_perf);
@@ -341,7 +347,7 @@ void UavcanNode::Run()
 
 		} else if (result == 1) {
 			// A transfer has been received, process it.
-			PX4_INFO("received Port ID: %d", receive.port_id);
+			// PX4_INFO("received Port ID: %d", receive.port_id);
 
 			if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) {
 				result = handlePnpNodeIDAllocationData(receive);
@@ -495,15 +501,6 @@ void UavcanNode::sendHeartbeat()
 		}
 
 		_uavcan_node_heartbeat_last = transfer.timestamp_usec;
-
-		/// TESTING -- Remove before flight!
-		// Since we don't have a mixer file here yet
-		uint16_t outputs[8] {};
-		outputs[0] = 1000;
-		outputs[1] = 2000;
-		outputs[2] = 3000;
-		outputs[3] = 4000;
-		_mixing_output.updateOutputs(false, outputs, 4, 1);
 	}
 }
 
@@ -690,9 +687,6 @@ int UavcanNode::handleRegisterAccess(const CanardTransfer &receive)
 
 	}
 
-	/// TODO: I'm getting the following error:
-	/// 	"error: the frame size of 2448 bytes is larger than 2048 bytes [-Werror=frame-larger-than=]"
-
 	/// TODO: Access_Response
 	uavcan_register_Access_Response_1_0 response {};
 	response.value = value;
@@ -764,6 +758,6 @@ void UavcanMixingInterface::Run()
 {
 	pthread_mutex_lock(&_node_mutex);
 	_mixing_output.update();
-	_mixing_output.updateSubscriptions(false);
+	_mixing_output.updateSubscriptions(false, false);
 	pthread_mutex_unlock(&_node_mutex);
 }

src/drivers/uavcan_v1/Uavcan.hpp

@@ -114,6 +114,9 @@ public:
 
 	MixingOutput &mixingOutput() { return _mixing_output; }
 
+	/// For use with PR-16808 once merged
+	// const char *get_param_prefix() override { return "UCAN1_ACT"; }
+
 protected:
 	void Run() override;
 private:

src/drivers/uavcan_v1/parameters.c

@@ -87,6 +87,7 @@ PARAM_DEFINE_INT32(UAVCAN_V1_BAT_MD, 0);
 PARAM_DEFINE_INT32(UAVCAN_V1_BAT_ID, 4242);
 
 // Subscription Port IDs
+PARAM_DEFINE_INT32(UCAN1_ESC0_PID, 0);
 PARAM_DEFINE_INT32(UCAN1_GPS0_PID, 0);
 PARAM_DEFINE_INT32(UCAN1_GPS1_PID, 0);
 PARAM_DEFINE_INT32(UCAN1_BMS0_PID, 0);