diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index 27cb715cae..c5a0c41d5b 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -56,13 +56,17 @@ #include #include #include +#include "../Subscribers/DynamicPortSubscriber.hpp" +#include "../Publishers/Publisher.hpp" #include +#include // UDRAL Specification Messages using std::isfinite; #include #include + 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_pub{ORB_ID(esc_status)}; + orb_advert_t _mavlink_log_pub{nullptr}; +}; diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp index ef7c6ba504..3eab0818bc 100644 --- a/src/drivers/cyphal/ParamManager.hpp +++ b/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"}, }; diff --git a/src/drivers/cyphal/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp index ce5b12a54d..eb9609aa5e 100644 --- a/src/drivers/cyphal/SubscriptionManager.hpp +++ b/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 #include #include +#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 * diff --git a/src/drivers/cyphal/parameters.c b/src/drivers/cyphal/parameters.c index 2cb06ddc2e..7564b5b3ca 100644 --- a/src/drivers/cyphal/parameters.c +++ b/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. *