Cyphal: add zubax.telega.CompactFeedback

src/drivers/cyphal/Actuators/EscClient.hpp

@@ -56,13 +56,17 @@
 #include <uORB/topics/actuator_test.h>
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
+#include "../Subscribers/DynamicPortSubscriber.hpp"
+#include "../Publishers/Publisher.hpp"
 #include <lib/mixer_module/mixer_module.hpp>
+#include <systemlib/mavlink_log.h>
 
 // UDRAL Specification Messages
 using std::isfinite;
 #include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
 #include <reg/udral/service/common/Readiness_0_1.h>
 
+
 class ReadinessPublisher : public UavcanPublisher
 {
 public:
@@ -223,3 +227,64 @@ private:
 
 	uint8_t _rotor_count {0};
 };
+
+class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UavcanEscFeedbackSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "zubax.", "feedback", instance) {}
+
+	void subscribe() override
+	{
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		const ZubaxCompactFeedback* feedback = ((const ZubaxCompactFeedback*)(receive.payload));
+		uint8_t esc_index = 0;
+		float voltage = 0.2 * feedback->dc_voltage;
+		float current = 0.2 * feedback->dc_current;
+		int32_t velocity = 0.10472 * feedback->velocity;
+
+		mavlink_log_info(&_mavlink_log_pub, "zubax.feedback size is %d", receive.payload_size);
+
+		if (esc_index < esc_status_s::CONNECTED_ESC_MAX) {
+			auto &ref = _esc_status.esc[esc_index];
+
+			ref.timestamp       = hrt_absolute_time();
+			ref.esc_address     = receive.metadata.remote_node_id;
+			ref.esc_voltage     = voltage;
+			ref.esc_current     = current;
+			ref.esc_temperature = NAN;
+			ref.esc_rpm         = velocity;
+			ref.esc_errorcount  = 0;
+
+			_esc_status.esc_count = 4; // _rotor_count;
+			_esc_status.counter += 1;
+			_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
+			_esc_status.esc_online_flags = 7; // check_escs_status();
+			_esc_status.esc_armed_flags = 7 ; // (1 << _rotor_count) - 1;
+			_esc_status.timestamp = hrt_absolute_time();
+			_esc_status_pub.publish(_esc_status);
+		}
+	};
+
+private:
+	// https://telega.zubax.com/interfaces/cyphal.html#compact
+	struct ZubaxCompactFeedback {
+		uint32_t dc_voltage 		: 11;
+		int32_t dc_current 		: 12;
+		int32_t phase_current_amplitude	: 12;
+		int32_t velocity 		: 13;
+		int8_t demand_factor_pct 	: 8;
+	};
+
+	esc_status_s	_esc_status{};
+	uORB::PublicationMulti<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
+	orb_advert_t _mavlink_log_pub{nullptr};
+};

src/drivers/cyphal/ParamManager.hpp

@@ -116,7 +116,7 @@ public:
 private:
 
 
-	const UavcanParamBinder _uavcan_params[14] {
+	const UavcanParamBinder _uavcan_params[15] {
 		{"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},
@@ -131,6 +131,7 @@ private:
 		{"uavcan.sub.udral.legacy_bms.0.id",         	"UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.sub.uorb.sensor_gps.0.id",    		"UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.uorb.sensor_gps.0.id",    		"UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.0.id",            	"UCAN1_FB_SUB",			px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_SUB"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_SUB"},
 	};

src/drivers/cyphal/SubscriptionManager.hpp

@@ -45,6 +45,10 @@
 #define CONFIG_CYPHAL_ESC_SUBSCRIBER 0
 #endif
 
+#ifndef CONFIG_CYPHAL_ESC_CONTROLLER
+#define CONFIG_CYPHAL_ESC_CONTROLLER 0
+#endif
+
 #ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
 #define CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 0
 #endif
@@ -65,6 +69,7 @@
 
 #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \
+	CONFIG_CYPHAL_ESC_CONTROLLER + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \
 	CONFIG_CYPHAL_BMS_SUBSCRIBER + \
 	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
@@ -72,6 +77,7 @@
 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
 #include <uavcan/node/port/List_0_1.h>
+#include "Actuators/EscClient.hpp"
 #include "Subscribers/DynamicPortSubscriber.hpp"
 #include "CanardInterface.hpp"
 
@@ -132,6 +138,16 @@ private:
 			0
 		},
 #endif
+#if CONFIG_CYPHAL_ESC_CONTROLLER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 0);
+			},
+			"zubax.feedback",
+			0
+		},
+#endif
 #if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
 		{
 			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *

src/drivers/cyphal/parameters.c

@@ -171,6 +171,15 @@ PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1);
  */
 PARAM_DEFINE_INT32(UCAN1_READ_PUB, -1);
 
+/**
+ * Cyphal ESC zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB_SUB, -1);
+
 /**
  * Cyphal GPS publication port ID.
  *