From 515543b1c5a3c1d11053c57f7f9eca803c94de78 Mon Sep 17 00:00:00 2001 From: PonomarevDA Date: Mon, 24 Apr 2023 00:11:39 +0300 Subject: [PATCH] Cyphal: divide EscClient into 2 publishers, so setpoint and readiness are 2 different ports now --- src/drivers/cyphal/Actuators/EscClient.hpp | 146 +++++++++++---------- src/drivers/cyphal/ParamManager.hpp | 3 +- src/drivers/cyphal/PublicationManager.hpp | 10 +- src/drivers/cyphal/parameters.c | 9 ++ 4 files changed, 100 insertions(+), 68 deletions(-) diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index c546c5b8b4..27cb715cae 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -63,16 +63,14 @@ using std::isfinite; #include #include -/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status -class UavcanEscController : public UavcanPublisher +class ReadinessPublisher : public UavcanPublisher { public: - static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) : - UavcanPublisher(handle, pmgr, "udral.", "esc") { }; + ReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr) : + 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(static_cast(_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(static_cast(_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; }; diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp index 67c5dbc20b..ef7c6ba504 100644 --- a/src/drivers/cyphal/ParamManager.hpp +++ b/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}, diff --git a/src/drivers/cyphal/PublicationManager.hpp b/src/drivers/cyphal/PublicationManager.hpp index c996e797e9..8defb8d8f4 100644 --- a/src/drivers/cyphal/PublicationManager.hpp +++ b/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 { diff --git a/src/drivers/cyphal/parameters.c b/src/drivers/cyphal/parameters.c index becd33fc42..2cb06ddc2e 100644 --- a/src/drivers/cyphal/parameters.c +++ b/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. *