From bf783779b63340d9c077327223b174a003d9cd25 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 8 Jan 2026 17:12:31 -0900 Subject: [PATCH] fix esc online mask ordering, fix bdshot channels ready check, improve dshot status formatting --- .../src/px4/stm/stm32_common/dshot/dshot.c | 2 ++ src/drivers/dshot/DShot.cpp | 27 ++++++++++++++----- src/drivers/dshot/DShot.h | 3 +++ 3 files changed, 26 insertions(+), 6 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 24e86e8ef6..3b03b9aad1 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -1021,7 +1021,9 @@ void up_bdshot_status(void) (double)_edt_volt[i].rate_hz, (double)_edt_curr[i].rate_hz); } + } + for (int i = 0; i < MAX_TIMER_IO_CHANNELS; i++) { PX4_INFO("Output %u: read %lu, failed CRC %lu", i, read_ok[i], read_fail_crc[i]); } } diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index f3a932065a..12c53e1b85 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -558,8 +558,8 @@ bool DShot::process_bdshot_telemetry() return false; } - // We wait until all are ready. - if (up_bdshot_num_channels_ready() < count_set_bits(_output_mask)) { + // We wait until all BDShot channels are ready. + if (up_bdshot_num_channels_ready() < count_set_bits(_bdshot_output_mask)) { return false; } @@ -656,17 +656,26 @@ 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 = 0xFF; + uint8_t online_mask_motor_order = 0xFF; if (is_bdshot) { - online_mask &= _bdshot_telem_online_mask; + online_mask_motor_order &= _bdshot_telem_online_mask; } if (_serial_telemetry_enabled) { - online_mask &= _serial_telem_online_mask; + online_mask_motor_order &= _serial_telem_online_mask; } - _esc_status.esc_online_flags = online_mask; + // Convert motor-order mask to actuator-order mask for esc_online_flags + uint8_t online_mask_actuator_order = 0; + + 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] + @@ -880,6 +889,11 @@ 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); @@ -889,6 +903,7 @@ void DShot::mixerChanged() if (motor_index >= 0 && motor_index < DSHOT_MAXIMUM_CHANNELS) { new_motor_mask |= (1 << motor_index); + _motor_to_channel[motor_index] = actuator_channel; } } } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index f8e986a936..c3e6092f02 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -132,6 +132,9 @@ 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)};