diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index f4d1d81b7d..8dbe2aff90 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -37,12 +37,11 @@ * Client-side implementation of UDRAL specification ESC service * * Publishes the following Cyphal messages: - * reg.drone.service.actuator.common.sp.Value8.0.1 - * reg.drone.service.common.Readiness.0.1 + * reg.udral.service.actuator.common.sp.Value31.0.1 + * reg.udral.service.common.Readiness.0.1 * * Subscribes to the following Cyphal messages: - * reg.drone.service.actuator.common.Feedback.0.1 - * reg.drone.service.actuator.common.Status.0.1 + * zubax.telega.CompactFeedback.0.1 * * @author Pavel Kirienko * @author Jacob Crabill @@ -51,7 +50,7 @@ #pragma once #include -#include +#include #include #include #include @@ -59,7 +58,6 @@ #include "../Subscribers/DynamicPortSubscriber.hpp" #include "../Publishers/Publisher.hpp" #include -#include // UDRAL Specification Messages using std::isfinite; @@ -117,7 +115,6 @@ public: 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; } @@ -140,8 +137,8 @@ public: 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. + .port_id = arming_pid, + .remote_node_id = CANARD_NODE_ID_UNSET, .transfer_id = _arming_transfer_id, }; @@ -149,8 +146,7 @@ public: &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. + ++_arming_transfer_id; result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, &transfer_metadata, payload_size, @@ -159,7 +155,6 @@ public: }; }; -/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status class UavcanEscController : public UavcanPublisher { public: @@ -190,6 +185,7 @@ public: } uint16_t payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_value_ARRAY_CAPACITY_]; + for (uint8_t i = 0; i < _max_number_of_nonzero_outputs; i++) { payload_buffer[i] = nunavutFloat16Pack(outputs[i] / 8192.0); } @@ -209,20 +205,8 @@ public: &payload_buffer); } - /** - * Sets the number of rotors - */ - void set_rotor_count(uint8_t count) { _rotor_count = count; } - private: - /** - * ESC status message reception will be reported via this callback. - */ - void esc_status_sub_cb(const CanardRxTransfer &msg); - uint8_t _max_number_of_nonzero_outputs{1}; - uint8_t _rotor_count {0}; - orb_advert_t _mavlink_log_pub{nullptr}; }; class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber @@ -235,44 +219,59 @@ public: { _canard_handle.RxSubscribe(CanardTransferKindMessage, _subj_sub._canard_sub.port_id, - reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_, + zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &_subj_sub._canard_sub); + _esc_status.esc_armed_flags |= 1 << _instance; + _esc_status.esc_count++; + }; + + void unsubscribe() override + { + _canard_handle.RxUnsubscribe(CanardTransferKindMessage, _subj_sub._canard_sub.port_id); + _esc_status.esc_armed_flags &= ~(1 << _instance); + _esc_status.esc_count--; }; 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; + if (_instance >= esc_status_s::CONNECTED_ESC_MAX) { + return; + } - mavlink_log_info(&_mavlink_log_pub, "zubax.feedback size is %d", receive.payload_size); + auto &ref = _esc_status.esc[_instance]; + const ZubaxCompactFeedback *feedback = ((const ZubaxCompactFeedback *)(receive.payload)); - 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 = 0.2 * feedback->dc_voltage; + ref.esc_current = 0.2 * feedback->dc_current; + ref.esc_temperature = NAN; + ref.esc_rpm = feedback->velocity * RAD_PER_SEC_TO_RPM; + ref.esc_errorcount = 0; - 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.counter++; + _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; + _esc_status.esc_armed_flags = (1 << _esc_status.esc_count) - 1; + _esc_status.timestamp = hrt_absolute_time(); + _esc_status_pub.publish(_esc_status); - _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); + _esc_status.esc_online_flags = 0; + const hrt_abstime now = hrt_absolute_time(); + + for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) { + if (_esc_status.esc[index].timestamp > 0 && now - _esc_status.esc[index].timestamp < 1200_ms) { + _esc_status.esc_online_flags |= (1 << index); + } } }; private: + static constexpr float RAD_PER_SEC_TO_RPM = 9.5492968; + static constexpr size_t zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES = 7; + // https://telega.zubax.com/interfaces/cyphal.html#compact +#pragma pack(push, 1) struct ZubaxCompactFeedback { uint32_t dc_voltage : 11; int32_t dc_current : 12; @@ -280,8 +279,10 @@ private: int32_t velocity : 13; int8_t demand_factor_pct : 8; }; +#pragma pack(pop) + static_assert(sizeof(ZubaxCompactFeedback) == zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES); - esc_status_s _esc_status{}; - uORB::PublicationMulti _esc_status_pub{ORB_ID(esc_status)}; - orb_advert_t _mavlink_log_pub{nullptr}; + static esc_status_s _esc_status; + + uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; }; diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp index 94030971b5..5a288274cf 100644 --- a/src/drivers/cyphal/Cyphal.cpp +++ b/src/drivers/cyphal/Cyphal.cpp @@ -62,6 +62,8 @@ using namespace time_literals; CyphalNode *CyphalNode::_instance; +esc_status_s UavcanEscFeedbackSubscriber::_esc_status; + CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), diff --git a/src/drivers/cyphal/ParamManager.cpp b/src/drivers/cyphal/ParamManager.cpp index c792101ad4..0dba575cde 100644 --- a/src/drivers/cyphal/ParamManager.cpp +++ b/src/drivers/cyphal/ParamManager.cpp @@ -98,13 +98,16 @@ bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &nam { size_t number_of_integer_registers = sizeof(_uavcan_params) / sizeof(UavcanParamBinder); size_t number_of_type_registers = sizeof(_type_registers) / sizeof(CyphalTypeRegister); + if (id < sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) { strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); name.name.count = strlen(_uavcan_params[id].uavcan_name); + } else if (id < number_of_integer_registers + number_of_type_registers) { id -= number_of_integer_registers; strncpy((char *)name.name.elements, _type_registers[id].name, strlen(_type_registers[id].name)); name.name.count = strlen(_type_registers[id].name); + } else { return false; } diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp index 056be73785..e1330c4e5f 100644 --- a/src/drivers/cyphal/ParamManager.hpp +++ b/src/drivers/cyphal/ParamManager.hpp @@ -120,7 +120,7 @@ public: private: - const UavcanParamBinder _uavcan_params[15] { + const UavcanParamBinder _uavcan_params[22] { {"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}, @@ -135,14 +135,28 @@ 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.zubax.feedback.0.id", "UCAN1_FB0_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.1.id", "UCAN1_FB1_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.2.id", "UCAN1_FB2_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.3.id", "UCAN1_FB3_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.4.id", "UCAN1_FB4_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.5.id", "UCAN1_FB5_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.6.id", "UCAN1_FB6_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.7.id", "UCAN1_FB7_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"}, }; - CyphalTypeRegister _type_registers[3] { + CyphalTypeRegister _type_registers[10] { {"uavcan.pub.udral.esc.0.type", "reg.udral.service.actuator.common.sp.Vector31"}, {"uavcan.pub.udral.readiness.0.type", "reg.udral.service.common.Readiness.0.1"}, {"uavcan.sub.zubax.feedback.0.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.1.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.2.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.3.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.4.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.5.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.6.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.7.type", "zubax.telega.CompactFeedback.0.1"}, }; }; diff --git a/src/drivers/cyphal/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp index eb9609aa5e..c9218d2f88 100644 --- a/src/drivers/cyphal/SubscriptionManager.hpp +++ b/src/drivers/cyphal/SubscriptionManager.hpp @@ -69,7 +69,7 @@ #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \ CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \ - CONFIG_CYPHAL_ESC_CONTROLLER + \ + 8 * CONFIG_CYPHAL_ESC_CONTROLLER + \ CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \ CONFIG_CYPHAL_BMS_SUBSCRIBER + \ CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER @@ -147,6 +147,62 @@ private: "zubax.feedback", 0 }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 1); + }, + "zubax.feedback", + 1 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 2); + }, + "zubax.feedback", + 2 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 3); + }, + "zubax.feedback", + 3 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 4); + }, + "zubax.feedback", + 4 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 5); + }, + "zubax.feedback", + 5 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 6); + }, + "zubax.feedback", + 6 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 7); + }, + "zubax.feedback", + 7 + }, #endif #if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 { diff --git a/src/drivers/cyphal/parameters.c b/src/drivers/cyphal/parameters.c index 7564b5b3ca..4731f1c14e 100644 --- a/src/drivers/cyphal/parameters.c +++ b/src/drivers/cyphal/parameters.c @@ -172,13 +172,76 @@ PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1); PARAM_DEFINE_INT32(UCAN1_READ_PUB, -1); /** - * Cyphal ESC zubax feedback port ID. + * Cyphal ESC 0 zubax feedback port ID. * * @min -1 * @max 6143 * @group Cyphal */ -PARAM_DEFINE_INT32(UCAN1_FB_SUB, -1); +PARAM_DEFINE_INT32(UCAN1_FB0_SUB, -1); + +/** + * Cyphal ESC 1 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB1_SUB, -1); + +/** + * Cyphal ESC 2 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB2_SUB, -1); + +/** + * Cyphal ESC 3 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB3_SUB, -1); + +/** + * Cyphal ESC 4 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB4_SUB, -1); + +/** + * Cyphal ESC 5 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB5_SUB, -1); + +/** + * Cyphal ESC 6 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB6_SUB, -1); + +/** + * Cyphal ESC 7 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB7_SUB, -1); /** * Cyphal GPS publication port ID.