From 09ebd21e554570148ed16c7d4261e277a3f4f080 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 9 Jul 2025 12:39:16 -0800 Subject: [PATCH] mixer: remove used flag stop_outputs dshot: fix motor test on CANnode Also includes fixes for the DShot driver since stop_outputs is removed. The esc info command has been removed because it doesn't work with AM32, can only be used via command line, and complicates the driver --- .../src/px4/stm/stm32_common/dshot/dshot.c | 6 +- src/drivers/actuators/vertiq_io/vertiq_io.cpp | 2 +- src/drivers/actuators/vertiq_io/vertiq_io.hpp | 2 +- src/drivers/actuators/voxl_esc/voxl_esc.cpp | 4 +- src/drivers/actuators/voxl_esc/voxl_esc.hpp | 2 +- src/drivers/cyphal/Actuators/EscClient.hpp | 2 +- src/drivers/cyphal/Cyphal.cpp | 4 +- src/drivers/cyphal/Cyphal.hpp | 2 +- src/drivers/dshot/DShot.cpp | 220 ++++++------------ src/drivers/dshot/DShot.h | 12 +- src/drivers/linux_pwm_out/linux_pwm_out.cpp | 2 +- src/drivers/linux_pwm_out/linux_pwm_out.hpp | 2 +- src/drivers/pca9685_pwm_out/main.cpp | 4 +- src/drivers/pwm_out/PWMOut.cpp | 2 +- src/drivers/pwm_out/PWMOut.hpp | 2 +- src/drivers/px4io/px4io.cpp | 4 +- src/drivers/roboclaw/Roboclaw.cpp | 12 +- src/drivers/roboclaw/Roboclaw.hpp | 2 +- src/drivers/tap_esc/TAP_ESC.cpp | 2 +- src/drivers/tap_esc/TAP_ESC.hpp | 2 +- src/drivers/uavcan/actuators/esc.cpp | 4 +- src/drivers/uavcan/actuators/esc.hpp | 2 +- src/drivers/uavcan/actuators/servo.cpp | 2 +- src/drivers/uavcan/actuators/servo.hpp | 2 +- src/drivers/uavcan/uavcan_main.cpp | 8 +- src/drivers/uavcan/uavcan_main.hpp | 4 +- src/drivers/voxl2_io/voxl2_io.cpp | 10 +- src/drivers/voxl2_io/voxl2_io.hpp | 2 +- src/lib/mixer_module/mixer_module.cpp | 6 +- src/lib/mixer_module/mixer_module.hpp | 4 +- src/lib/mixer_module/mixer_module_tests.cpp | 2 +- .../gz_bridge/GZMixingInterfaceESC.cpp | 2 +- .../gz_bridge/GZMixingInterfaceESC.hpp | 2 +- .../gz_bridge/GZMixingInterfaceServo.cpp | 2 +- .../gz_bridge/GZMixingInterfaceServo.hpp | 2 +- src/modules/simulation/pwm_out_sim/PWMSim.cpp | 2 +- src/modules/simulation/pwm_out_sim/PWMSim.hpp | 2 +- 37 files changed, 120 insertions(+), 228 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 1ec91f28a2..a667537d9f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -569,11 +569,11 @@ void process_capture_results(uint8_t timer_index, uint8_t channel_index) } /** -* bits 1-11 - throttle value (0-47 are reserved, 48-2047 give 2000 steps of throttle resolution) +* bits 1-11 - throttle value (0-47 are reserved for commands, 48-2047 give 2000 steps of throttle resolution) * bit 12 - dshot telemetry enable/disable * bits 13-16 - XOR checksum **/ -void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) +void dshot_motor_data_set(unsigned channel, uint16_t data, bool telemetry) { uint8_t timer_index = timer_io_channels[channel].timer_index; uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1; @@ -586,7 +586,7 @@ void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) uint16_t packet = 0; uint16_t checksum = 0; - packet |= throttle << DSHOT_THROTTLE_POSITION; + packet |= data << DSHOT_THROTTLE_POSITION; packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION; uint16_t csum_data = packet; diff --git a/src/drivers/actuators/vertiq_io/vertiq_io.cpp b/src/drivers/actuators/vertiq_io/vertiq_io.cpp index 80add143fd..23269d4080 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_io.cpp +++ b/src/drivers/actuators/vertiq_io/vertiq_io.cpp @@ -195,7 +195,7 @@ void VertiqIo::OutputControls(uint16_t outputs[MAX_ACTUATORS]) _serial_interface.ProcessSerialTx(); } -bool VertiqIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool VertiqIo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { #ifdef CONFIG_USE_IFCI_CONFIGURATION diff --git a/src/drivers/actuators/vertiq_io/vertiq_io.hpp b/src/drivers/actuators/vertiq_io/vertiq_io.hpp index db8d37053e..379315d0e7 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_io.hpp +++ b/src/drivers/actuators/vertiq_io/vertiq_io.hpp @@ -94,7 +94,7 @@ public: void print_info(); /** @see OutputModuleInterface */ - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; /** diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index 89aebcbbaf..ec4db9e2d4 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -1196,7 +1196,7 @@ void VoxlEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) } /* OutputModuleInterface */ -bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool VoxlEsc::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { //in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function) @@ -1212,7 +1212,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], } for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { - if (!_outputs_on || stop_motors) { + if (!_outputs_on) { _esc_chans[i].rate_req = 0; } else { diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp index c28d1e1ab9..c5885b20d7 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -81,7 +81,7 @@ public: void print_params(); /** @see OutputModuleInterface */ - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; virtual int init(); diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index 8dbe2aff90..b6f469c90e 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -169,7 +169,7 @@ public: { } - void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) + void update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) { if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) { return; diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp index 5a288274cf..c102f2e5b7 100644 --- a/src/drivers/cyphal/Cyphal.cpp +++ b/src/drivers/cyphal/Cyphal.cpp @@ -435,7 +435,7 @@ void CyphalNode::sendPortList() _uavcan_node_port_List_last = now; } -bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool UavcanMixingInterface::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { // Note: This gets called from MixingOutput from within its update() function @@ -445,7 +445,7 @@ bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX auto publisher = static_cast(_pub_manager.getPublisher("esc")); if (publisher) { - publisher->update_outputs(stop_motors, outputs, num_outputs); + publisher->update_outputs(outputs, num_outputs); } return true; diff --git a/src/drivers/cyphal/Cyphal.hpp b/src/drivers/cyphal/Cyphal.hpp index 0132830fb6..bc2383bd9b 100644 --- a/src/drivers/cyphal/Cyphal.hpp +++ b/src/drivers/cyphal/Cyphal.hpp @@ -87,7 +87,7 @@ public: _node_mutex(node_mutex), _pub_manager(pub_manager) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; void printInfo() { _mixing_output.printStatus(); } diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index db6e16bf7d..92fec8e76e 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -58,6 +58,9 @@ DShot::~DShot() up_dshot_arm(false); perf_free(_cycle_perf); + perf_free(_bdshot_rpm_perf); + perf_free(_dshot_telem_perf); + delete _telemetry; } @@ -248,6 +251,8 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem _last_telemetry_index = telemetry_index; + perf_count(_dshot_telem_perf); + return ret; } @@ -323,10 +328,10 @@ int DShot::handle_new_bdshot_erpm(void) ++telemetry_index; } - - } + perf_count(_bdshot_rpm_perf); + return num_erpms; } @@ -362,59 +367,12 @@ int DShot::send_command_thread_safe(const dshot_command_t command, const int num return 0; } -void DShot::retrieve_and_print_esc_info_thread_safe(const int motor_index) -{ - if (_request_esc_info.load() != nullptr) { - // already in progress (not expected to ever happen) - return; - } - - DShotTelemetry::OutputBuffer output_buffer{}; - output_buffer.motor_index = motor_index; - - // start the request - _request_esc_info.store(&output_buffer); - - // wait until processed - int max_time = 1000; - - while (_request_esc_info.load() != nullptr && max_time-- > 0) { - px4_usleep(1000); - } - - _request_esc_info.store(nullptr); // just in case we time out... - - if (output_buffer.buf_pos == 0) { - PX4_ERR("No data received. If telemetry is setup correctly, try again"); - return; - } - - DShotTelemetry::decodeAndPrintEscInfoPacket(output_buffer); -} - -int DShot::request_esc_info() -{ - _telemetry->redirectOutput(*_request_esc_info.load()); - _waiting_for_esc_info = true; - - int motor_index = _request_esc_info.load()->motor_index; - - _current_command.motor_mask = 1 << motor_index; - _current_command.num_repetitions = 1; - _current_command.command = DShot_cmd_esc_info; - _current_command.save = false; - - PX4_DEBUG("Requesting ESC info for motor %i", motor_index); - return motor_index; -} - void DShot::mixerChanged() { update_num_motors(); - } -bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool DShot::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { if (!_outputs_on) { @@ -424,93 +382,47 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], int requested_telemetry_index = -1; if (_telemetry) { - // check for an ESC info request. We only process it when we're not expecting other telemetry data - if (_request_esc_info.load() != nullptr && !_waiting_for_esc_info && stop_motors - && !_telemetry->expectingData() && !_current_command.valid()) { - requested_telemetry_index = request_esc_info(); - - } else { - requested_telemetry_index = _telemetry->getRequestMotorIndex(); - } + requested_telemetry_index = _telemetry->getRequestMotorIndex(); } - if (stop_motors) { + int telemetry_index = 0; - int telemetry_index = 0; + for (int i = 0; i < (int)num_outputs; i++) { + + uint16_t output = outputs[i]; + + if (output == DSHOT_DISARM_VALUE) { - // when motors are stopped we check if we have other commands to send - for (int i = 0; i < (int)num_outputs; i++) { if (_current_command.valid() && (_current_command.motor_mask & (1 << i))) { - // for some reason we need to always request telemetry when sending a command up_dshot_motor_command(i, _current_command.command, true); } else { up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index); } - telemetry_index += _mixing_output.isFunctionSet(i); - } + } else { - if (_current_command.valid()) { - --_current_command.num_repetitions; - - if (_current_command.num_repetitions == 0 && _current_command.save) { - _current_command.save = false; - _current_command.num_repetitions = 10; - _current_command.command = dshot_command_t::DShot_cmd_save_settings; - } - } - - } else { - int telemetry_index = 0; - - for (int i = 0; i < (int)num_outputs; i++) { - - uint16_t output = outputs[i]; - - if (output == DSHOT_DISARM_VALUE) { - up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index); - - } else { - - // DShot 3D splits the throttle ranges in two. - // This is in terms of DShot values, code below is in terms of actuator_output - // Direction 1) 48 is the slowest, 1047 is the fastest. - // Direction 2) 1049 is the slowest, 2047 is the fastest. - if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) { - if (output >= _param_dshot_3d_dead_l.get() && output < _param_dshot_3d_dead_h.get()) { - output = DSHOT_DISARM_VALUE; - - } else { - bool upper_range = output >= 1000; - - if (upper_range) { - output -= 1000; - - } else { - output = 999 - output; // lower range is inverted - } - - float max_output = 999.f; - float min_output = max_output * _param_dshot_min.get(); - output = math::min(max_output, (min_output + output * (max_output - min_output) / max_output)); - - if (upper_range) { - output += 1000; - } - - } - } - - up_dshot_motor_data_set(i, math::min(output, static_cast(DSHOT_MAX_THROTTLE)), - telemetry_index == requested_telemetry_index); + if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) { + output = convert_output_to_3d_scaling(output); } - telemetry_index += _mixing_output.isFunctionSet(i); + up_dshot_motor_data_set(i, math::min(output, static_cast(DSHOT_MAX_THROTTLE)), + telemetry_index == requested_telemetry_index); } - // clear commands when motors are running - _current_command.clear(); + telemetry_index += _mixing_output.isFunctionSet(i); + } + + // Decrement the command counter + if (_current_command.valid()) { + --_current_command.num_repetitions; + + // Queue a save command after the burst if save has been requested + if (_current_command.num_repetitions == 0 && _current_command.save) { + _current_command.save = false; + _current_command.num_repetitions = 10; + _current_command.command = dshot_command_t::DShot_cmd_save_settings; + } } up_dshot_trigger(); @@ -518,6 +430,36 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], return true; } +uint16_t DShot::convert_output_to_3d_scaling(uint16_t output) +{ + // DShot 3D splits the throttle ranges in two. + // This is in terms of DShot values, code below is in terms of actuator_output + // Direction 1) 48 is the slowest, 1047 is the fastest. + // Direction 2) 1049 is the slowest, 2047 is the fastest. + if (output >= _param_dshot_3d_dead_l.get() && output < _param_dshot_3d_dead_h.get()) { + return DSHOT_DISARM_VALUE; + } + + bool upper_range = output >= 1000; + + if (upper_range) { + output -= 1000; + + } else { + output = 999 - output; // lower range is inverted + } + + float max_output = 999.f; + float min_output = max_output * _param_dshot_min.get(); + output = math::min(max_output, (min_output + output * (max_output - min_output) / max_output)); + + if (upper_range) { + output += 1000; + } + + return output; +} + void DShot::Run() { if (should_exit()) { @@ -542,14 +484,7 @@ void DShot::Run() if (_telemetry) { const int telem_update = _telemetry->update(_num_motors); - // Are we waiting for ESC info? - if (_waiting_for_esc_info) { - if (telem_update != -1) { - _request_esc_info.store(nullptr); - _waiting_for_esc_info = false; - } - - } else if (telem_update >= 0) { + if (telem_update >= 0) { const int need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->latestESCData(), _bidirectional_dshot_enabled); @@ -772,27 +707,6 @@ int DShot::custom_command(int argc, char *argv[]) } } - if (!strcmp(verb, "esc_info")) { - if (!is_running()) { - PX4_ERR("module not running"); - return -1; - } - - if (motor_index == -1) { - PX4_ERR("No motor index specified"); - return -1; - } - - if (!get_instance()->telemetry_enabled()) { - PX4_ERR("Telemetry is not enabled, but required to get ESC info"); - return -1; - } - - get_instance()->retrieve_and_print_esc_info_thread_safe(motor_index); - return 0; - } - - if (!is_running()) { int ret = DShot::task_spawn(argc, argv); @@ -810,6 +724,9 @@ int DShot::print_status() PX4_INFO("Outputs used: 0x%" PRIx32, _output_mask); PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); perf_print_counter(_cycle_perf); + perf_print_counter(_bdshot_rpm_perf); + perf_print_counter(_dshot_telem_perf); + _mixing_output.printStatus(); if (_telemetry) { @@ -881,9 +798,6 @@ After saving, the reversed direction will be regarded as the normal one. So to r PRINT_MODULE_USAGE_COMMAND_DESCR("beep5", "Send Beep pattern 5"); PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("esc_info", "Request ESC information"); - PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based)", false); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index d0300d0470..8137e461e2 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -76,8 +76,6 @@ public: /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - void retrieve_and_print_esc_info_thread_safe(const int motor_index); - /** * Send a dshot command to one or all motors * This is expected to be called from another thread. @@ -92,7 +90,7 @@ public: bool telemetry_enabled() const { return _telemetry != nullptr; } - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: @@ -131,8 +129,6 @@ private: int handle_new_bdshot_erpm(void); - int request_esc_info(); - void Run() override; void update_params(); @@ -141,6 +137,8 @@ private: void handle_vehicle_commands(); + uint16_t convert_output_to_3d_scaling(uint16_t output); + MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uint32_t _reversible_outputs{}; @@ -154,11 +152,9 @@ private: px4::atomic _new_command{nullptr}; - px4::atomic _request_esc_info{nullptr}; bool _outputs_initialized{false}; bool _outputs_on{false}; - bool _waiting_for_esc_info{false}; bool _bidirectional_dshot_enabled{false}; static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS}; @@ -167,6 +163,8 @@ private: int _num_motors{0}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _bdshot_rpm_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot rpm")}; + perf_counter_t _dshot_telem_perf{perf_alloc(PC_COUNT, MODULE_NAME": dshot telem")}; Command _current_command{}; diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 7712973051..9e2dbf1ab4 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -95,7 +95,7 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool LinuxPWMOut::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { _pwm_out->send_output_pwm(outputs, num_outputs); diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.hpp b/src/drivers/linux_pwm_out/linux_pwm_out.hpp index ca3ab3829e..14f7201bd6 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.hpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.hpp @@ -71,7 +71,7 @@ public: int init(); - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index ffe53ce3e7..60d0cc6340 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -70,7 +70,7 @@ public: static int custom_command(int argc, char *argv[]); static int print_usage(const char *reason = nullptr); - bool updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, + bool updateOutputs(uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) override; int print_status() override; @@ -136,7 +136,7 @@ int PCA9685Wrapper::init() return PX4_OK; } -bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, +bool PCA9685Wrapper::updateOutputs(uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) { if (state != STATE::RUNNING) { return false; } diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 0a3cbad4ca..4db1933a22 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -125,7 +125,7 @@ bool PWMOut::update_pwm_out_state(bool on) return true; } -bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool PWMOut::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { /* output to the servos */ diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index 55d498a8fc..da28fa64d5 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -71,7 +71,7 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d10573984d..3e92eb9af2 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -153,7 +153,7 @@ public: uint16_t system_status() const { return _status; } - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: @@ -360,7 +360,7 @@ PX4IO::~PX4IO() perf_free(_interface_write_perf); } -bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool PX4IO::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { for (size_t i = 0; i < num_outputs; i++) { diff --git a/src/drivers/roboclaw/Roboclaw.cpp b/src/drivers/roboclaw/Roboclaw.cpp index d4aeeee5d3..ba308b6aa6 100644 --- a/src/drivers/roboclaw/Roboclaw.cpp +++ b/src/drivers/roboclaw/Roboclaw.cpp @@ -154,20 +154,14 @@ int Roboclaw::initializeUART() } } -bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool Roboclaw::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f; float left_motor_output = ((float)outputs[1] - 128.0f) / 127.f; - if (stop_motors) { - setMotorSpeed(Motor::Right, 0.f); - setMotorSpeed(Motor::Left, 0.f); - - } else { - setMotorSpeed(Motor::Right, right_motor_output); - setMotorSpeed(Motor::Left, left_motor_output); - } + setMotorSpeed(Motor::Right, right_motor_output); + setMotorSpeed(Motor::Left, left_motor_output); return true; } diff --git a/src/drivers/roboclaw/Roboclaw.hpp b/src/drivers/roboclaw/Roboclaw.hpp index c58ef1bf5e..1bb6364649 100644 --- a/src/drivers/roboclaw/Roboclaw.hpp +++ b/src/drivers/roboclaw/Roboclaw.hpp @@ -77,7 +77,7 @@ public: void Run() override; /** @see OutputModuleInterface */ - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; void setMotorSpeed(Motor motor, float value); ///< rev/sec diff --git a/src/drivers/tap_esc/TAP_ESC.cpp b/src/drivers/tap_esc/TAP_ESC.cpp index 0d37dda2c0..92ec758d87 100644 --- a/src/drivers/tap_esc/TAP_ESC.cpp +++ b/src/drivers/tap_esc/TAP_ESC.cpp @@ -241,7 +241,7 @@ void TAP_ESC::send_tune_packet(EscbusTunePacket &tune_packet) tap_esc_common::send_packet(_uart_fd, buzzer_packet, -1); } -bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool TAP_ESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { if (_initialized) { diff --git a/src/drivers/tap_esc/TAP_ESC.hpp b/src/drivers/tap_esc/TAP_ESC.hpp index 673237a1ca..cc70d32c6b 100644 --- a/src/drivers/tap_esc/TAP_ESC.hpp +++ b/src/drivers/tap_esc/TAP_ESC.hpp @@ -99,7 +99,7 @@ public: int init(); - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index aaf3cd6670..ec16b6a700 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -85,10 +85,8 @@ UavcanEscController::init() } void -UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs) +UavcanEscController::update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs) { - // TODO: remove stop_motors as it's unnecessary and unused almost everywhere - // TODO: configurable rate limit const auto timestamp = _node.getMonotonicTime(); diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index aceeed1ba9..0518935cad 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -69,7 +69,7 @@ public: int init(); - void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs); + void update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs); /** * Sets the number of rotors and enable timer diff --git a/src/drivers/uavcan/actuators/servo.cpp b/src/drivers/uavcan/actuators/servo.cpp index 56fdea4bba..1af9a7173c 100644 --- a/src/drivers/uavcan/actuators/servo.cpp +++ b/src/drivers/uavcan/actuators/servo.cpp @@ -45,7 +45,7 @@ UavcanServoController::UavcanServoController(uavcan::INode &node) : } void -UavcanServoController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) +UavcanServoController::update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) { uavcan::equipment::actuator::ArrayCommand msg; diff --git a/src/drivers/uavcan/actuators/servo.hpp b/src/drivers/uavcan/actuators/servo.hpp index f0f1e97fb6..78d6f233e1 100644 --- a/src/drivers/uavcan/actuators/servo.hpp +++ b/src/drivers/uavcan/actuators/servo.hpp @@ -52,7 +52,7 @@ public: UavcanServoController(uavcan::INode &node); ~UavcanServoController() = default; - void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs); + void update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs); private: uavcan::INode &_node; diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 4bb86cac44..7fb665a87c 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -1012,10 +1012,10 @@ UavcanNode::Run() } #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) -bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool UavcanMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - _esc_controller.update_outputs(stop_motors, outputs, num_outputs); + _esc_controller.update_outputs(outputs, num_outputs); return true; } @@ -1042,10 +1042,10 @@ void UavcanMixingInterfaceESC::mixerChanged() _esc_controller.set_rotor_count(rotor_count); } -bool UavcanMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool UavcanMixingInterfaceServo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - _servo_controller.update_outputs(stop_motors, outputs, num_outputs); + _servo_controller.update_outputs(outputs, num_outputs); return true; } diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index d249e5d838..15fb23e413 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -127,7 +127,7 @@ public: _node_mutex(node_mutex), _esc_controller(esc_controller) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; void mixerChanged() override; @@ -158,7 +158,7 @@ public: _node_mutex(node_mutex), _servo_controller(servo_controller) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; MixingOutput &mixingOutput() { return _mixing_output; } diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp index 3461c99a4b..66551e468a 100644 --- a/src/drivers/voxl2_io/voxl2_io.cpp +++ b/src/drivers/voxl2_io/voxl2_io.cpp @@ -276,7 +276,7 @@ int Voxl2IO::get_version_info() return (got_response == true ? 0 : -1); } -bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], +bool Voxl2IO::updateOutputs(uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], unsigned num_outputs, unsigned num_control_groups_updated) { // Stop Mixer while ESCs are being calibrated @@ -305,13 +305,7 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN _pwm_on = true; } - //Do we even need this condition? mixer should handle stopping motors anyway by sending the disable command, right? - if (0) { //(!_pwm_on || stop_motors) { - output_cmds[i] = _parameters.pwm_dis * MIXER_OUTPUT_TO_CMD_SCALE; //0; //convert to ns - - } else { - output_cmds[i] = ((uint32_t)outputs[i]) * MIXER_OUTPUT_TO_CMD_SCALE; //convert to ns - } + output_cmds[i] = ((uint32_t)outputs[i]) * MIXER_OUTPUT_TO_CMD_SCALE; //convert to ns } Command cmd; diff --git a/src/drivers/voxl2_io/voxl2_io.hpp b/src/drivers/voxl2_io/voxl2_io.hpp index b3458456a0..31f9cac9da 100644 --- a/src/drivers/voxl2_io/voxl2_io.hpp +++ b/src/drivers/voxl2_io/voxl2_io.hpp @@ -82,7 +82,7 @@ public: int print_status() override; /** @see OutputModuleInterface */ - bool updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], + bool updateOutputs(uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], unsigned num_outputs, unsigned num_control_groups_updated) override; virtual int init(); diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 2de38f7370..8bb000a33a 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -473,16 +473,12 @@ bool MixingOutput::update() void MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates) { - bool stop_motors = !_throttle_armed && !_actuator_test.inTestMode(); - if (_armed.lockdown || _armed.manual_lockdown) { // overwrite outputs in case of lockdown with disarmed values for (size_t i = 0; i < _max_num_outputs; i++) { _current_output_value[i] = _disarmed_value[i]; } - stop_motors = true; - } else if (_armed.force_failsafe) { // overwrite outputs in case of force_failsafe with _failsafe_value values for (size_t i = 0; i < _max_num_outputs; i++) { @@ -513,7 +509,7 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat } /* now return the outputs to the driver */ - if (_interface.updateOutputs(stop_motors, _current_output_value, _max_num_outputs, has_updates)) { + if (_interface.updateOutputs(_current_output_value, _max_num_outputs, has_updates)) { actuator_outputs_s actuator_outputs{}; setAndPublishActuatorOutputs(_max_num_outputs, actuator_outputs); diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 83828ab166..7ed8ed8a73 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -79,14 +79,12 @@ public: /** * Callback to update the (physical) actuator outputs in the driver - * @param stop_motors if true, all motors must be stopped (if false, individual motors - * might still be stopped via outputs[i] == disarmed_value) * @param outputs individual actuator outputs in range [min, max] or failsafe/disarmed value * @param num_outputs number of outputs (<= max_num_outputs) * @param num_control_groups_updated number of actuator_control groups updated * @return if true, the update got handled, and actuator_outputs can be published */ - virtual bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + virtual bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) = 0; /** called whenever the mixer gets updated/reset */ diff --git a/src/lib/mixer_module/mixer_module_tests.cpp b/src/lib/mixer_module/mixer_module_tests.cpp index 3901a1c85d..d774c3d44b 100644 --- a/src/lib/mixer_module/mixer_module_tests.cpp +++ b/src/lib/mixer_module/mixer_module_tests.cpp @@ -86,7 +86,7 @@ public: was_scheduled = true; } - bool updateOutputs(bool stop_motors, uint16_t outputs_[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs_[MAX_ACTUATORS], unsigned num_outputs_, unsigned num_control_groups_updated) override { memcpy(outputs, outputs_, sizeof(outputs)); diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp index 82ff4cf3da..f556817ea9 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp @@ -62,7 +62,7 @@ bool GZMixingInterfaceESC::init(const std::string &model_name) return true; } -bool GZMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool GZMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { unsigned active_output_count = 0; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp index d724c5c6f9..5ffe105b5d 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp @@ -55,7 +55,7 @@ public: _node(node) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; MixingOutput &mixingOutput() { return _mixing_output; } diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 6fd069ec09..aaca2a3035 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -128,7 +128,7 @@ bool GZMixingInterfaceServo::init(const std::string &model_name) return true; } -bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool GZMixingInterfaceServo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { bool updated = false; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 9980918233..0e6759408b 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -49,7 +49,7 @@ public: _node(node) {} - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; MixingOutput &mixingOutput() { return _mixing_output; } diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index c11a5eb365..2453be4f1b 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -58,7 +58,7 @@ PWMSim::~PWMSim() perf_free(_interval_perf); } -bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, +bool PWMSim::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { // Only publish once we receive actuator_controls (important for lock-step to work correctly) diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index b16a9fac5e..2c6fee7347 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp @@ -70,7 +70,7 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: