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; }