From 53a4a871efeec1f4bda5acdf65dc174a7fe39b1f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Feb 2026 05:32:28 +1300 Subject: [PATCH] dshot: enable up to 16 DShot channels For some boards, we can have DShot on 12 to 14 timer channels, so we need to support up to 16 to have them covered. --- src/drivers/dshot/DShot.cpp | 77 ++++++++++++++------------------- src/drivers/dshot/DShot.h | 12 ++--- src/drivers/dshot/DShotCommon.h | 8 ++-- 3 files changed, 44 insertions(+), 53 deletions(-) diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 49f3f0385a..2274034c64 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -403,14 +403,18 @@ void DShot::update_motor_commands(int num_outputs) } } -uint8_t DShot::esc_armed_mask(uint16_t *outputs, uint8_t num_outputs) +uint16_t DShot::esc_armed_mask(uint16_t *outputs, uint8_t num_outputs) { - uint8_t mask = 0; + uint16_t mask = 0; for (uint8_t i = 0; i < num_outputs; i++) { if (_mixing_output.isMotor(i)) { if (outputs[i] != DSHOT_DISARM_VALUE) { - mask |= (1 << i); + int motor_index = (int)_mixing_output.outputFunction(i) - (int)OutputFunction::Motor1; + + if (motor_index >= 0 && motor_index < esc_status_s::CONNECTED_ESC_MAX) { + mask |= (1 << motor_index); + } } } } @@ -583,7 +587,7 @@ bool DShot::process_bdshot_telemetry() esc.motor_index = motor_index; esc.timestamp = now; - // Error counts are stored in actuator order to match _esc_status.esc[] + // Error counts are stored in actuator order _bdshot_telem_errors[output_channel] = up_bdshot_num_errors(output_channel); if (up_bdshot_channel_online(output_channel)) { @@ -597,8 +601,8 @@ bool DShot::process_bdshot_telemetry() esc.erpm = erpm * 100; } else { - // Use previous value (esc_status is in actuator order) - esc.erpm = _esc_status.esc[output_channel].esc_rpm * (_param_mot_pole_count.get() / 2); + // Use previous value (esc_status is indexed by motor_index) + esc.erpm = _esc_status.esc[motor_index].esc_rpm * (_param_mot_pole_count.get() / 2); } // Extended DShot Telemetry @@ -610,21 +614,21 @@ bool DShot::process_bdshot_telemetry() esc.temperature = value; // BDShot temperature is in C } else { - esc.temperature = _esc_status.esc[output_channel].esc_temperature; // use previous + esc.temperature = _esc_status.esc[motor_index].esc_temperature; // use previous } if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_VOLTAGE, &value) == PX4_OK) { esc.voltage = value / 4.f; // BDShot voltage is in 0.25V } else { - esc.voltage = _esc_status.esc[output_channel].esc_voltage; // use previous + esc.voltage = _esc_status.esc[motor_index].esc_voltage; // use previous } if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_CURRENT, &value) == PX4_OK) { esc.current = value / 2.f; // BDShot current is in 0.5A } else { - esc.current = _esc_status.esc[output_channel].esc_current; // use previous + esc.current = _esc_status.esc[motor_index].esc_current; // use previous } } @@ -648,7 +652,7 @@ void DShot::consume_esc_data(const EscData &esc, TelemetrySource source) int actuator_channel = esc.actuator_channel; int motor_index = esc.motor_index; - if (!math::isInRange(actuator_channel, 0, esc_status_s::CONNECTED_ESC_MAX - 1) || + if (!math::isInRange(actuator_channel, 0, DSHOT_MAXIMUM_CHANNELS - 1) || !math::isInRange(motor_index, 0, esc_status_s::CONNECTED_ESC_MAX - 1)) { return; } @@ -657,51 +661,42 @@ void DShot::consume_esc_data(const EscData &esc, TelemetrySource source) bool is_bdshot = _bdshot_motor_mask & (1 << motor_index); // Require both sources online when enabled (masks are in motor order) - uint8_t online_mask_motor_order = 0xFF; + uint16_t online_mask = (1u << esc_status_s::CONNECTED_ESC_MAX) - 1; if (is_bdshot) { - online_mask_motor_order &= _bdshot_telem_online_mask; + online_mask &= _bdshot_telem_online_mask; } if (_serial_telemetry_enabled) { - online_mask_motor_order &= _serial_telem_online_mask; + online_mask &= _serial_telem_online_mask; } - // Convert motor-order mask to actuator-order mask for esc_online_flags - uint8_t online_mask_actuator_order = 0; + _esc_status.esc_online_flags = online_mask; - for (int motor_idx = 0; motor_idx < DSHOT_MAXIMUM_CHANNELS; motor_idx++) { - if ((online_mask_motor_order & (1 << motor_idx)) && _motor_to_channel[motor_idx] >= 0) { - online_mask_actuator_order |= (1 << _motor_to_channel[motor_idx]); - } - } - - _esc_status.esc_online_flags = online_mask_actuator_order; - - // Sum the errors from both interfaces (error arrays are in actuator order) - _esc_status.esc[actuator_channel].esc_errorcount = _serial_telem_errors[actuator_channel] + + // esc_status is indexed by motor_index (Motor1=0, Motor2=1, ...) + _esc_status.esc[motor_index].esc_errorcount = _serial_telem_errors[actuator_channel] + _bdshot_telem_errors[actuator_channel]; if (source == TelemetrySource::Serial) { // Only use SerialTelemetry eRPM when BDShot is disabled if (!is_bdshot) { - _esc_status.esc[actuator_channel].timestamp = esc.timestamp; - _esc_status.esc[actuator_channel].esc_rpm = esc.erpm / (_param_mot_pole_count.get() / 2); + _esc_status.esc[motor_index].timestamp = esc.timestamp; + _esc_status.esc[motor_index].esc_rpm = esc.erpm / (_param_mot_pole_count.get() / 2); } - _esc_status.esc[actuator_channel].esc_voltage = esc.voltage; - _esc_status.esc[actuator_channel].esc_current = esc.current; - _esc_status.esc[actuator_channel].esc_temperature = esc.temperature; + _esc_status.esc[motor_index].esc_voltage = esc.voltage; + _esc_status.esc[motor_index].esc_current = esc.current; + _esc_status.esc[motor_index].esc_temperature = esc.temperature; } else if (source == TelemetrySource::BDShot) { - _esc_status.esc[actuator_channel].timestamp = esc.timestamp; - _esc_status.esc[actuator_channel].esc_rpm = esc.erpm / (_param_mot_pole_count.get() / 2); + _esc_status.esc[motor_index].timestamp = esc.timestamp; + _esc_status.esc[motor_index].esc_rpm = esc.erpm / (_param_mot_pole_count.get() / 2); // Only use BDShot Volt/Curr/Temp when Serial Telemetry is disabled if (!_serial_telemetry_enabled) { - _esc_status.esc[actuator_channel].esc_voltage = esc.voltage; - _esc_status.esc[actuator_channel].esc_current = esc.current; - _esc_status.esc[actuator_channel].esc_temperature = esc.temperature; + _esc_status.esc[motor_index].esc_voltage = esc.voltage; + _esc_status.esc[motor_index].esc_current = esc.current; + _esc_status.esc[motor_index].esc_temperature = esc.temperature; } } } @@ -893,21 +888,15 @@ void DShot::mixerChanged() uint32_t new_output_mask = 0; uint32_t new_motor_mask = 0; - // Reset motor-to-channel mapping - for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) { - _motor_to_channel[i] = -1; - } - for (int actuator_channel = 0; actuator_channel < DSHOT_MAXIMUM_CHANNELS; actuator_channel++) { if (_mixing_output.isMotor(actuator_channel)) { - _esc_status.esc[actuator_channel].actuator_function = (uint8_t)_mixing_output.outputFunction(actuator_channel); new_output_mask |= (1 << actuator_channel); int motor_index = (int)_mixing_output.outputFunction(actuator_channel) - (int)OutputFunction::Motor1; - if (motor_index >= 0 && motor_index < DSHOT_MAXIMUM_CHANNELS) { + if (motor_index >= 0 && motor_index < esc_status_s::CONNECTED_ESC_MAX) { + _esc_status.esc[motor_index].actuator_function = (uint8_t)_mixing_output.outputFunction(actuator_channel); new_motor_mask |= (1 << motor_index); - _motor_to_channel[motor_index] = actuator_channel; } } } @@ -931,7 +920,7 @@ void DShot::mixerChanged() if ((new_bdshot_output_mask & (1 << actuator_channel)) && _mixing_output.isMotor(actuator_channel)) { int motor_index = (int)_mixing_output.outputFunction(actuator_channel) - (int)OutputFunction::Motor1; - if (motor_index >= 0 && motor_index < DSHOT_MAXIMUM_CHANNELS) { + if (motor_index >= 0 && motor_index < esc_status_s::CONNECTED_ESC_MAX) { new_bdshot_motor_mask |= (1 << motor_index); } } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 540d839d64..a8e7ef7633 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -49,6 +50,10 @@ using namespace time_literals; # error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS" #endif +static_assert(DSHOT_MAXIMUM_CHANNELS <= 16, "DShot driver uses uint16_t bitmasks"); +static_assert(esc_status_s::CONNECTED_ESC_MAX <= DSHOT_MAXIMUM_CHANNELS, + "esc_status array must not exceed DShot channel count"); + static constexpr hrt_abstime ESC_INIT_TELEM_DELAY = 5_s; /// Dshot PWM frequency (Hz) @@ -98,7 +103,7 @@ private: bool initialize_dshot(); void init_telemetry(const char *device, bool swap_rxtx); - uint8_t esc_armed_mask(uint16_t *outputs, uint8_t num_outputs); + uint16_t esc_armed_mask(uint16_t *outputs, uint8_t num_outputs); void update_motor_outputs(uint16_t *outputs, int num_outputs); void update_motor_commands(int num_outputs); @@ -132,9 +137,6 @@ private: uint32_t _motor_mask{0}; uint32_t _bdshot_motor_mask{0}; - // Motor-to-channel mapping: _motor_to_channel[motor_index] = actuator_channel (-1 if not assigned) - int8_t _motor_to_channel[DSHOT_MAXIMUM_CHANNELS] = {-1, -1, -1, -1, -1, -1, -1, -1}; - // uORB uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; @@ -188,7 +190,7 @@ private: struct DShotCommand { uint16_t command{}; int num_repetitions{0}; - uint8_t motor_mask{0xff}; + uint16_t motor_mask{(1u << DSHOT_MAXIMUM_CHANNELS) - 1}; bool save{false}; bool expect_response{false}; diff --git a/src/drivers/dshot/DShotCommon.h b/src/drivers/dshot/DShotCommon.h index 973a64f34e..b95a4cb91f 100644 --- a/src/drivers/dshot/DShotCommon.h +++ b/src/drivers/dshot/DShotCommon.h @@ -34,9 +34,9 @@ #pragma once #include -#include +#include -static constexpr int DSHOT_MAXIMUM_CHANNELS = esc_status_s::CONNECTED_ESC_MAX; +static constexpr int DSHOT_MAXIMUM_CHANNELS = DIRECT_PWM_OUTPUT_CHANNELS; enum class TelemetrySource { Serial = 0, @@ -44,8 +44,8 @@ enum class TelemetrySource { }; struct EscData { - int actuator_channel; // Actuator output channel 0-7 - int motor_index; // Motors 0-7 + int actuator_channel; // Actuator output channel 0..(DSHOT_MAXIMUM_CHANNELS-1) + int motor_index; // Motor index 0..(CONNECTED_ESC_MAX-1) hrt_abstime timestamp; // Sample time TelemetrySource source;