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 c8dfe228c42..0870f689f28 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -168,6 +168,7 @@ static volatile erpm_data_t _erpms[MAX_TIMER_IO_CHANNELS] = {}; static volatile edt_data_t _edt_temp[MAX_TIMER_IO_CHANNELS] = {}; static volatile edt_data_t _edt_volt[MAX_TIMER_IO_CHANNELS] = {}; static volatile edt_data_t _edt_curr[MAX_TIMER_IO_CHANNELS] = {}; +static volatile edt_data_t _edt_state[MAX_TIMER_IO_CHANNELS] = {}; static float calculate_rate_hz(uint64_t last_timestamp, float last_rate_hz, uint64_t timestamp); @@ -750,7 +751,8 @@ void process_capture_results(uint8_t timer_index, uint8_t channel_index) } case DSHOT_EDT_STATE_EVENT: - // TODO: Handle these? + _edt_state[output_channel].value = packet.value; + _edt_state[output_channel].ready = true; break; default: @@ -1046,6 +1048,15 @@ int up_bdshot_get_extended_telemetry(uint8_t channel, int type, uint8_t *value) break; + case DSHOT_EDT_STATE_EVENT: + if (_edt_state[channel].ready) { + *value = _edt_state[channel].value; + _edt_state[channel].ready = false; + result = PX4_OK; + } + + break; + default: break; } diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index ba7f4c1bf12..34f3d8ee7f2 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -137,6 +137,15 @@ bool DShot::updateOutputs(float *outputs, unsigned num_outputs, unsigned num_con _esc_status.esc_armed_flags = esc_armed_mask(hw_outputs, num_outputs); bool armed = _esc_status.esc_armed_flags != 0; + // Bluejay drops EDT when the motor stops, so it has to be re-enabled after every flight. + if (_armed_prev && !armed) { + for (int i = 0; i < DSHOT_MAX_MOTORS; i++) { + clear_edt_confirmation(i); + } + } + + _armed_prev = armed; + if (!armed) { // Select next command to send (if any) if (_telemetry.telemetryResponseFinished() && @@ -193,7 +202,7 @@ void DShot::select_next_command() // - Settings Programming // EDT Request mask - uint16_t needs_edt_request_mask = _bdshot_telem_online_mask & ~_bdshot_edt_requested_mask; + uint16_t needs_edt_request_mask = _bdshot_telem_online_mask & ~_bdshot_edt_confirmed_mask; // Settings Request mask uint16_t needs_settings_request_mask = _serial_telem_online_mask & ~_settings_requested_mask; @@ -210,20 +219,36 @@ void DShot::select_next_command() hrt_abstime now = hrt_absolute_time(); for (int motor_index = 0; motor_index < DSHOT_MAX_MOTORS; motor_index++) { - if (edt_motors_to_request & (1 << motor_index)) { - // Wait 1 second after ESC comes online before sending EDT (ESC init sequence) - if (_bdshot_telem_online_timestamps[motor_index] == 0 - || (now - _bdshot_telem_online_timestamps[motor_index]) < 1_s) { - continue; + if (!(edt_motors_to_request & (1 << motor_index))) { + continue; + } + + // The ESC only takes commands once it has seen zero throttle for a while (AM32: one second plus + // its arming tune), so hold off after it comes online and space the retries. + if (_bdshot_telem_online_timestamps[motor_index] == 0 + || (now - _bdshot_telem_online_timestamps[motor_index]) < 1_s + || (now - _bdshot_edt_last_request[motor_index]) < BDSHOT_EDT_RETRY_INTERVAL) { + continue; + } + + if (_bdshot_edt_attempts[motor_index] >= BDSHOT_EDT_MAX_ATTEMPTS) { + // Warn once, a retry interval after the last attempt went unanswered + if (_bdshot_edt_attempts[motor_index] == BDSHOT_EDT_MAX_ATTEMPTS) { + PX4_WARN("ESC%d: no EDT frames after %d enable requests", motor_index + 1, BDSHOT_EDT_MAX_ATTEMPTS); + _bdshot_edt_attempts[motor_index]++; } - _current_command.num_repetitions = 10; - _current_command.command = DSHOT_EXTENDED_TELEMETRY_ENABLE; - _current_command.motor_mask = (1 << motor_index); - _bdshot_edt_requested_mask |= (1 << motor_index); - PX4_DEBUG("ESC%d: requesting EDT at time %.2fs", motor_index + 1, (double)now / 1000000.); - break; + continue; } + + _current_command.num_repetitions = 10; + _current_command.command = DSHOT_EXTENDED_TELEMETRY_ENABLE; + _current_command.motor_mask = (1 << motor_index); + _bdshot_edt_attempts[motor_index]++; + _bdshot_edt_last_request[motor_index] = now; + PX4_DEBUG("ESC%d: requesting EDT (attempt %d) at time %.2fs", motor_index + 1, + _bdshot_edt_attempts[motor_index], (double)now / 1000000.); + break; } } else if (_esc_type != 0 && _serial_telemetry_enabled && serial_telem_delay_elapsed && (_motor_mask & needs_settings_request_mask)) { @@ -417,6 +442,13 @@ void DShot::update_motor_commands(int num_outputs) } } +void DShot::clear_edt_confirmation(int motor_index) +{ + _bdshot_edt_confirmed_mask &= ~(1 << motor_index); + _bdshot_edt_attempts[motor_index] = 0; + _bdshot_edt_last_request[motor_index] = hrt_absolute_time(); +} + uint16_t DShot::esc_armed_mask(uint16_t *outputs, uint8_t num_outputs) { uint16_t mask = 0; @@ -663,8 +695,12 @@ bool DShot::process_bdshot_telemetry() uint8_t value = 0; + // The enable ack is a single state/event frame and can be lost, so any EDT frame counts + bool edt_frame = up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_STATE_EVENT, &value) == PX4_OK; + if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_TEMPERATURE, &value) == PX4_OK) { esc.temperature = value; // BDShot temperature is in C + edt_frame = true; } else { esc.temperature = _esc_status.esc[motor_index].esc_temperature; // use previous @@ -672,6 +708,7 @@ bool DShot::process_bdshot_telemetry() if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_VOLTAGE, &value) == PX4_OK) { esc.voltage = static_cast(value) / 4.f; // BDShot voltage is in 0.25V + edt_frame = true; } else { esc.voltage = _esc_status.esc[motor_index].esc_voltage; // use previous @@ -679,16 +716,23 @@ bool DShot::process_bdshot_telemetry() if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_CURRENT, &value) == PX4_OK) { esc.current = static_cast(value); // EDT current is in 1A steps + edt_frame = true; } else { esc.current = _esc_status.esc[motor_index].esc_current; // use previous } + + // Only frames after our own request count, or one still in flight at the disarm edge would + // hide Bluejay dropping EDT with the motor stop. + if (edt_frame && _bdshot_edt_attempts[motor_index] > 0) { + _bdshot_edt_confirmed_mask |= (1 << motor_index); + } } } else { _bdshot_telem_online_mask &= ~(1 << motor_index); _bdshot_telem_online_timestamps[motor_index] = 0; - _bdshot_edt_requested_mask &= ~(1 << motor_index); + clear_edt_confirmation(motor_index); perf_count(_bdshot_error_perf); } @@ -1189,7 +1233,7 @@ int DShot::print_status() } if (_bdshot_output_mask && _bdshot_edt_enabled) { - PX4_INFO(" EDT Requested Mask (motor order): 0x%02x", _bdshot_edt_requested_mask); + PX4_INFO(" EDT Confirmed Mask (motor order): 0x%02x", _bdshot_edt_confirmed_mask); } if (_serial_telemetry_enabled) { diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 3efd6d164fa..f5d37f0ff6b 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -122,6 +122,7 @@ private: void update_motor_outputs(uint16_t *outputs, int num_outputs); void update_motor_commands(int num_outputs); void select_next_command(); + void clear_edt_confirmation(int motor_index); bool set_next_telemetry_index(); // Returns true when the telemetry index has wrapped, indicating all configured motors have been sampled. bool process_serial_telemetry(); @@ -171,7 +172,16 @@ private: uint32_t _serial_telem_online_mask = 0; // Mask indicating telem receive status for serial telem uint32_t _serial_telem_errors[DSHOT_MAX_MOTORS] = {}; uint32_t _bdshot_telem_errors[DSHOT_MAX_MOTORS] = {}; - uint16_t _bdshot_edt_requested_mask = 0; + + // EDT enable is acked with a state/event frame, but any EDT frame proves it took. Retry until one arrives, + // giving up until reconnect or disarm so ESCs without EDT do not get a command burst every second. + static constexpr int BDSHOT_EDT_MAX_ATTEMPTS = 5; + static constexpr hrt_abstime BDSHOT_EDT_RETRY_INTERVAL = 1_s; + uint16_t _bdshot_edt_confirmed_mask = 0; + uint8_t _bdshot_edt_attempts[DSHOT_MAX_MOTORS] = {}; + hrt_abstime _bdshot_edt_last_request[DSHOT_MAX_MOTORS] = {}; + bool _armed_prev = false; + uint16_t _settings_requested_mask = 0; // Array of timestamps indicating when the telemetry came online