From 0e30739762f3086f493749c033a0a7f55cb245bf Mon Sep 17 00:00:00 2001 From: HalfManBear <89969229+halfmanbear@users.noreply.github.com> Date: Tue, 26 May 2026 23:05:54 +0100 Subject: [PATCH 1/9] Add bidirectional DShot telemetry and RPM gyro filtering Ports bidirectional DShot (GCR) telemetry from Betaflight, enabling RPM-based gyro filtering without a dedicated ESC telemetry UART. After each DShot output frame the motor pin is switched to timer input capture mode via per-channel DMA to receive the ESC's GCR-encoded eRPM response. The decoded eRPM feeds the existing RPM filter infrastructure. New settings: - dshot_bidir_enabled: enable bidirectional DShot telemetry - dshot_edt_enabled: enable Extended DShot Telemetry (temperature, voltage, current in addition to eRPM) Changes: - drivers/dshot.c/.h: GCR decode, EDT frame parsing, eRPM-to-RPM conversion, motor frequency LPF (ported from Betaflight, GPLv3) - drivers/pwm_output.c: per-channel DMA direction switching between DShot output and GCR input capture on STM32 (StdPeriph/HAL) and AT32 - sensors/esc_sensor.c/.h: DShot bidir data path into ESC sensor framework (escSensorSetDshotData, escSensorIsActive) - flight/rpm_filter.c: DShot telemetry as frequency source alongside serial ESC sensor path - target/common_post.h: USE_RPM_FILTER enabled for any USE_DSHOT target - fc/fc_init.c: call initDshotTelemetry() at startup; fix RPM filter init condition to accept DShot bidir as a valid source - blackbox/blackbox.c: fix NULL dereference when escSensorGetData() returns NULL; fix uninitialized blackboxSlowState_t comparison - telemetry/srxl.c: replace Betaflight-specific USE_DSHOT_TELEMETRY guards with INAV's USE_DSHOT/USE_ESC_SENSOR; use escSensorGetData() Tested on SPEEDYBEEF405V4 (STM32F405, StdPeriph) with Bluejay ESC firmware. eRPM telemetry confirmed in blackbox; RPM gyro filter updating notch frequencies with motor speed. Co-Authored-By: Claude Sonnet 4.6 --- src/main/CMakeLists.txt | 2 + src/main/blackbox/blackbox.c | 8 +- src/main/drivers/dshot.c | 237 ++++++++++++++++++++ src/main/drivers/dshot.h | 87 ++++++++ src/main/drivers/pwm_output.c | 393 ++++++++++++++++++++++++++++++++-- src/main/fc/fc_init.c | 4 +- src/main/fc/settings.yaml | 10 + src/main/flight/mixer.c | 6 +- src/main/flight/mixer.h | 2 + src/main/flight/rpm_filter.c | 23 +- src/main/sensors/esc_sensor.c | 59 ++++- src/main/sensors/esc_sensor.h | 3 + src/main/target/common_post.h | 2 +- src/main/telemetry/srxl.c | 22 +- 14 files changed, 804 insertions(+), 54 deletions(-) create mode 100644 src/main/drivers/dshot.c create mode 100644 src/main/drivers/dshot.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index c243d9215f3..dc38fb5c6be 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -233,6 +233,8 @@ main_sources(COMMON_SRC drivers/pwm_esc_detect.h drivers/pwm_mapping.c drivers/pwm_mapping.h + drivers/dshot.c + drivers/dshot.h drivers/pwm_output.c drivers/pwm_output.h drivers/pinio.c diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f5225ca3e1e..42da98188a8 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1429,8 +1429,10 @@ static void loadSlowState(blackboxSlowState_t *slow) #ifdef USE_ESC_SENSOR escSensorData_t * escSensor = escSensorGetData(); - slow->escRPM = escSensor->rpm; - slow->escTemperature = escSensor->temperature; + if (escSensor) { + slow->escRPM = escSensor->rpm; + slow->escTemperature = escSensor->temperature; + } #endif } @@ -1448,7 +1450,7 @@ static bool writeSlowFrameIfNeeded(bool allowPeriodicWrite) if (shouldWrite) { loadSlowState(&slowHistory); } else { - blackboxSlowState_t newSlowState; + blackboxSlowState_t newSlowState = {0}; loadSlowState(&newSlowState); diff --git a/src/main/drivers/dshot.c b/src/main/drivers/dshot.c new file mode 100644 index 00000000000..c23b560c8a6 --- /dev/null +++ b/src/main/drivers/dshot.c @@ -0,0 +1,237 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" +#include "common/utils.h" +#include "drivers/dshot.h" + +#ifdef USE_DSHOT + +#include "common/filter.h" +#include "common/maths.h" + +#include "flight/mixer.h" +#include "drivers/pwm_mapping.h" + +#define DSHOT_RPM_LPF_HZ 150 + +bool useDshotTelemetry = false; +dshotTelemetryState_t dshotTelemetryState; + +static pt1Filter_t motorFreqLpf[MAX_SUPPORTED_MOTORS]; +static float motorFrequencyHz[MAX_SUPPORTED_MOTORS]; +static float dshotRpm[MAX_SUPPORTED_MOTORS]; +static float dshotRpmAverage; +static float erpmToHz; +static bool edtAlwaysDecode; + +static const dshotTelemetryType_e extendedTelemetryLookup[8] = { + DSHOT_TELEMETRY_TYPE_ERPM, + DSHOT_TELEMETRY_TYPE_TEMPERATURE, + DSHOT_TELEMETRY_TYPE_VOLTAGE, + DSHOT_TELEMETRY_TYPE_CURRENT, + DSHOT_TELEMETRY_TYPE_DEBUG1, + DSHOT_TELEMETRY_TYPE_DEBUG2, + DSHOT_TELEMETRY_TYPE_DEBUG3, + DSHOT_TELEMETRY_TYPE_STATE_EVENTS, +}; + +static float erpmToRpm(uint32_t erpm) +{ + return erpm * erpmToHz * 60.0f; +} + +static uint32_t dshotDecodeErpmTelemetryValue(uint16_t value) +{ + if (value == 0x0fff) { + return 0; + } + + value = (value & 0x01ff) << ((value & 0xfe00) >> 9); + if (!value) { + return DSHOT_TELEMETRY_INVALID; + } + + return (1000000 * 60 / 100 + value / 2) / value; +} + +static void dshotDecodeTelemetryValue(uint8_t motorIndex, uint32_t *decoded, dshotTelemetryType_e *type) +{ + const uint16_t value = dshotTelemetryState.motorState[motorIndex].rawValue; + const bool edtEnabled = edtAlwaysDecode || (dshotTelemetryState.motorState[motorIndex].telemetryTypes & DSHOT_EXTENDED_TELEMETRY_MASK) != 0; + const unsigned telemetryType = (value & 0x0f00) >> 8; + const bool isErpm = !edtEnabled || (telemetryType & 0x01) || (telemetryType == 0); + + if (isErpm) { + *decoded = dshotDecodeErpmTelemetryValue(value); + *type = DSHOT_TELEMETRY_TYPE_ERPM; + } else { + const unsigned typeIndex = telemetryType >> 1; + *type = typeIndex < ARRAYLEN(extendedTelemetryLookup) ? extendedTelemetryLookup[typeIndex] : DSHOT_TELEMETRY_TYPE_STATE_EVENTS; + *decoded = value & 0x00ff; + } +} + +void dshotResetTelemetry(void) +{ + memset(&dshotTelemetryState, 0, sizeof(dshotTelemetryState)); + memset(dshotRpm, 0, sizeof(dshotRpm)); + memset(motorFrequencyHz, 0, sizeof(motorFrequencyHz)); + dshotRpmAverage = 0.0f; +} + +bool isDshotTelemetryConfigured(void) +{ + return useDshotTelemetry; +} + +bool isDshotTelemetryActive(void) +{ + return useDshotTelemetry; +} + +void initDshotTelemetry(timeUs_t looptimeUs) +{ + useDshotTelemetry = motorConfig()->useDshotTelemetry && (motorConfig()->motorPwmProtocol >= PWM_TYPE_DSHOT150); + edtAlwaysDecode = motorConfig()->useDshotEdt != 0; + + dshotResetTelemetry(); + + if (!useDshotTelemetry) { + return; + } + + erpmToHz = ERPM_PER_LSB / 60.0f / (motorConfig()->motorPoleCount / 2.0f); + + for (unsigned i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + pt1FilterInit(&motorFreqLpf[i], DSHOT_RPM_LPF_HZ, looptimeUs * 1e-6f); + } +} + +uint16_t dshotProcessPacket(uint16_t rawValue, uint8_t motorIndex) +{ + if (!useDshotTelemetry || motorIndex >= MAX_SUPPORTED_MOTORS) { + return rawValue; + } + + if (rawValue == DSHOT_TELEMETRY_INVALID || rawValue == DSHOT_TELEMETRY_NOEDGE) { + if (rawValue == DSHOT_TELEMETRY_INVALID) { + dshotTelemetryState.invalidPacketCount++; + } + return rawValue; + } + + dshotTelemetryState.readCount++; + dshotTelemetryState.motorState[motorIndex].rawValue = rawValue; + + dshotTelemetryType_e type; + uint32_t decoded; + dshotDecodeTelemetryValue(motorIndex, &decoded, &type); + if (decoded == DSHOT_TELEMETRY_INVALID) { + dshotTelemetryState.invalidPacketCount++; + return DSHOT_TELEMETRY_INVALID; + } + + dshotTelemetryState.motorState[motorIndex].telemetryData[type] = decoded; + dshotTelemetryState.motorState[motorIndex].telemetryTypes |= (1 << type); + + if (type == DSHOT_TELEMETRY_TYPE_TEMPERATURE && decoded > dshotTelemetryState.motorState[motorIndex].maxTemp) { + dshotTelemetryState.motorState[motorIndex].maxTemp = decoded; + } + + if (type == DSHOT_TELEMETRY_TYPE_ERPM) { + dshotRpm[motorIndex] = erpmToRpm(decoded); + motorFrequencyHz[motorIndex] = pt1FilterApply(&motorFreqLpf[motorIndex], erpmToHz * decoded); + } + + float rpmTotal = 0.0f; + int rpmCount = 0; + for (unsigned i = 0; i < getMotorCount(); i++) { + if (dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_ERPM)) { + rpmTotal += dshotRpm[i]; + rpmCount++; + } + } + dshotRpmAverage = rpmCount ? rpmTotal / rpmCount : 0.0f; + dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_PROCESSED; + + return rawValue; +} + +uint16_t getDshotErpm(uint8_t motorIndex) +{ + return dshotTelemetryState.motorState[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_ERPM]; +} + +float getDshotRpm(uint8_t motorIndex) +{ + return dshotRpm[motorIndex]; +} + +float getDshotRpmAverage(void) +{ + return dshotRpmAverage; +} + +float getMotorFrequencyHz(uint8_t motorIndex) +{ + return motorFrequencyHz[motorIndex]; +} + +bool getDshotEscSensorData(escSensorData_t *data, uint8_t motorIndex) +{ + if (!useDshotTelemetry || motorIndex >= MAX_SUPPORTED_MOTORS) { + return false; + } + + const dshotTelemetryMotorState_t *state = &dshotTelemetryState.motorState[motorIndex]; + if ((state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_ERPM)) == 0) { + return false; + } + + data->rpm = state->telemetryData[DSHOT_TELEMETRY_TYPE_ERPM]; + data->temperature = (state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) ? state->telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE] : 0; + data->voltage = (state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_VOLTAGE)) ? state->telemetryData[DSHOT_TELEMETRY_TYPE_VOLTAGE] * 250 : 0; + data->current = (state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) ? state->telemetryData[DSHOT_TELEMETRY_TYPE_CURRENT] * 1000 : 0; + + return true; +} + +#endif + +#ifndef USE_DSHOT +bool useDshotTelemetry = false; +dshotTelemetryState_t dshotTelemetryState; + +void initDshotTelemetry(timeUs_t looptimeUs) { UNUSED(looptimeUs); } +void dshotResetTelemetry(void) {} +bool isDshotTelemetryConfigured(void) { return false; } +bool isDshotTelemetryActive(void) { return false; } +uint16_t dshotProcessPacket(uint16_t rawValue, uint8_t motorIndex) { UNUSED(motorIndex); return rawValue; } +float getDshotRpm(uint8_t motorIndex) { UNUSED(motorIndex); return 0.0f; } +uint16_t getDshotErpm(uint8_t motorIndex) { UNUSED(motorIndex); return 0; } +float getDshotRpmAverage(void) { return 0.0f; } +float getMotorFrequencyHz(uint8_t motorIndex) { UNUSED(motorIndex); return 0.0f; } +bool getDshotEscSensorData(escSensorData_t *data, uint8_t motorIndex) { UNUSED(data); UNUSED(motorIndex); return false; } +#endif diff --git a/src/main/drivers/dshot.h b/src/main/drivers/dshot.h new file mode 100644 index 00000000000..31061c76300 --- /dev/null +++ b/src/main/drivers/dshot.h @@ -0,0 +1,87 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include +#include + +#include "platform.h" + +#include "drivers/time.h" +#include "flight/mixer.h" + +#include "sensors/esc_sensor.h" + +#define DSHOT_TELEMETRY_NOEDGE (0xfffe) +#define DSHOT_TELEMETRY_INVALID (0xffff) + +#define MIN_GCR_EDGES (7) +#define MAX_GCR_EDGES (22) + +typedef enum { + DSHOT_TELEMETRY_TYPE_ERPM = 0, + DSHOT_TELEMETRY_TYPE_TEMPERATURE, + DSHOT_TELEMETRY_TYPE_VOLTAGE, + DSHOT_TELEMETRY_TYPE_CURRENT, + DSHOT_TELEMETRY_TYPE_DEBUG1, + DSHOT_TELEMETRY_TYPE_DEBUG2, + DSHOT_TELEMETRY_TYPE_DEBUG3, + DSHOT_TELEMETRY_TYPE_STATE_EVENTS, + DSHOT_TELEMETRY_TYPE_COUNT +} dshotTelemetryType_e; + +#define DSHOT_NORMAL_TELEMETRY_MASK (1 << DSHOT_TELEMETRY_TYPE_ERPM) +#define DSHOT_EXTENDED_TELEMETRY_MASK (~DSHOT_NORMAL_TELEMETRY_MASK) + +typedef enum { + DSHOT_RAW_VALUE_STATE_INVALID = 0, + DSHOT_RAW_VALUE_STATE_NOT_PROCESSED, + DSHOT_RAW_VALUE_STATE_PROCESSED, +} dshotRawValueState_e; + +typedef struct { + uint16_t rawValue; + uint16_t telemetryData[DSHOT_TELEMETRY_TYPE_COUNT]; + uint8_t telemetryTypes; + uint8_t maxTemp; +} dshotTelemetryMotorState_t; + +typedef struct { + uint32_t invalidPacketCount; + uint32_t readCount; + dshotTelemetryMotorState_t motorState[MAX_SUPPORTED_MOTORS]; + dshotRawValueState_e rawValueState; +} dshotTelemetryState_t; + +extern bool useDshotTelemetry; +extern dshotTelemetryState_t dshotTelemetryState; + +void initDshotTelemetry(timeUs_t looptimeUs); +void dshotResetTelemetry(void); +bool isDshotTelemetryConfigured(void); +bool isDshotTelemetryActive(void); +uint16_t dshotProcessPacket(uint16_t rawValue, uint8_t motorIndex); +float getDshotRpm(uint8_t motorIndex); +uint16_t getDshotErpm(uint8_t motorIndex); +float getDshotRpmAverage(void); +float getMotorFrequencyHz(uint8_t motorIndex); +bool getDshotEscSensorData(escSensorData_t *data, uint8_t motorIndex); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index a4efb9e3008..8d0b9441064 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -25,12 +25,15 @@ #if !defined(SITL_BUILD) #include "build/debug.h" +#include "build/build_config.h" #include "common/log.h" #include "common/maths.h" #include "common/circular_queue.h" #include "drivers/io.h" +#include "drivers/dshot.h" +#include "drivers/nvic.h" #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" @@ -59,6 +62,8 @@ #define DSHOT_MOTOR_BITLENGTH 20 #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ +#define DSHOT_TELEMETRY_DEADTIME_US 35 +#define GCR_TELEMETRY_INPUT_LEN MAX_GCR_EDGES #define MAX_DMA_TIMERS 8 #define DSHOT_COMMAND_DELAY_US 1000 @@ -85,10 +90,13 @@ typedef struct { #ifdef USE_DSHOT // DSHOT parameters - timerDMASafeType_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; + timerDMASafeType_t dmaBuffer[GCR_TELEMETRY_INPUT_LEN]; #ifdef USE_DSHOT_DMAR timerDMASafeType_t *dmaBurstBuffer; #endif + bool telemetryInputActive; + timeUs_t telemetryInputStampUs; + timeUs_t dshotTelemetryDeadtimeUs; #endif } pwmOutputPort_t; @@ -120,6 +128,7 @@ static timeUs_t digitalMotorUpdateIntervalUs = 0; static timeUs_t digitalMotorLastUpdateUs; static timeUs_t lastCommandSent = 0; static timeUs_t commandPostDelay = 0; +static bool dshotTelemetryPending = false; static circularBuffer_t commandsCircularBuffer; static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE]; @@ -136,6 +145,13 @@ static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS]; uint8_t burstDmaTimersCount = 0; #endif + +// GCR decode algorithm ported from Betaflight (GPLv3) +static uint16_t dshotDecodeTelemetryPacket(const uint32_t buffer[], uint32_t count); +static bool pwmDshotDecodeTelemetry(void); +static void pwmDshotSetDirectionOutput(pwmOutputPort_t *port); +static void pwmDshotSetDirectionInput(pwmOutputPort_t *port); +static void pwmDshotDmaIrqHandler(DMA_t descriptor); #endif static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) @@ -338,6 +354,319 @@ static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer) } #endif +static uint32_t dshotDmaSource(const pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) || !defined(AT32F43x) + static const uint32_t sources[] = { TIM_DMA_CC1, TIM_DMA_CC2, TIM_DMA_CC3, TIM_DMA_CC4 }; +#else + static const uint32_t sources[] = { TMR_C1_DMA_REQUEST, TMR_C2_DMA_REQUEST, TMR_C3_DMA_REQUEST, TMR_C4_DMA_REQUEST }; +#endif + return sources[port->tch->timHw->channelIndex]; +} + +static uint32_t dshotDmaStream(const pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) + static const uint32_t streams[] = { + LL_DMA_STREAM_0, LL_DMA_STREAM_1, LL_DMA_STREAM_2, LL_DMA_STREAM_3, + LL_DMA_STREAM_4, LL_DMA_STREAM_5, LL_DMA_STREAM_6, LL_DMA_STREAM_7 + }; + return streams[DMATAG_GET_STREAM(port->tch->timHw->dmaTag)]; +#else + UNUSED(port); + return 0; +#endif +} + +static uint32_t dshotTimChannel(const pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) + static const uint32_t channels[] = { TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3, TIM_CHANNEL_4 }; +#elif defined(AT32F43x) + static const uint32_t channels[] = { TMR_SELECT_CHANNEL_1, TMR_SELECT_CHANNEL_2, TMR_SELECT_CHANNEL_3, TMR_SELECT_CHANNEL_4 }; +#else + static const uint32_t channels[] = { TIM_Channel_1, TIM_Channel_2, TIM_Channel_3, TIM_Channel_4 }; +#endif + return channels[port->tch->timHw->channelIndex]; +} + +static uint16_t dshotDecodeTelemetryPacket(const uint32_t buffer[], uint32_t count) +{ + uint32_t value = 0; + uint32_t oldValue = buffer[0]; + int bits = 0; + + for (uint32_t i = 1; i <= count; i++) { + int len; + if (i < count) { + const int diff = buffer[i] - oldValue; + if (bits >= 21) { + break; + } + len = (diff + 8) / 16; + } else { + len = 21 - bits; + } + + value <<= len; + value |= 1 << (len - 1); + if (i < count) { + oldValue = buffer[i]; + } + bits += len; + } + + if (bits != 21) { + return DSHOT_TELEMETRY_INVALID; + } + + static const uint32_t decode[32] = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15, + 0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 + }; + + uint32_t decodedValue = decode[value & 0x1f]; + decodedValue |= decode[(value >> 5) & 0x1f] << 4; + decodedValue |= decode[(value >> 10) & 0x1f] << 8; + decodedValue |= decode[(value >> 15) & 0x1f] << 12; + + uint32_t csum = decodedValue; + csum ^= csum >> 8; + csum ^= csum >> 4; + + if ((csum & 0xf) != 0xf) { + return DSHOT_TELEMETRY_INVALID; + } + + return decodedValue >> 4; +} + +static void pwmDshotSetDirectionOutput(pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) + TIM_OC_InitTypeDef init = {0}; + init.OCMode = TIM_OCMODE_PWM1; + init.OCIdleState = TIM_OCIDLESTATE_SET; + init.OCPolarity = TIM_OCPOLARITY_LOW; + init.OCNIdleState = TIM_OCNIDLESTATE_SET; + init.OCNPolarity = TIM_OCNPOLARITY_LOW; + init.Pulse = 0; + init.OCFastMode = TIM_OCFAST_DISABLE; + HAL_TIM_PWM_ConfigChannel(port->tch->timCtx->timHandle, &init, dshotTimChannel(port)); + if (port->tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + HAL_TIMEx_PWMN_Start(port->tch->timCtx->timHandle, dshotTimChannel(port)); + } else { + HAL_TIM_PWM_Start(port->tch->timCtx->timHandle, dshotTimChannel(port)); + } + + const uint32_t streamLL = dshotDmaStream(port); + DMA_TypeDef *dmaBase = port->tch->dma->dma; + LL_DMA_DisableStream(dmaBase, streamLL); + while (LL_DMA_IsEnabledStream(dmaBase, streamLL)) { } + LL_DMA_ConfigAddresses(dmaBase, streamLL, (uint32_t)port->dmaBuffer, (uint32_t)port->ccr, LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + LL_DMA_SetDataLength(dmaBase, streamLL, DSHOT_DMA_BUFFER_SIZE); + LL_DMA_EnableIT_TC(dmaBase, streamLL); + port->telemetryInputActive = false; +#elif defined(AT32F43x) + tmr_output_config_type output = {0}; + tmr_output_default_para_init(&output); + output.oc_mode = TMR_OUTPUT_CONTROL_PWM_MODE_A; + if (port->tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + output.oc_output_state = FALSE; + output.occ_output_state = TRUE; + output.occ_polarity = TMR_OUTPUT_ACTIVE_LOW; + output.occ_idle_state = FALSE; + } else { + output.oc_output_state = TRUE; + output.occ_output_state = FALSE; + output.oc_polarity = TMR_OUTPUT_ACTIVE_LOW; + output.oc_idle_state = TRUE; + } + tmr_output_channel_config(port->tch->timHw->tim, dshotTimChannel(port), &output); + tmr_channel_value_set(port->tch->timHw->tim, dshotTimChannel(port), 0); + tmr_output_channel_buffer_enable(port->tch->timHw->tim, dshotTimChannel(port), TRUE); + dma_channel_enable(port->tch->dma->ref, FALSE); + port->tch->dma->ref->maddr = (uint32_t)port->dmaBuffer; + port->tch->dma->ref->dtcnt = DSHOT_DMA_BUFFER_SIZE; + port->telemetryInputActive = false; +#else + TIM_ARRPreloadConfig(port->tch->timHw->tim, DISABLE); + TIM_SetAutoreload(port->tch->timHw->tim, DSHOT_MOTOR_BITLENGTH - 1); + TIM_ARRPreloadConfig(port->tch->timHw->tim, ENABLE); + TIM_SetCounter(port->tch->timHw->tim, 0); + TIM_OCInitTypeDef init; + TIM_OCStructInit(&init); + init.TIM_OCMode = TIM_OCMode_PWM1; + init.TIM_Pulse = 0; + if (port->tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + init.TIM_OutputState = TIM_OutputState_Disable; + init.TIM_OutputNState = TIM_OutputNState_Enable; + init.TIM_OCNPolarity = TIM_OCPolarity_Low; + init.TIM_OCNIdleState = TIM_OCIdleState_Reset; + } else { + init.TIM_OutputState = TIM_OutputState_Enable; + init.TIM_OutputNState = TIM_OutputNState_Disable; + init.TIM_OCPolarity = TIM_OCPolarity_Low; + init.TIM_OCIdleState = TIM_OCIdleState_Set; + } + switch (port->tch->timHw->channelIndex) { + case 0: TIM_OC1Init(port->tch->timHw->tim, &init); TIM_OC1PreloadConfig(port->tch->timHw->tim, TIM_OCPreload_Enable); break; + case 1: TIM_OC2Init(port->tch->timHw->tim, &init); TIM_OC2PreloadConfig(port->tch->timHw->tim, TIM_OCPreload_Enable); break; + case 2: TIM_OC3Init(port->tch->timHw->tim, &init); TIM_OC3PreloadConfig(port->tch->timHw->tim, TIM_OCPreload_Enable); break; + default: TIM_OC4Init(port->tch->timHw->tim, &init); TIM_OC4PreloadConfig(port->tch->timHw->tim, TIM_OCPreload_Enable); break; + } + DMA_Cmd(port->tch->dma->ref, DISABLE); + port->tch->dma->ref->CR = (port->tch->dma->ref->CR & ~(DMA_DIR_MemoryToPeripheral | DMA_DIR_MemoryToMemory)) | DMA_DIR_MemoryToPeripheral; + port->tch->dma->ref->M0AR = (uint32_t)port->dmaBuffer; + DMA_SetCurrDataCounter(port->tch->dma->ref, DSHOT_DMA_BUFFER_SIZE); + port->telemetryInputActive = false; +#endif +} + +static void pwmDshotSetDirectionInput(pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) + TIM_IC_InitTypeDef init = {0}; + init.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE; + init.ICSelection = TIM_ICSELECTION_DIRECTTI; + init.ICPrescaler = TIM_ICPSC_DIV1; + init.ICFilter = 2; + HAL_TIM_IC_ConfigChannel(port->tch->timCtx->timHandle, &init, dshotTimChannel(port)); + HAL_TIM_IC_Start(port->tch->timCtx->timHandle, dshotTimChannel(port)); + + const uint32_t streamLL = dshotDmaStream(port); + DMA_TypeDef *dmaBase = port->tch->dma->dma; + LL_DMA_DisableStream(dmaBase, streamLL); + while (LL_DMA_IsEnabledStream(dmaBase, streamLL)) { } + LL_DMA_ConfigAddresses(dmaBase, streamLL, (uint32_t)port->ccr, (uint32_t)port->dmaBuffer, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_SetDataLength(dmaBase, streamLL, GCR_TELEMETRY_INPUT_LEN); + LL_DMA_EnableIT_TC(dmaBase, streamLL); + LL_DMA_EnableStream(dmaBase, streamLL); + LL_TIM_EnableDMAReq_CCx(port->tch->timHw->tim, dshotDmaSource(port)); +#elif defined(AT32F43x) + tmr_input_config_type input; + tmr_input_default_para_init(&input); + input.input_channel_select = dshotTimChannel(port); + input.input_mapped_select = TMR_CC_CHANNEL_MAPPED_DIRECT; + input.input_filter_value = 2; + input.input_polarity_select = TMR_INPUT_BOTH_EDGE; + tmr_input_channel_init(port->tch->timHw->tim, &input, TMR_CHANNEL_INPUT_DIV_1); + port->tch->dma->ref->paddr = (uint32_t)port->ccr; + port->tch->dma->ref->maddr = (uint32_t)port->dmaBuffer; + port->tch->dma->ref->dtcnt = GCR_TELEMETRY_INPUT_LEN; + dma_channel_enable(port->tch->dma->ref, TRUE); + tmr_dma_request_enable(port->tch->timHw->tim, dshotDmaSource(port), TRUE); +#else + TIM_ARRPreloadConfig(port->tch->timHw->tim, ENABLE); + TIM_SetAutoreload(port->tch->timHw->tim, 0xffff); + TIM_ICInitTypeDef init; + TIM_ICStructInit(&init); + init.TIM_Channel = dshotTimChannel(port); + init.TIM_ICSelection = TIM_ICSelection_DirectTI; + init.TIM_ICPrescaler = TIM_ICPSC_DIV1; + init.TIM_ICFilter = 2; + init.TIM_ICPolarity = TIM_ICPolarity_BothEdge; + TIM_ICInit(port->tch->timHw->tim, &init); + DMA_Cmd(port->tch->dma->ref, DISABLE); + port->tch->dma->ref->CR &= ~(DMA_DIR_MemoryToPeripheral | DMA_DIR_MemoryToMemory); // P→M + port->tch->dma->ref->PAR = (uint32_t)port->ccr; + port->tch->dma->ref->M0AR = (uint32_t)port->dmaBuffer; + DMA_SetCurrDataCounter(port->tch->dma->ref, GCR_TELEMETRY_INPUT_LEN); + // Clear stale DMA flags (TCIF/HTIF from completed output DMA) before + // enabling input capture, otherwise a spurious TC fires immediately and + // kills the capture DMA before any edges can be recorded. + DMA_CLEAR_FLAG(port->tch->dma, DMA_IT_TCIF | DMA_IT_HTIF | DMA_IT_TEIF); + DMA_Cmd(port->tch->dma->ref, ENABLE); + TIM_DMACmd(port->tch->timHw->tim, dshotDmaSource(port), ENABLE); +#endif + port->telemetryInputStampUs = micros(); + port->telemetryInputActive = true; +} + +static void pwmDshotDmaIrqHandler(DMA_t descriptor) +{ + if (!DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { + return; + } + + pwmOutputPort_t *port = &pwmOutputPorts[descriptor->userParam]; + +#if defined(USE_HAL_DRIVER) + const uint32_t streamLL = dshotDmaStream(port); + LL_DMA_DisableStream(port->tch->dma->dma, streamLL); + LL_TIM_DisableDMAReq_CCx(port->tch->timHw->tim, dshotDmaSource(port)); +#elif defined(AT32F43x) + dma_channel_enable(port->tch->dma->ref, FALSE); + tmr_dma_request_enable(port->tch->timHw->tim, dshotDmaSource(port), FALSE); +#else + DMA_Cmd(port->tch->dma->ref, DISABLE); + TIM_DMACmd(port->tch->timHw->tim, dshotDmaSource(port), DISABLE); +#endif + + if (useDshotTelemetry && !port->telemetryInputActive) { + pwmDshotSetDirectionInput(port); + dshotTelemetryPending = true; + } + + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); +} + +static bool pwmDshotDecodeTelemetry(void) +{ + if (!useDshotTelemetry || !dshotTelemetryPending) { + return true; + } + + const timeUs_t now = micros(); + for (int motorIndex = 0; motorIndex < getMotorCount(); motorIndex++) { + pwmOutputPort_t *port = motors[motorIndex].pwmPort; + if (!port || !port->configured || !port->telemetryInputActive) { + continue; + } + + if ((now - port->telemetryInputStampUs) < port->dshotTelemetryDeadtimeUs) { + return false; + } + + uint32_t edges = 0; +#if defined(USE_HAL_DRIVER) + const uint32_t streamLL = dshotDmaStream(port); + edges = GCR_TELEMETRY_INPUT_LEN - LL_DMA_GetDataLength(port->tch->dma->dma, streamLL); + LL_TIM_DisableDMAReq_CCx(port->tch->timHw->tim, dshotDmaSource(port)); + LL_DMA_DisableStream(port->tch->dma->dma, streamLL); +#elif defined(AT32F43x) + edges = GCR_TELEMETRY_INPUT_LEN - dma_data_number_get(port->tch->dma->ref); + tmr_dma_request_enable(port->tch->timHw->tim, dshotDmaSource(port), FALSE); + dma_channel_enable(port->tch->dma->ref, FALSE); +#else + edges = GCR_TELEMETRY_INPUT_LEN - DMA_GetCurrDataCounter(port->tch->dma->ref); + TIM_DMACmd(port->tch->timHw->tim, dshotDmaSource(port), DISABLE); + DMA_Cmd(port->tch->dma->ref, DISABLE); +#endif + + if (edges > MIN_GCR_EDGES) { + const uint16_t rawValue = dshotDecodeTelemetryPacket((const uint32_t *)port->dmaBuffer, edges); + const uint16_t processed = dshotProcessPacket(rawValue, motorIndex); + + if (processed != DSHOT_TELEMETRY_INVALID && processed != DSHOT_TELEMETRY_NOEDGE) { +#ifdef USE_ESC_SENSOR + escSensorData_t data; + if (getDshotEscSensorData(&data, motorIndex)) { + escSensorSetDshotData(motorIndex, computeRpm((int16_t)data.rpm), data.temperature, data.voltage, data.current); + } else { + escSensorSetDshotData(motorIndex, (uint32_t)getDshotRpm(motorIndex), 0, 0, 0); + } +#endif + } + } + + pwmDshotSetDirectionOutput(port); + } + + dshotTelemetryPending = false; + return true; +} + static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput) { // Try allocating new port @@ -347,28 +676,37 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, return NULL; } + if (enableOutput && useDshotTelemetry) { + IOConfigGPIOAF(IOGetByTag(timerHardware->tag), IOCFG_AF_PP_UP, timerHardware->alternateFunction); + } + // Configure timer DMA #ifdef USE_DSHOT_DMAR - //uint8_t timerIndex = lookupTimerIndex(timerHardware->tim); - uint8_t burstDmaTimerIndex = getBurstDmaTimerIndex(timerHardware->tim); - if (burstDmaTimerIndex >= MAX_DMA_TIMERS) { - return NULL; - } + if (!useDshotTelemetry) { + uint8_t burstDmaTimerIndex = getBurstDmaTimerIndex(timerHardware->tim); + if (burstDmaTimerIndex >= MAX_DMA_TIMERS) { + return NULL; + } - port->dmaBurstBuffer = &dmaBurstBuffer[burstDmaTimerIndex][0]; - burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex]; - burstDmaTimer->dmaBurstBuffer = port->dmaBurstBuffer; + port->dmaBurstBuffer = &dmaBurstBuffer[burstDmaTimerIndex][0]; + burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex]; + burstDmaTimer->dmaBurstBuffer = port->dmaBurstBuffer; - if (timerPWMConfigDMABurst(burstDmaTimer, port->tch, port->dmaBurstBuffer, sizeof(port->dmaBurstBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { - port->configured = true; - } -#else - if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { + if (timerPWMConfigDMABurst(burstDmaTimer, port->tch, port->dmaBurstBuffer, sizeof(port->dmaBurstBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { + port->configured = true; + } + } else +#endif + { + if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), GCR_TELEMETRY_INPUT_LEN)) { // Only mark as DSHOT channel if DMA was set successfully ZERO_FARRAY(port->dmaBuffer); port->configured = true; + port->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 * (16 * DSHOT_MOTOR_BITLENGTH) / dshotHz; + pwmDshotSetDirectionOutput(port); + dmaSetHandler(port->tch->dma, pwmDshotDmaIrqHandler, NVIC_PRIO_TIMER_DMA, allocatedOutputPortCount - 1); + } } -#endif return port; } @@ -391,11 +729,17 @@ static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) dmaBuffer[i] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; // MSB first packet <<= 1; } + dmaBuffer[16] = 0; + dmaBuffer[17] = 0; } #endif static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) { + if (useDshotTelemetry) { + requestTelemetry = true; + } + uint16_t packet = (value << 1) | (requestTelemetry ? 1 : 0); // compute checksum @@ -405,6 +749,11 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) csum ^= csum_data; // xor data by nibbles csum_data >>= 4; } +#ifdef USE_DSHOT + if (useDshotTelemetry) { + csum = ~csum; + } +#endif csum &= 0xf; // append checksum @@ -524,6 +873,10 @@ void pwmCompleteMotorUpdate(void) { return; } + if (useDshotTelemetry && !pwmDshotDecodeTelemetry()) { + return; + } + int motorCount = getMotorCount(); timeUs_t currentTimeUs = micros(); @@ -542,6 +895,7 @@ void pwmCompleteMotorUpdate(void) { } #ifdef USE_DSHOT_DMAR + if (!useDshotTelemetry) { for (int index = 0; index < motorCount; index++) { if (motors[index].pwmPort && motors[index].pwmPort->configured) { uint16_t packet = prepareDshotPacket(motors[index].value, motors[index].requestTelemetry); @@ -554,7 +908,9 @@ void pwmCompleteMotorUpdate(void) { burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex]; pwmBurstDMAStart(burstDmaTimer, DSHOT_DMA_BUFFER_SIZE * 4); } -#else + } else +#endif + { // Generate DMA buffers for (int index = 0; index < motorCount; index++) { if (motors[index].pwmPort && motors[index].pwmPort->configured) { @@ -571,7 +927,7 @@ void pwmCompleteMotorUpdate(void) { timerPWMStartDMA(motors[index].pwmPort->tch); } } -#endif + } } #endif } @@ -590,6 +946,9 @@ void pwmMotorPreconfigure(void) { // Keep track of initial motor protocol initMotorProtocol = motorConfig()->motorPwmProtocol; +#ifdef USE_DSHOT + useDshotTelemetry = motorConfig()->useDshotTelemetry && getMotorProtocolProperties(initMotorProtocol)->isDSHOT; +#endif #ifdef BRUSHED_MOTORS initMotorProtocol = PWM_TYPE_BRUSHED; // Override proto diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a8ca6c0c199..223e2546a66 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -50,6 +50,7 @@ #include "drivers/compass/compass.h" #include "drivers/bus.h" #include "drivers/dma.h" +#include "drivers/dshot.h" #include "drivers/exti.h" #include "drivers/io.h" #include "drivers/flash.h" @@ -695,6 +696,7 @@ void init(void) #ifdef USE_DSHOT initDShotCommands(); + initDshotTelemetry(getLooptime()); #endif #ifdef USE_SERIAL_GIMBAL @@ -728,7 +730,7 @@ void init(void) #ifdef USE_RPM_FILTER disableRpmFilters(); - if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) { + if ((escSensorIsActive() || isDshotTelemetryActive()) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) { rpmFiltersInit(); setTaskEnabled(TASK_RPM_FILTER, true); } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6ef471e3bb0..debdba477f1 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -851,6 +851,16 @@ groups: default_value: "ONESHOT125" field: motorPwmProtocol table: motor_pwm_protocol + - name: dshot_bidir_enabled + description: "Enable bidirectional DShot telemetry on motor outputs. Required for RPM filtering without a separate ESC telemetry UART" + field: useDshotTelemetry + type: bool + default_value: OFF + - name: dshot_edt_enabled + description: "Enable extended DShot telemetry decoding (temperature, voltage, current in addition to eRPM)" + field: useDshotEdt + type: bool + default_value: OFF - name: motor_poles field: motorPoleCount description: "The number of motor poles. Required to compute motor RPM" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a80992b772d..53c8255e8d1 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -86,11 +86,13 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, .neutral = SETTING_3D_NEUTRAL_DEFAULT ); -PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 11); +PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 12); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT, .motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT, + .useDshotTelemetry = 0, + .useDshotEdt = 0, .mincommand = SETTING_MIN_COMMAND_DEFAULT, .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles ); @@ -737,4 +739,4 @@ uint16_t getMaxThrottle(void) { } return throttle; -} \ No newline at end of file +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 12688bd2c09..7a9e112c9d8 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -84,6 +84,8 @@ typedef struct motorConfig_s { uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) uint8_t motorPwmProtocol; + uint8_t useDshotTelemetry; + uint8_t useDshotEdt; uint16_t digitalIdleOffsetValue; uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry } motorConfig_t; diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index ffca6c8ea68..14da11482bb 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -36,13 +36,13 @@ #include "common/maths.h" #include "common/filter.h" #include "flight/mixer.h" +#include "drivers/dshot.h" #include "sensors/esc_sensor.h" #include "fc/config.h" #include "fc/settings.h" #ifdef USE_RPM_FILTER -#define HZ_TO_RPM 1/60.0f #define RPM_FILTER_RPM_LPF_HZ 150 #define RPM_FILTER_HARMONICS 3 @@ -66,7 +66,9 @@ typedef struct typedef float (*rpmFilterApplyFnPtr)(rpmFilterBank_t *filter, uint8_t axis, float input); typedef void (*rpmFilterUpdateFnPtr)(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency); +#ifdef USE_ESC_SENSOR static EXTENDED_FASTRAM pt1Filter_t motorFrequencyFilter[MAX_SUPPORTED_MOTORS]; +#endif static EXTENDED_FASTRAM rpmFilterBank_t gyroRpmFilters; static EXTENDED_FASTRAM rpmFilterApplyFnPtr rpmGyroApplyFn; static EXTENDED_FASTRAM rpmFilterUpdateFnPtr rpmGyroUpdateFn; @@ -159,10 +161,11 @@ void rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequ void rpmFiltersInit(void) { - for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) - { +#ifdef USE_ESC_SENSOR + for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { pt1FilterInit(&motorFrequencyFilter[i], RPM_FILTER_RPM_LPF_HZ, US2S(RPM_FILTER_UPDATE_RATE_US)); } +#endif rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate; @@ -188,9 +191,17 @@ void rpmFilterUpdateTask(timeUs_t currentTimeUs) */ for (uint8_t i = 0; i < motorCount; i++) { - const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry - const float baseFrequency = pt1FilterApply(&motorFrequencyFilter[i], escState->rpm * HZ_TO_RPM); //Filter motor frequency - + float baseFrequency; + if (isDshotTelemetryActive()) { + baseFrequency = getMotorFrequencyHz(i); + } else { +#ifdef USE_ESC_SENSOR + const escSensorData_t *escState = getEscTelemetry(i); + baseFrequency = pt1FilterApply(&motorFrequencyFilter[i], (float)escState->rpm / 60.0f); +#else + baseFrequency = 0.0f; +#endif + } rpmGyroUpdateFn(&gyroRpmFilters, i, baseFrequency); } } diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index bc77c281c79..3556f679938 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -41,6 +41,8 @@ #include "config/parameter_group_ids.h" #include "flight/mixer.h" +#include "drivers/dshot.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "sensors/esc_sensor.h" #include "io/serial.h" @@ -77,6 +79,7 @@ static int bufferPosition = 0; static escSensorData_t escSensorData[MAX_SUPPORTED_MOTORS]; static escSensorData_t escSensorDataCombined; static bool escSensorDataNeedsUpdate; +static bool escSensorDshotActive; PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSOR_CONFIG, 1); PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, @@ -154,9 +157,40 @@ escSensorData_t NOINLINE * getEscTelemetry(uint8_t esc) return &escSensorData[esc]; } +void escSensorInitData(void) +{ + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + escSensorData[i].dataAge = ESC_DATA_INVALID; + escSensorData[i].temperature = 0; + escSensorData[i].voltage = 0; + escSensorData[i].current = 0; + escSensorData[i].rpm = 0; + } + escSensorDataNeedsUpdate = true; +} + +void escSensorSetDshotData(uint8_t esc, uint32_t rpm, int16_t temperature, int16_t voltage, int32_t current) +{ + if (esc >= MAX_SUPPORTED_MOTORS) { + return; + } + + escSensorData[esc].dataAge = 0; + escSensorData[esc].rpm = rpm; + escSensorData[esc].temperature = temperature; + escSensorData[esc].voltage = voltage; + escSensorData[esc].current = current; + escSensorDataNeedsUpdate = true; +} + +bool escSensorIsActive(void) +{ + return escSensorPort || escSensorDshotActive; +} + escSensorData_t * escSensorGetData(void) { - if (!escSensorPort) { + if (!escSensorIsActive()) { return NULL; } @@ -206,12 +240,21 @@ bool escSensorInitialize(void) { escSensorDataNeedsUpdate = true; escSensorPort = NULL; + escSensorDshotActive = false; // Fail immediately if motor output are disabled or motor outputs are not configured if (!feature(FEATURE_PWM_OUTPUT_ENABLE) || getMotorCount() == 0) { return false; } + escSensorInitData(); + + if (motorConfig()->useDshotTelemetry && (motorConfig()->motorPwmProtocol >= PWM_TYPE_DSHOT150)) { + escSensorDshotActive = true; + ENABLE_STATE(ESC_SENSOR_ENABLED); + return true; + } + // FUNCTION_ESCSERIAL is shared between SERIALSHOT and ESC_SENSOR telemetry // They are mutually exclusive serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_ESCSERIAL); @@ -224,10 +267,6 @@ bool escSensorInitialize(void) return false; } - for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - escSensorData[i].dataAge = ESC_DATA_INVALID; - } - ENABLE_STATE(ESC_SENSOR_ENABLED); return true; @@ -293,3 +332,13 @@ void escSensorUpdate(timeUs_t currentTimeUs) } #endif + +#ifndef USE_ESC_SENSOR +bool escSensorIsActive(void) { return false; } +void escSensorInitData(void) {} +void escSensorSetDshotData(uint8_t esc, uint32_t rpm, int16_t temperature, int16_t voltage, int32_t current) +{ + UNUSED(esc); UNUSED(rpm); UNUSED(temperature); UNUSED(voltage); UNUSED(current); +} +escSensorData_t * escSensorGetData(void) { return NULL; } +#endif diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index a5958c85c16..ba86ccc66fc 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -48,3 +48,6 @@ void escSensorUpdate(timeUs_t currentTimeUs); escSensorData_t * escSensorGetData(void); escSensorData_t * getEscTelemetry(uint8_t esc); uint32_t computeRpm(int16_t erpm); +void escSensorInitData(void); +void escSensorSetDshotData(uint8_t esc, uint32_t rpm, int16_t temperature, int16_t voltage, int32_t current); +bool escSensorIsActive(void); diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index ccd14d35a4a..102ec273e04 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -131,7 +131,7 @@ extern uint8_t __config_end; #endif -#ifdef USE_ESC_SENSOR +#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT) #define USE_RPM_FILTER #endif diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index ae3dcf9b7f1..865e4551659 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -47,6 +47,7 @@ #include "flight/imu.h" #include "flight/mixer.h" +#include "drivers/dshot.h" #include "io/gps.h" @@ -64,7 +65,6 @@ #include "telemetry/srxl.h" #include "drivers/vtx_common.h" -//#include "drivers/dshot.h" #include "io/vtx_tramp.h" #include "io/vtx_smartaudio.h" @@ -184,30 +184,14 @@ typedef struct uint16_t getMotorAveragePeriod(void) { -#if defined( USE_ESC_SENSOR_TELEMETRY) || defined( USE_DSHOT_TELEMETRY) +#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT) uint32_t rpm = 0; uint16_t period_us = SPEKTRUM_RPM_UNUSED; -#if defined( USE_ESC_SENSOR_TELEMETRY) - escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); + escSensorData_t *escData = escSensorGetData(); if (escData != NULL) { rpm = escData->rpm; } -#endif - -#if defined(USE_DSHOT_TELEMETRY) - if (useDshotTelemetry) { - uint16_t motors = getMotorCount(); - - if (motors > 0) { - for (int motor = 0; motor < motors; motor++) { - rpm += getDshotTelemetry(motor); - } - rpm = 100.0f / (motorConfig()->motorPoleCount / 2.0f) * rpm; // convert erpm freq to RPM. - rpm /= motors; // Average combined rpm - } - } -#endif if (rpm > SPEKTRUM_MIN_RPM && rpm < SPEKTRUM_MAX_RPM) { period_us = MICROSEC_PER_MINUTE / rpm; // revs/minute -> microSeconds From fe94094af2de2d377ee50d58042136bef710dd99 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 19 Aug 2026 20:10:51 -0500 Subject: [PATCH 2/9] Fix unused-function CI failure on F4 StdPeriph targets dshotDmaStream() is only called from the USE_HAL_DRIVER code paths, but was defined unconditionally with a dead fallback stub, so non-HAL F4 targets (e.g. FF_F35_LIGHTNING) built it with -Werror=unused-function. Scope the definition to USE_HAL_DRIVER to match its call sites. --- src/main/drivers/pwm_output.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 8f4448fd9a2..2da0628913d 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -364,19 +364,16 @@ static uint32_t dshotDmaSource(const pwmOutputPort_t *port) return sources[port->tch->timHw->channelIndex]; } +#if defined(USE_HAL_DRIVER) static uint32_t dshotDmaStream(const pwmOutputPort_t *port) { -#if defined(USE_HAL_DRIVER) static const uint32_t streams[] = { LL_DMA_STREAM_0, LL_DMA_STREAM_1, LL_DMA_STREAM_2, LL_DMA_STREAM_3, LL_DMA_STREAM_4, LL_DMA_STREAM_5, LL_DMA_STREAM_6, LL_DMA_STREAM_7 }; return streams[DMATAG_GET_STREAM(port->tch->timHw->dmaTag)]; -#else - UNUSED(port); - return 0; -#endif } +#endif static uint32_t dshotTimChannel(const pwmOutputPort_t *port) { From 0dcf861e5677fff769c48cba55599d7ea72108dc Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 19 Aug 2026 20:35:49 -0500 Subject: [PATCH 3/9] Fix implicit-function-declaration on F7/H7 DMA request calls pwm_output.c called LL_TIM_EnableDMAReq_CCx/LL_TIM_DisableDMAReq_CCx, which don't exist in ST's LL timer driver (only per-channel CC1..CC4 variants are provided). Add local static inline wrappers that set/clear the DIER CCxDE bits directly by mask, matching the equivalent helpers already used internally by timer_impl_hal.c. --- src/main/drivers/pwm_output.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 2da0628913d..d9417c2a4e9 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -373,6 +373,18 @@ static uint32_t dshotDmaStream(const pwmOutputPort_t *port) }; return streams[DMATAG_GET_STREAM(port->tch->timHw->dmaTag)]; } + +// The LL driver only exposes per-channel LL_TIM_{En,Dis}ableDMAReq_CC1..CC4; +// there is no runtime-channel variant, so set/clear the DIER bit directly. +static inline void LL_TIM_EnableDMAReq_CCx(TIM_TypeDef *TIMx, uint16_t dmaSources) +{ + SET_BIT(TIMx->DIER, dmaSources & (TIM_DMA_CC1 | TIM_DMA_CC2 | TIM_DMA_CC3 | TIM_DMA_CC4)); +} + +static inline void LL_TIM_DisableDMAReq_CCx(TIM_TypeDef *TIMx, uint16_t dmaSources) +{ + CLEAR_BIT(TIMx->DIER, dmaSources & (TIM_DMA_CC1 | TIM_DMA_CC2 | TIM_DMA_CC3 | TIM_DMA_CC4)); +} #endif static uint32_t dshotTimChannel(const pwmOutputPort_t *port) From 61fcc05f74274f7b590fdcc7a3becbd2f39441c2 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 19 Aug 2026 23:38:00 -0500 Subject: [PATCH 4/9] docs: regenerate Settings.md for PR #11605's new DShot telemetry settings dshot_bidir_enabled and dshot_edt_enabled were added to settings.yaml but docs/Settings.md was never regenerated, failing CI's update_cli_docs.py check. Ran the generator; purely additive diff, no other drift. --- docs/Settings.md | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index c8eb8bb0716..6402f11ba02 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -925,6 +925,26 @@ Sets the DShot beeper tone --- +### dshot_bidir_enabled + +Enable bidirectional DShot telemetry on motor outputs. Required for RPM filtering without a separate ESC telemetry UART + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### dshot_edt_enabled + +Enable extended DShot telemetry decoding (temperature, voltage, current in addition to eRPM) + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### dterm_lpf2_hz Dterm pre-differentiation LPF cutoff (Hz). Filters gyro before differentiation to reduce noise amplification. Higher = less delay, more noise. 0 = disabled. Values around 200-250Hz can add smoothing with small delay. From 6d26d2a9e150f21565957f4605beaeaf011debce Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Thu, 20 Aug 2026 00:00:53 -0500 Subject: [PATCH 5/9] Fix implicit-function-declaration for loadDmaBufferDshot on DMAR targets loadDmaBufferDshot() was only defined when USE_DSHOT_DMAR was unset, but pwmCompleteMotorUpdate() also calls it as the per-channel DMA fallback on DMAR targets whenever bidir telemetry is enabled (burst DMA can't drive per-channel direction switching). Define it unconditionally; keep loadDmaBufferDshotStride() gated behind USE_DSHOT_DMAR since it's only used by the DMAR-specific burst path. --- src/main/drivers/pwm_output.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index d9417c2a4e9..5bac17c2e9e 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -135,9 +135,8 @@ static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE]; static currentExecutingCommand_t currentExecutingCommand; static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry); -#ifndef USE_DSHOT_DMAR static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet); -#else +#ifdef USE_DSHOT_DMAR static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, uint16_t packet); #endif @@ -731,7 +730,11 @@ static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, dmaBuffer[i++ * stride] = 0; dmaBuffer[i++ * stride] = 0; } -#else +#endif + +// Needed on every target, DMAR or not: DMAR targets fall back to this +// per-channel loader whenever bidir telemetry is enabled, since burst DMA +// can't drive per-channel direction switching. static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) { for (int i = 0; i < 16; i++) { @@ -741,7 +744,6 @@ static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) dmaBuffer[16] = 0; dmaBuffer[17] = 0; } -#endif static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) { From 3b2a74dec7bed6db2d347df3de2fb35e1475a221 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 20 Aug 2026 17:48:31 +0300 Subject: [PATCH 6/9] fix: keep DShot telemetry decode out of ITCM --- src/main/drivers/pwm_output.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 5bac17c2e9e..ac4eed2c26c 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -619,7 +619,9 @@ static void pwmDshotDmaIrqHandler(DMA_t descriptor) DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); } -static bool pwmDshotDecodeTelemetry(void) +// DMA has already captured the telemetry waveform before this runs. Keep the +// decode path out of the fast motor-update caller on constrained F7 targets. +static bool NOINLINE pwmDshotDecodeTelemetry(void) { if (!useDshotTelemetry || !dshotTelemetryPending) { return true; From ac6c735edf20f6e4f6041e20de0fbe975960c21a Mon Sep 17 00:00:00 2001 From: Error414 Date: Sat, 29 Aug 2026 20:52:09 +0200 Subject: [PATCH 7/9] BidirDshot - fix for H7, invertion of timer, reset DMA for reading telemetry, magic H7 workdown from betaflight --- src/main/drivers/pwm_output.c | 124 ++++++++++++++++++++++++++++++---- 1 file changed, 109 insertions(+), 15 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index ac4eed2c26c..3afdce20079 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -449,15 +449,64 @@ static uint16_t dshotDecodeTelemetryPacket(const uint32_t buffer[], uint32_t cou return decodedValue >> 4; } +#if defined(USE_HAL_DRIVER) +// Betaflight-style direction switching: instead of poking DIR/addresses into a live +// stream, every switch does a full LL_DMA_DeInit (which also clears all five stream +// event flags - EN=1 is ignored while any of them is set, RM0433) followed by a +// complete LL_DMA_Init built from scratch for the requested direction. +static void dshotDmaInit(const pwmOutputPort_t *port, LL_DMA_InitTypeDef *init, uint32_t direction) +{ + LL_DMA_StructInit(init); +#if defined(STM32H7) || defined(STM32G4) + init->PeriphRequest = DMATAG_GET_CHANNEL(port->tch->timHw->dmaTag); +#else + static const uint32_t channels[] = { + LL_DMA_CHANNEL_0, LL_DMA_CHANNEL_1, LL_DMA_CHANNEL_2, LL_DMA_CHANNEL_3, + LL_DMA_CHANNEL_4, LL_DMA_CHANNEL_5, LL_DMA_CHANNEL_6, LL_DMA_CHANNEL_7 + }; + init->Channel = channels[DMATAG_GET_CHANNEL(port->tch->timHw->dmaTag)]; +#endif + init->PeriphOrM2MSrcAddress = (uint32_t)port->ccr; + init->MemoryOrM2MDstAddress = (uint32_t)port->dmaBuffer; + init->Direction = direction; + init->NbData = (direction == LL_DMA_DIRECTION_MEMORY_TO_PERIPH) ? DSHOT_DMA_BUFFER_SIZE : GCR_TELEMETRY_INPUT_LEN; + init->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + init->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; + init->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD; + init->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD; + init->Mode = LL_DMA_MODE_NORMAL; + init->Priority = LL_DMA_PRIORITY_HIGH; + init->FIFOMode = LL_DMA_FIFOMODE_ENABLE; + // 1/4 threshold like Betaflight: each captured word is pushed to memory promptly + // instead of waiting for four, so a partial GCR frame isn't stuck in the FIFO. + init->FIFOThreshold = LL_DMA_FIFOTHRESHOLD_1_4; + init->MemBurst = LL_DMA_MBURST_SINGLE; + init->PeriphBurst = LL_DMA_PBURST_SINGLE; +} +#endif + static void pwmDshotSetDirectionOutput(pwmOutputPort_t *port) { + // Bidirectional DSHOT is carried on inverted (idle-high) signalling - flip the + // channel's normal polarity whenever telemetry is active, matching Betaflight. + const bool inverted = ((port->tch->timHw->output & TIMER_OUTPUT_INVERTED) != 0) ^ useDshotTelemetry; + #if defined(USE_HAL_DRIVER) + // Telemetry input capture widens ARR to 0xffff (see pwmDshotSetDirectionInput); + // restore the DSHOT bit period now, bypassing the ARR preload shadow so it + // takes effect immediately instead of after the next update event. + TIM_TypeDef *tim = port->tch->timHw->tim; + CLEAR_BIT(tim->CR1, TIM_CR1_ARPE); + tim->ARR = DSHOT_MOTOR_BITLENGTH - 1; + SET_BIT(tim->CR1, TIM_CR1_ARPE); + tim->CNT = 0; + TIM_OC_InitTypeDef init = {0}; init.OCMode = TIM_OCMODE_PWM1; init.OCIdleState = TIM_OCIDLESTATE_SET; - init.OCPolarity = TIM_OCPOLARITY_LOW; + init.OCPolarity = inverted ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH; init.OCNIdleState = TIM_OCNIDLESTATE_SET; - init.OCNPolarity = TIM_OCNPOLARITY_LOW; + init.OCNPolarity = inverted ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH; init.Pulse = 0; init.OCFastMode = TIM_OCFAST_DISABLE; HAL_TIM_PWM_ConfigChannel(port->tch->timCtx->timHandle, &init, dshotTimChannel(port)); @@ -471,8 +520,10 @@ static void pwmDshotSetDirectionOutput(pwmOutputPort_t *port) DMA_TypeDef *dmaBase = port->tch->dma->dma; LL_DMA_DisableStream(dmaBase, streamLL); while (LL_DMA_IsEnabledStream(dmaBase, streamLL)) { } - LL_DMA_ConfigAddresses(dmaBase, streamLL, (uint32_t)port->dmaBuffer, (uint32_t)port->ccr, LL_DMA_DIRECTION_MEMORY_TO_PERIPH); - LL_DMA_SetDataLength(dmaBase, streamLL, DSHOT_DMA_BUFFER_SIZE); + LL_DMA_DeInit(dmaBase, streamLL); + LL_DMA_InitTypeDef dmaInit; + dshotDmaInit(port, &dmaInit, LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + LL_DMA_Init(dmaBase, streamLL, &dmaInit); LL_DMA_EnableIT_TC(dmaBase, streamLL); port->telemetryInputActive = false; #elif defined(AT32F43x) @@ -482,12 +533,12 @@ static void pwmDshotSetDirectionOutput(pwmOutputPort_t *port) if (port->tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { output.oc_output_state = FALSE; output.occ_output_state = TRUE; - output.occ_polarity = TMR_OUTPUT_ACTIVE_LOW; + output.occ_polarity = inverted ? TMR_OUTPUT_ACTIVE_LOW : TMR_OUTPUT_ACTIVE_HIGH; output.occ_idle_state = FALSE; } else { output.oc_output_state = TRUE; output.occ_output_state = FALSE; - output.oc_polarity = TMR_OUTPUT_ACTIVE_LOW; + output.oc_polarity = inverted ? TMR_OUTPUT_ACTIVE_LOW : TMR_OUTPUT_ACTIVE_HIGH; output.oc_idle_state = TRUE; } tmr_output_channel_config(port->tch->timHw->tim, dshotTimChannel(port), &output); @@ -509,12 +560,12 @@ static void pwmDshotSetDirectionOutput(pwmOutputPort_t *port) if (port->tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { init.TIM_OutputState = TIM_OutputState_Disable; init.TIM_OutputNState = TIM_OutputNState_Enable; - init.TIM_OCNPolarity = TIM_OCPolarity_Low; + init.TIM_OCNPolarity = inverted ? TIM_OCPolarity_Low : TIM_OCPolarity_High; init.TIM_OCNIdleState = TIM_OCIdleState_Reset; } else { init.TIM_OutputState = TIM_OutputState_Enable; init.TIM_OutputNState = TIM_OutputNState_Disable; - init.TIM_OCPolarity = TIM_OCPolarity_Low; + init.TIM_OCPolarity = inverted ? TIM_OCPolarity_Low : TIM_OCPolarity_High; init.TIM_OCIdleState = TIM_OCIdleState_Set; } switch (port->tch->timHw->channelIndex) { @@ -534,6 +585,29 @@ static void pwmDshotSetDirectionOutput(pwmOutputPort_t *port) static void pwmDshotSetDirectionInput(pwmOutputPort_t *port) { #if defined(USE_HAL_DRIVER) + // Betaflight ordering: reset the stream first (DeInit also clears all five event + // flags), then reconfigure the timer channel for capture, then rebuild the stream + // for the capture direction and arm it. + const uint32_t streamLL = dshotDmaStream(port); + DMA_TypeDef *dmaBase = port->tch->dma->dma; + LL_DMA_DisableStream(dmaBase, streamLL); + while (LL_DMA_IsEnabledStream(dmaBase, streamLL)) { } + LL_DMA_DeInit(dmaBase, streamLL); + + // Widen ARR so the free-running counter doesn't wrap every DSHOT bit period + // (20 ticks) while timing GCR edges, which span ~21 bits per telemetry frame. + SET_BIT(port->tch->timHw->tim->CR1, TIM_CR1_ARPE); + port->tch->timHw->tim->ARR = 0xffff; + +#if defined(STM32H7) + // H7 errata workaround (matches Betaflight): reconfiguring the channel from output + // compare to input capture can glitch the pin for a cycle while CCMR/CCER are mid-update. + // Disconnect the pin from the timer first so the ESC never sees the glitch, then + // reconnect it once the IC channel is safely configured. + const IO_t io = IOGetByTag(port->tch->timHw->tag); + IOConfigGPIO(io, IOCFG_OUT_PP); +#endif + TIM_IC_InitTypeDef init = {0}; init.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE; init.ICSelection = TIM_ICSELECTION_DIRECTTI; @@ -542,12 +616,14 @@ static void pwmDshotSetDirectionInput(pwmOutputPort_t *port) HAL_TIM_IC_ConfigChannel(port->tch->timCtx->timHandle, &init, dshotTimChannel(port)); HAL_TIM_IC_Start(port->tch->timCtx->timHandle, dshotTimChannel(port)); - const uint32_t streamLL = dshotDmaStream(port); - DMA_TypeDef *dmaBase = port->tch->dma->dma; - LL_DMA_DisableStream(dmaBase, streamLL); - while (LL_DMA_IsEnabledStream(dmaBase, streamLL)) { } - LL_DMA_ConfigAddresses(dmaBase, streamLL, (uint32_t)port->ccr, (uint32_t)port->dmaBuffer, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - LL_DMA_SetDataLength(dmaBase, streamLL, GCR_TELEMETRY_INPUT_LEN); +#if defined(STM32H7) + const bool baseInverted = port->tch->timHw->output & TIMER_OUTPUT_INVERTED; + IOConfigGPIOAF(io, baseInverted ? IOCFG_AF_PP_PD : IOCFG_AF_PP_UP, port->tch->timHw->alternateFunction); +#endif + + LL_DMA_InitTypeDef dmaInit; + dshotDmaInit(port, &dmaInit, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_Init(dmaBase, streamLL, &dmaInit); LL_DMA_EnableIT_TC(dmaBase, streamLL); LL_DMA_EnableStream(dmaBase, streamLL); LL_TIM_EnableDMAReq_CCx(port->tch->timHw->tim, dshotDmaSource(port)); @@ -655,6 +731,13 @@ static bool NOINLINE pwmDshotDecodeTelemetry(void) #endif if (edges > MIN_GCR_EDGES) { +#if defined(STM32H7) + // port->dmaBuffer lives in the cacheable DMA_RAM region (write-through on the + // write side, but reads still need an explicit invalidate) - without this the + // CPU can read stale data instead of what the capture DMA just wrote. + uint32_t alignedAddr = (uint32_t)port->dmaBuffer & ~0x1F; + SCB_InvalidateDCache_by_Addr((uint32_t *)alignedAddr, edges * sizeof(port->dmaBuffer[0]) + ((uint32_t)port->dmaBuffer - alignedAddr)); +#endif const uint16_t rawValue = dshotDecodeTelemetryPacket((const uint32_t *)port->dmaBuffer, edges); const uint16_t processed = dshotProcessPacket(rawValue, motorIndex); @@ -687,7 +770,18 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, } if (enableOutput && useDshotTelemetry) { - IOConfigGPIOAF(IOGetByTag(timerHardware->tag), IOCFG_AF_PP_UP, timerHardware->alternateFunction); + // The idle/listen level always needs a pull matching this channel's *un-flipped* + // polarity (pwmDshotSetDirectionOutput flips OCPolarity for bidir, but the GCR + // listen phase still idles toward the same physical level either way). + const IO_t io = IOGetByTag(timerHardware->tag); + const bool baseInverted = timerHardware->output & TIMER_OUTPUT_INVERTED; +#if defined(STM32H7) + // Preset ODR to the bidir idle level once - the H7 OC->IC erratum workaround + // in pwmDshotSetDirectionInput() disconnects the pin from the timer and relies + // on this cached ODR bit (matches Betaflight) instead of redriving the level. + baseInverted ? IOLo(io) : IOHi(io); +#endif + IOConfigGPIOAF(io, baseInverted ? IOCFG_AF_PP_PD : IOCFG_AF_PP_UP, timerHardware->alternateFunction); } // Configure timer DMA From abc7374b1aa2933451b3b2281e8fe3b682ed2709 Mon Sep 17 00:00:00 2001 From: Error414 Date: Sat, 5 Sep 2026 12:27:33 +0200 Subject: [PATCH 8/9] BidirDshot - hold the line to avoid double beeping durring FC bootup --- src/main/drivers/pwm_output.c | 56 ++++++++++++++++++++++++++++------- 1 file changed, 45 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 3afdce20079..37a90d757f5 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -667,6 +667,34 @@ static void pwmDshotSetDirectionInput(pwmOutputPort_t *port) port->telemetryInputActive = true; } +// Set when bidir pins are held low as plain GPIO until the first frame +static bool dshotPinsParkedLow = false; + +// Connect bidir DSHOT pins to their timer AF (idle-high). Called on the first +// real motor update so the line only goes high microseconds before frames start, +// keeping it low through the ESC's boot-time bootloader-entry window. +static void dshotConnectOutputs(void) +{ + for (int index = 0; index < getMotorCount(); index++) { + pwmOutputPort_t *port = motors[index].pwmPort; + if (port && port->configured) { + const timerHardware_t *timHw = port->tch->timHw; + const IO_t io = IOGetByTag(timHw->tag); + // The pull must match this channel's *un-flipped* polarity + // (pwmDshotSetDirectionOutput flips OCPolarity for bidir, but the GCR + // listen phase still idles toward the same physical level either way). + const bool baseInverted = timHw->output & TIMER_OUTPUT_INVERTED; +#if defined(STM32H7) + // Preset ODR to the bidir idle level once - the H7 OC->IC erratum workaround + // in pwmDshotSetDirectionInput() disconnects the pin from the timer and relies + // on this cached ODR bit (matches Betaflight) instead of redriving the level. + baseInverted ? IOLo(io) : IOHi(io); +#endif + IOConfigGPIOAF(io, baseInverted ? IOCFG_AF_PP_PD : IOCFG_AF_PP_UP, timHw->alternateFunction); + } + } +} + static void pwmDshotDmaIrqHandler(DMA_t descriptor) { if (!DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { @@ -770,18 +798,19 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, } if (enableOutput && useDshotTelemetry) { - // The idle/listen level always needs a pull matching this channel's *un-flipped* - // polarity (pwmDshotSetDirectionOutput flips OCPolarity for bidir, but the GCR - // listen phase still idles toward the same physical level either way). + // Bidir signalling idles high, but holding the line high with no edges + // during ESC boot triggers the BLHeli/Bluejay bootloader-entry check + // (~150ms of continuous high right after the ESC's startup melody). The + // first INAV frame goes out seconds after this pin is configured, so the + // ESC would enter its bootloader, time out and reset - replaying the + // startup melody. Park the pin so the ESC sees low, like normal DSHOT + // idle, and defer the AF connect until the first frame goes out + // (dshotConnectOutputs()). On TIMER_OUTPUT_INVERTED hardware the pin + // level is re-inverted downstream, so park high there. const IO_t io = IOGetByTag(timerHardware->tag); - const bool baseInverted = timerHardware->output & TIMER_OUTPUT_INVERTED; -#if defined(STM32H7) - // Preset ODR to the bidir idle level once - the H7 OC->IC erratum workaround - // in pwmDshotSetDirectionInput() disconnects the pin from the timer and relies - // on this cached ODR bit (matches Betaflight) instead of redriving the level. - baseInverted ? IOLo(io) : IOHi(io); -#endif - IOConfigGPIOAF(io, baseInverted ? IOCFG_AF_PP_PD : IOCFG_AF_PP_UP, timerHardware->alternateFunction); + IOConfigGPIO(io, IOCFG_OUT_PP); + (timerHardware->output & TIMER_OUTPUT_INVERTED) ? IOHi(io) : IOLo(io); + dshotPinsParkedLow = true; } // Configure timer DMA @@ -997,6 +1026,11 @@ void pwmCompleteMotorUpdate(void) { #ifdef USE_DSHOT if (isMotorProtocolDshot()) { + if (dshotPinsParkedLow) { + dshotConnectOutputs(); + dshotPinsParkedLow = false; + } + if (!executeDShotCommands()) { return; } From 449945555ee1390d4482bcf4c9740634c4f50abd Mon Sep 17 00:00:00 2001 From: Error414 Date: Tue, 8 Sep 2026 21:14:39 +0200 Subject: [PATCH 9/9] BidirDshot - skip circular DMA keepalive during EEPROM writes pwmSetMotorDMACircular() predates bidirectional DSHOT and corrupts its per-channel direction-switching state machine when a config save runs (e.g. servo autotrim saving on disarm): - On USE_DSHOT_DMAR targets the burst path is never armed with bidir, so dmaBurstBuffer stays NULL and loadDmaBufferDshotStride() writes the zero-throttle frame through the NULL pointer into 0x00000000 - which is ITCM RAM holding .tcm_code (FAST_CODE), corrupting the firmware. - On per-channel targets impl_timerPWMSetDMACircular() re-enables the stream without regard to the current transfer direction, and on exit disables the TIM DMA request before the stream: a FIFO-enabled mem-to-periph stream then cannot drain, EN sticks at 1 and every later reconfiguration is silently ignored - the output is dead until reboot. - Circular output also repeats frames back-to-back, colliding with the GCR reply the ESC drives onto the same line after every frame. Match Betaflight instead: with bidir telemetry active, leave motor DMA alone during the flash write and accept the frame gap. Saves only happen disarmed, the ESC holds zero throttle, and the existing decode path rebuilds the streams (full DeInit/Init) on the first update afterwards. Co-Authored-By: Claude Fable 5 Claude-Session: https://claude.ai/code/session_01BB55zdRNZq3Q6hZYsDYwTc --- src/main/drivers/pwm_output.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 37a90d757f5..c7d19f2c1dd 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -260,6 +260,15 @@ void pwmSetMotorDMACircular(bool circular) return; } + // Bidirectional DSHOT uses per-channel DMA with output/input direction + // switching: the burst path is never armed (dmaBurstBuffer stays NULL on + // USE_DSHOT_DMAR targets) and a port may currently be in the input-capture + // direction. Skip the circular keepalive and accept the frame gap during + // the flash write, matching Betaflight behaviour. + if (useDshotTelemetry) { + return; + } + int motorCount = getMotorCount(); if (circular) {