From 6a1cefd7a6e8b4a6273754f8ae82db3dde77d4c3 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 7 Jul 2025 20:01:29 -0800 Subject: [PATCH] uavcan: esc: fix uavcan ESC control This fixes a number of issues with uavcan ESC control. - Motor Testing when using a CANnode as a PWM expander now works by allowing the FunctionMotors class to perform prearm control - The esc.RawCommand message is now always published for configured uavcan ESC outputs. Previously prior to arming the message would be published empty, which causes certain ESCs to enter an error state. - Useless and redundant code has been removed and small name changes have been applied. --- src/drivers/uavcan/actuators/esc.cpp | 61 ++++++++----------- src/drivers/uavcan/actuators/esc.hpp | 10 ++- src/drivers/uavcan/uavcan_main.cpp | 12 ---- .../uavcannode/Subscribers/ESCRawCommand.hpp | 15 +++-- .../mixer_module/functions/FunctionMotors.hpp | 4 ++ src/lib/mixer_module/mixer_module.cpp | 4 +- src/lib/mixer_module/mixer_module.hpp | 2 +- 7 files changed, 45 insertions(+), 63 deletions(-) diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index 3eb07e08d5..aaf3cd6670 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -74,15 +74,22 @@ UavcanEscController::init() _uavcan_pub_raw_cmd.getTransferSender().setIfaceMask(iface_mask); } + char param_name[17]; + + for (unsigned i = 0; i < MAX_ACTUATORS; ++i) { + snprintf(param_name, sizeof(param_name), "UAVCAN_EC_FUNC%d", i + 1); + _param_handles[i] = param_find(param_name); + } + return res; } void -UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) +UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs) { - /* - * Rate limiting - we don't want to congest the bus - */ + // TODO: remove stop_motors as it's unnecessary and unused almost everywhere + + // TODO: configurable rate limit const auto timestamp = _node.getMonotonicTime(); if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { @@ -91,44 +98,28 @@ UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUA _prev_cmd_pub = timestamp; - /* - * Fill the command message - * If unarmed, we publish an empty message anyway - */ uavcan::equipment::esc::RawCommand msg; - for (unsigned i = 0; i < num_outputs; i++) { - if (stop_motors || outputs[i] == DISARMED_OUTPUT_VALUE) { - msg.cmd.push_back(static_cast(0)); + // directly output values from mixer + for (unsigned i = 0; i < total_outputs; i++) { + msg.cmd.push_back(static_cast(outputs[i])); + } - } else { - msg.cmd.push_back(static_cast(outputs[i])); + // but only output as many channels as are configured + uint8_t min_size = 0; + + for (int i = 0; i < MAX_ACTUATORS; i++) { + int32_t val = 0; + + if (param_get(_param_handles[i], &val) == 0) { + if (val > 0) { + min_size = i + 1; + } } } - /* - * Remove channels that are always zero. - * The objective of this optimization is to avoid broadcasting multi-frame transfers when a single frame - * transfer would be enough. This is a valid optimization as the UAVCAN specification implies that all - * non-specified ESC setpoints should be considered zero. - * The positive outcome is a (marginally) lower bus traffic and lower CPU load. - * - * From the standpoint of the PX4 architecture, however, this is a hack. It should be investigated why - * the mixer returns more outputs than are actually used. - */ - for (int index = int(msg.cmd.size()) - 1; index >= _max_number_of_nonzero_outputs; index--) { - if (msg.cmd[index] != 0) { - _max_number_of_nonzero_outputs = index + 1; - break; - } - } + msg.cmd.resize(min_size); - msg.cmd.resize(_max_number_of_nonzero_outputs); - - /* - * Publish the command message to the bus - * Note that for a quadrotor it takes one CAN frame - */ _uavcan_pub_raw_cmd.broadcast(msg); } diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 516b147214..aceeed1ba9 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -53,13 +53,13 @@ #include #include #include +#include class UavcanEscController { public: static constexpr int MAX_ACTUATORS = esc_status_s::CONNECTED_ESC_MAX; static constexpr unsigned MAX_RATE_HZ = 400; - static constexpr uint16_t DISARMED_OUTPUT_VALUE = UINT16_MAX; static_assert(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize >= MAX_ACTUATORS, "Too many actuators"); @@ -69,7 +69,7 @@ public: int init(); - void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs); + void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs); /** * Sets the number of rotors and enable timer @@ -111,8 +111,6 @@ private: uavcan::Publisher _uavcan_pub_raw_cmd; uavcan::Subscriber _uavcan_sub_status; - /* - * ESC states - */ - uint8_t _max_number_of_nonzero_outputs{0}; + + param_t _param_handles[MAX_ACTUATORS] {PARAM_INVALID}; }; diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 05fd6201d2..4bb86cac44 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -602,18 +602,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) PX4_DEBUG("sensor bridge '%s' init ok", br->get_name()); } -#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) - - // Ensure we don't exceed maximum limits and assumptions. FIXME: these should be static assertions - if (UavcanEscController::max_output_value() >= UavcanEscController::DISARMED_OUTPUT_VALUE - || UavcanEscController::max_output_value() > (int)UINT16_MAX) { - PX4_ERR("ESC max output value assertion failed"); - return -EINVAL; - } - - _mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE); -#endif - /* Set up shared service clients */ _param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset)); _param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode)); diff --git a/src/drivers/uavcannode/Subscribers/ESCRawCommand.hpp b/src/drivers/uavcannode/Subscribers/ESCRawCommand.hpp index 83604fd161..0cd477a708 100644 --- a/src/drivers/uavcannode/Subscribers/ESCRawCommand.hpp +++ b/src/drivers/uavcannode/Subscribers/ESCRawCommand.hpp @@ -80,9 +80,7 @@ public: private: void callback(const uavcan::ReceivedDataStructure &msg) { - - actuator_motors_s actuator_motors; - + actuator_motors_s actuator_motors = {}; actuator_motors.timestamp = hrt_absolute_time(); actuator_motors.timestamp_sample = actuator_motors.timestamp; @@ -91,13 +89,18 @@ private: break; } - // Normalized to -8192, 8191 in uavcan. actuator_motors is -1 to 1 - actuator_motors.control[i] = msg.cmd[i] / 8192.f; + // Treat zero as disarmed value + if (msg.cmd[i] == 0) { + // NAN maps to disarmed + actuator_motors.control[i] = NAN; + } else { + // Normalized to -8192, 8191 in uavcan. actuator_motors is -1 to 1 + actuator_motors.control[i] = msg.cmd[i] / 8192.f; + } } _actuator_motors_pub.publish(actuator_motors); - } uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; diff --git a/src/lib/mixer_module/functions/FunctionMotors.hpp b/src/lib/mixer_module/functions/FunctionMotors.hpp index efba281884..76e2b274f0 100644 --- a/src/lib/mixer_module/functions/FunctionMotors.hpp +++ b/src/lib/mixer_module/functions/FunctionMotors.hpp @@ -68,7 +68,11 @@ public: float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Motor1]; } +#if defined(CONFIG_DRIVERS_UAVCANNODE) + bool allowPrearmControl() const override { return true; } +#else bool allowPrearmControl() const override { return false; } +#endif uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; } diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 56a2d61356..2de38f7370 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -541,8 +541,6 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const void MixingOutput::output_limit_calc(const bool armed, const int num_channels, const float output[MAX_ACTUATORS]) { - const bool pre_armed = armNoThrottle(); - // time to slowly ramp up the ESCs static constexpr hrt_abstime RAMP_TIME_US = 500_ms; @@ -589,7 +587,7 @@ MixingOutput::output_limit_calc(const bool armed, const int num_channels, const */ auto local_limit_state = _output_state; - if (pre_armed) { + if (isPrearmed()) { local_limit_state = OutputLimitState::ON; } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index c55b72da4e..83828ab166 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -209,7 +209,7 @@ protected: private: - bool armNoThrottle() const + bool isPrearmed() const { return (_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode; }