diff --git a/docs/Blackbox.md b/docs/Blackbox.md index 55b0f19c103..400778c5075 100644 --- a/docs/Blackbox.md +++ b/docs/Blackbox.md @@ -194,6 +194,8 @@ Available debug modes include: - `GPS` - GPS debugging - `ALTITUDE` - Altitude estimation debugging - `MAG` - Raw, uncalibrated magnetometer samples (useful for external compass calibration) +- `RPM_FILTER` - Per-motor RPM as seen by the RPM filter, motors 1-8 in `debug[0]`-`debug[7]` (needs the RPM filter enabled, so bidirectional DShot or ESC telemetry) +- `DYN_IDLE` - Dynamic idle (`dynamic_idle_min_rpm`): P, I and D in `debug[0]`-`debug[2]` (0.0001 of the motor range), the slowest motor's frequency in `debug[3]` (0.1 Hz) - And 20+ other modes for specific subsystems To use debug mode logging: diff --git a/docs/Settings.md b/docs/Settings.md index a0bdc187935..1118e1c6809 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -803,6 +803,8 @@ Defines debug values exposed in debug variables (developer / debugging setting) | ESC | | | FW_TURN | | | MAG | | +| RPM_FILTER | | +| DYN_IDLE | | --- @@ -944,6 +946,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 (temperature, voltage, current in addition to eRPM). The ESC is asked for it on every arm, so the values appear after the first arm + +| 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. @@ -1029,6 +1051,56 @@ Q factor for dynamic notches --- +### dynamic_idle_d_gain + +D gain of `dynamic_idle_min_rpm`, on how fast the slowest motor slows down + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 250 | + +--- + +### dynamic_idle_i_gain + +I gain of `dynamic_idle_min_rpm`: it rises fast and falls slowly + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 1 | 250 | + +--- + +### dynamic_idle_max_increase + +The most `dynamic_idle_min_rpm` may raise the motors' low end, in tenths of a percent of the motor range: 150 is 15 % + +| Default | Min | Max | +| --- | --- | --- | +| 150 | 10 | 255 | + +--- + +### dynamic_idle_min_rpm + +Multirotor only, ported from Betaflight, needs `dshot_bidir_enabled`. The slowest speed the motors are kept at once the throttle has been raised [RPM]; the low end of the motor output then moves with it, starting from the lowest DShot value instead of `throttle_idle`. Until the throttle has passed `airmode_throttle_threshold` once after arming the motors go no higher than `throttle_idle`. Betaflight's `dyn_idle_min_rpm` is in hundreds of RPM: 35 there is 3500 here. It starts once at least 80% of every motor's replies decode and falls back to `throttle_idle` below 50%. 0 disables it. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 20000 | + +--- + +### dynamic_idle_p_gain + +P gain of `dynamic_idle_min_rpm`, on how far the slowest motor is below it + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 1 | 250 | + +--- + ### enable_broken_o4_workaround DJI O4 release firmware has a broken MSP DisplayPort implementation. This enables a workaround to restore ARM detection. diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index ec6054fb51b..4c5fcf89b5f 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -4180,7 +4180,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Fixed 8-byte reply carrying one 64-bit mask as two `uint32_t` halves, low half first. Only the first `MIN(MAX_LOGIC_CONDITIONS, 64)` bits are evaluated. A condition counts as configured when any of `enabled`, `activatorId` (default -1), `operation`, `operandA.type`, `operandA.value`, `operandB.type`, `operandB.value` or `flags` differs from its default. ## `MSP2_INAV_ESC_RPM (8256 / 0x2040)` -**Description:** Retrieves the RPM reported by each ESC via telemetry. +**Description:** Retrieves the RPM and telemetry link quality reported by each ESC. **Request Payload:** **None** @@ -4188,8 +4188,10 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: |Field|C Type|Size (Bytes)|Units|Description| |---|---|---|---|---| | `escRpm` | `uint32_t` | 4 | RPM | RPM reported by the ESC | +| `frameSuccess` | `uint8_t` | 1 | % | Share of telemetry replies decoded in the last closed window. 255 when no reply was expected in that window | +| `frameTotal` | `uint16_t` | 2 | frames | Telemetry replies expected in the last closed window, missing ones included | -**Notes:** Requires `USE_ESC_SENSOR`. Payload size depends on the number of detected motors with telemetry. +**Notes:** Requires `USE_ESC_SENSOR` or `USE_DSHOT_BIDIR`. 7 bytes per motor, `getMotorCount()` entries. Source follows the RPM filter: bidirectional DSHOT when `isDshotTelemetryActive()`, serial ESC telemetry otherwise. The frame statistics come from fixed back-to-back windows of `ESC_FRAME_WINDOW_MS` (1 s), so they change once per window. A reply counts as missing when the ESC stays silent, fails CRC/GCR decoding or (serial) times out. Sources without frame counting (SRXL2) always report 255 / 0. ## `MSP2_INAV_ESC_TELEM (8257 / 0x2041)` **Description:** Retrieves the full telemetry data structure reported by each ESC. diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 50d6d0b27d0..0f69ef03788 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9847,13 +9847,25 @@ "ctype": "uint32_t", "desc": "RPM reported by the ESC", "units": "RPM" + }, + { + "name": "frameSuccess", + "ctype": "uint8_t", + "desc": "Share of telemetry replies decoded in the last closed window. 255 when no reply was expected in that window", + "units": "%" + }, + { + "name": "frameTotal", + "ctype": "uint16_t", + "desc": "Telemetry replies expected in the last closed window, missing ones included", + "units": "frames" } ], "repeating": "getMotorCount()" }, "variable_len": "getMotorCount()", - "notes": "Requires `USE_ESC_SENSOR`. Payload size depends on the number of detected motors with telemetry.", - "description": "Retrieves the RPM reported by each ESC via telemetry." + "notes": "Requires `USE_ESC_SENSOR` or `USE_DSHOT_BIDIR`. 7 bytes per motor, `getMotorCount()` entries. Source follows the RPM filter: bidirectional DSHOT when `isDshotTelemetryActive()`, serial ESC telemetry otherwise. The frame statistics come from fixed back-to-back windows of `ESC_FRAME_WINDOW_MS` (1 s), so they change once per window. A reply counts as missing when the ESC stays silent, fails CRC/GCR decoding or (serial) times out. Sources without frame counting (SRXL2) always report 255 / 0.", + "description": "Retrieves the RPM and telemetry link quality reported by each ESC." }, "MSP2_INAV_ESC_TELEM": { "code": 8257, diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 1cfd028c3f9..d637af2d53f 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -252,6 +252,8 @@ main_sources(COMMON_SRC drivers/pwm_esc_detect.h drivers/pwm_mapping.c drivers/pwm_mapping.h + drivers/bidir_dshot.c + drivers/bidir_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 fc6b8c11d3b..d6a05751965 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1573,8 +1573,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 #ifdef USE_DRONECAN @@ -1600,7 +1602,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/build/debug.h b/src/main/build/debug.h index 23caa6b6fa7..b92870fb038 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -87,6 +87,8 @@ typedef enum { DEBUG_ESC, DEBUG_FW_TURN, DEBUG_MAG, + DEBUG_RPM_FILTER, + DEBUG_DYN_IDLE, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/drivers/bidir_dshot.c b/src/main/drivers/bidir_dshot.c new file mode 100644 index 00000000000..ac0d982bccd --- /dev/null +++ b/src/main/drivers/bidir_dshot.c @@ -0,0 +1,166 @@ +/* + * 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 "platform.h" +#include "drivers/bidir_dshot.h" + +// Bidirectional DSHOT telemetry only: eRPM / EDT decoding, RPM state for the RPM filter and +// the ESC sensor. Frame generation, DMA and DSHOT commands for plain DSHOT live in +// pwm_output.c under USE_DSHOT. Every user of this API is guarded by USE_DSHOT_BIDIR. +// +// Values are published as decoded, unfiltered: frames arrive at the DSHOT frame rate, which +// follows the scheduler's idle time rather than any fixed period, so smoothing belongs to the +// consumer that samples them at a known rate (rpmFilterUpdateTask()). + +#ifdef USE_DSHOT_BIDIR + +#include "flight/mixer.h" + +// Only eRPM, temperature, voltage and current are kept; the EDT debug and status frames +// have no consumer and just mark their type as seen +#define DSHOT_TELEMETRY_STORED_TYPE_COUNT (DSHOT_TELEMETRY_TYPE_CURRENT + 1) + +typedef struct { + uint16_t telemetryData[DSHOT_TELEMETRY_STORED_TYPE_COUNT]; + uint8_t telemetryTypes; // bit per dshotTelemetryType_e seen since boot +} dshotTelemetryMotorState_t; + +bool useDshotTelemetry = false; + +static dshotTelemetryMotorState_t motorTelemetry[MAX_SUPPORTED_MOTORS]; +static float erpmToHz; + +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(uint16_t value, uint8_t motorIndex, uint32_t *decoded, dshotTelemetryType_e *type) +{ + const bool edtEnabled = motorConfig()->useDshotEdt || (motorTelemetry[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 { + // EDT frame types 2, 4, ... 14 are the enum values 1..7 in order + *type = telemetryType >> 1; + *decoded = value & 0x00ff; + } +} + +bool isDshotTelemetryActive(void) +{ + return useDshotTelemetry; +} + +// useDshotTelemetry itself is set by the motor driver (pwmMotorPreconfigure()), which +// knows whether the outputs were configured for DSHOT at all +void initDshotTelemetry(void) +{ + if (!useDshotTelemetry) { + return; + } + + erpmToHz = ERPM_PER_LSB / 60.0f / (motorConfig()->motorPoleCount / 2.0f); +} + +uint16_t dshotProcessPacket(uint16_t rawValue, uint8_t motorIndex) +{ + if (!useDshotTelemetry || motorIndex >= MAX_SUPPORTED_MOTORS) { + return rawValue; + } + + // As in Betaflight, a window without a reply is not counted: an ESC may skip replies + // when busy, so only replies that were at least partly captured count + if (rawValue == DSHOT_TELEMETRY_NOEDGE) { + return rawValue; + } + + escFrameCounter_t *frames = escSensorFrameCounter(motorIndex); + frames->total++; + + if (rawValue == DSHOT_TELEMETRY_INVALID) { + return rawValue; + } + + dshotTelemetryType_e type; + uint32_t decoded; + dshotDecodeTelemetryValue(rawValue, motorIndex, &decoded, &type); + if (decoded == DSHOT_TELEMETRY_INVALID) { + return DSHOT_TELEMETRY_INVALID; + } + + frames->valid++; + + motorTelemetry[motorIndex].telemetryTypes |= (1 << type); + if (type < DSHOT_TELEMETRY_STORED_TYPE_COUNT) { + motorTelemetry[motorIndex].telemetryData[type] = decoded; + } + + return rawValue; +} + +float getMotorFrequencyHz(uint8_t motorIndex) +{ + return motorTelemetry[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_ERPM] * erpmToHz; +} + +float getDshotRpm(uint8_t motorIndex) +{ + return getMotorFrequencyHz(motorIndex) * 60.0f; +} + +bool getDshotEscSensorData(escSensorData_t *data, uint8_t motorIndex) +{ + if (!useDshotTelemetry || motorIndex >= MAX_SUPPORTED_MOTORS) { + return false; + } + + const dshotTelemetryMotorState_t *state = &motorTelemetry[motorIndex]; + if ((state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_ERPM)) == 0) { + return false; + } + + // escSensorData_t units: rpm (mechanical), degrees C, 0.01 V, 0.01 A. EDT reports + // the temperature in degrees C, the voltage in 0.25 V and the current in 1 A steps. + data->rpm = lrintf(getDshotRpm(motorIndex)); + 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] * 25 : 0; + data->current = (state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) ? state->telemetryData[DSHOT_TELEMETRY_TYPE_CURRENT] * 100 : 0; + + return true; +} + +#endif diff --git a/src/main/drivers/bidir_dshot.h b/src/main/drivers/bidir_dshot.h new file mode 100644 index 00000000000..ac37fde5cf5 --- /dev/null +++ b/src/main/drivers/bidir_dshot.h @@ -0,0 +1,65 @@ +/* + * 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" // timeUs_t for sensors/esc_sensor.h, which has no includes of its own +#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) + +#ifdef USE_DSHOT_BIDIR +extern bool useDshotTelemetry; + +void initDshotTelemetry(void); +bool isDshotTelemetryActive(void); +uint16_t dshotProcessPacket(uint16_t rawValue, uint8_t motorIndex); +float getDshotRpm(uint8_t motorIndex); +// Mechanical frequency from the last decoded eRPM frame, unfiltered (see bidir_dshot.c) +float getMotorFrequencyHz(uint8_t motorIndex); +bool getDshotEscSensorData(escSensorData_t *data, uint8_t motorIndex); +#endif diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index 10ece999253..3a866f6eae8 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -100,8 +100,8 @@ typedef void (*dmaCallbackHandlerFuncPtr)(DMA_t channelDescriptor); dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\ } -#define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) d->dma->HIFCR = (flag << (d->flagsShift - 32)); else d->dma->LIFCR = (flag << d->flagsShift) -#define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & (flag << (d->flagsShift - 32)): d->dma->LISR & (flag << d->flagsShift)) +#define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) d->dma->HIFCR = ((flag) << (d->flagsShift - 32)); else d->dma->LIFCR = ((flag) << d->flagsShift) +#define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & ((flag) << (d->flagsShift - 32)): d->dma->LISR & ((flag) << d->flagsShift)) @@ -129,8 +129,8 @@ DMA_t dmaGetByRef(const DMA_Stream_TypeDef * ref); } //Same as dma_flag_clear/dma_flag_get -#define DMA_CLEAR_FLAG(d, flag) d->dma->clr = (flag << d->flagsShift) -#define DMA_GET_FLAG_STATUS(d, flag) (d->dma->sts & (flag << d->flagsShift)) +#define DMA_CLEAR_FLAG(d, flag) d->dma->clr = ((flag) << d->flagsShift) +#define DMA_GET_FLAG_STATUS(d, flag) (d->dma->sts & ((flag) << d->flagsShift)) DMA_t dmaGetByRef(const dma_channel_type * ref); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 20aed6c7264..3ffc8f45a1d 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -26,6 +26,7 @@ #include "build/atomic.h" #include "build/debug.h" +#include "build/build_config.h" #include "common/log.h" #include "common/maths.h" @@ -33,6 +34,7 @@ #include "drivers/io.h" #include "drivers/nvic.h" +#include "drivers/bidir_dshot.h" #include "drivers/time.h" #include "drivers/timer.h" #include "drivers/pwm_mapping.h" @@ -70,6 +72,16 @@ #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ #define MAX_DMA_TIMERS 8 +#ifdef USE_DSHOT_BIDIR +#define DSHOT_TELEMETRY_DEADTIME_US 35 +#define GCR_TELEMETRY_INPUT_LEN MAX_GCR_EDGES +/* A bidir port captures the GCR edges of the ESC reply into the same buffer it sends the + * frame from, so the buffer is sized for the larger of the two */ +#define DSHOT_PORT_DMA_BUFFER_SIZE GCR_TELEMETRY_INPUT_LEN +#else +#define DSHOT_PORT_DMA_BUFFER_SIZE DSHOT_DMA_BUFFER_SIZE +#endif + /* Keep-alive frame replayed by circular DMA while the CPU is stalled by a flash write: * 16 data bits followed by an idle (line low) gap of DSHOT_KEEPALIVE_GAP_US. * One DMA slot is one DSHOT bit period, so the gap needs gapUs * dshotHz / bitLength slots. */ @@ -115,10 +127,13 @@ typedef struct { #ifdef USE_DSHOT // DSHOT parameters - timerDMASafeType_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; + timerDMASafeType_t dmaBuffer[DSHOT_PORT_DMA_BUFFER_SIZE]; #ifdef USE_DSHOT_DMAR timerDMASafeType_t *dmaBurstBuffer; #endif +#ifdef USE_DSHOT_BIDIR + bool telemetryInputActive; +#endif #endif } pwmOutputPort_t; @@ -150,7 +165,17 @@ static timeUs_t digitalMotorUpdateIntervalUs = 0; static timeUs_t digitalMotorLastUpdateUs; static timeUs_t lastCommandSent = 0; static timeUs_t commandPostDelay = 0; - +#ifdef USE_DSHOT_BIDIR +static bool dshotTelemetryPending = false; +/* One stamp for the whole frame, taken from the last port turned round for the reply + * (all ports switch within microseconds of each other). Turning a port back to output + * resets the counter and period of its timer, which every other port on that timer + * shares, so no port may switch back while a sibling could still be capturing: the + * deadtime is therefore waited out once, for all ports, before any of them is touched */ +static timeUs_t dshotTelemetryInputStampUs = 0; +static timeUs_t dshotTelemetryDeadtimeUs = 0; +#endif + static circularBuffer_t commandsCircularBuffer; static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE]; static currentExecutingCommand_t currentExecutingCommand; @@ -167,6 +192,19 @@ static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS]; uint8_t burstDmaTimersCount = 0; #endif + +#ifdef USE_DSHOT_BIDIR +// 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); +// Set while bidir pins are plain GPIO, disconnected from their timer: parked low until +// the first frame, or held by the pull only during a flash write. The next frame +// reconnects them (dshotConnectOutputs()). +static bool dshotPinsDisconnected = false; +#endif #endif static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) @@ -295,11 +333,39 @@ void pwmSetMotorDMACircular(bool circular) int motorCount = getMotorCount(); const uint32_t dshotHz = getDshotHz(initMotorProtocol); const uint32_t keepaliveSlots = DSHOT_KEEPALIVE_SLOTS(dshotHz); + // A frame started by pwmCompleteMotorUpdate() may still be in flight for this long + const uint32_t frameUs = DSHOT_DMA_BUFFER_SIZE * DSHOT_MOTOR_BITLENGTH * 1000000UL / dshotHz; + +#ifdef USE_DSHOT_BIDIR + // No keep-alive frames for bidir: the ESC sees a frame gap for the flash write, as in + // Betaflight, with the line held at its idle level only by the pull. + if (useDshotTelemetry) { + if (circular && !dshotPinsDisconnected) { + // Complete the DSHOT exchange before the write: let a frame in flight finish, + // with the deadtime as margin for its completion IRQ, then wait out and decode + // the reply, which turns every port back to output + delayMicroseconds(frameUs + DSHOT_TELEMETRY_DEADTIME_US); + while (!pwmDshotDecodeTelemetry()) { } + + // Disconnect the pins from the timers so nothing drives the line during the + // write; the next frame after it reconnects them + for (int i = 0; i < motorCount; i++) { + const pwmOutputPort_t *port = motors[i].pwmPort; + if (port && port->configured) { + const bool baseInverted = port->tch->timHw->output & TIMER_OUTPUT_INVERTED; + IOConfigGPIO(IOGetByTag(port->tch->timHw->tag), baseInverted ? IOCFG_IPD : IOCFG_IPU); + } + } + dshotPinsDisconnected = true; + } + return; + } +#endif if (circular) { // A frame started by pwmCompleteMotorUpdate() may still be in flight: let it finish // and keep the line low for one full gap before the keep-alive stream starts - delayMicroseconds(DSHOT_DMA_BUFFER_SIZE * DSHOT_MOTOR_BITLENGTH * 1000000UL / dshotHz + DSHOT_KEEPALIVE_GAP_US); + delayMicroseconds(frameUs + DSHOT_KEEPALIVE_GAP_US); // Load a zero-throttle packet into the shared keep-alive buffer. The padding slots // must be zero (line low between frames); DMA_RAM is NOLOAD and not cleared at @@ -420,6 +486,473 @@ static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer) } #endif +#ifdef USE_DSHOT_BIDIR +// The CC1..CC4 DMA request bits sit next to each other in DIER on every supported MCU +static uint32_t dshotDmaSource(const pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) || !defined(AT32F43x) + STATIC_ASSERT(TIM_DMA_CC2 == TIM_DMA_CC1 << 1 && TIM_DMA_CC4 == TIM_DMA_CC1 << 3, tim_dma_cc_bits_adjacent); + return TIM_DMA_CC1 << port->tch->timHw->channelIndex; +#else + STATIC_ASSERT(TMR_C2_DMA_REQUEST == TMR_C1_DMA_REQUEST << 1 && TMR_C4_DMA_REQUEST == TMR_C1_DMA_REQUEST << 3, tmr_dma_request_bits_adjacent); + return TMR_C1_DMA_REQUEST << port->tch->timHw->channelIndex; +#endif +} + +#if defined(USE_HAL_DRIVER) +static uint32_t dshotDmaStream(const pwmOutputPort_t *port) +{ + STATIC_ASSERT(LL_DMA_STREAM_0 == 0 && LL_DMA_STREAM_7 == 7, ll_dma_stream_is_index); + return 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 + +// The channel identifiers are evenly spaced, so channelIndex maps onto them linearly +static uint32_t dshotTimChannel(const pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) + STATIC_ASSERT(TIM_CHANNEL_4 == TIM_CHANNEL_1 + 3 * (TIM_CHANNEL_2 - TIM_CHANNEL_1), tim_channel_ids_linear); + return TIM_CHANNEL_1 + port->tch->timHw->channelIndex * (TIM_CHANNEL_2 - TIM_CHANNEL_1); +#elif defined(AT32F43x) + STATIC_ASSERT(TMR_SELECT_CHANNEL_4 == TMR_SELECT_CHANNEL_1 + 3 * (TMR_SELECT_CHANNEL_2 - TMR_SELECT_CHANNEL_1), tmr_channel_ids_linear); + return TMR_SELECT_CHANNEL_1 + port->tch->timHw->channelIndex * (TMR_SELECT_CHANNEL_2 - TMR_SELECT_CHANNEL_1); +#else + STATIC_ASSERT(TIM_Channel_4 == TIM_Channel_1 + 3 * (TIM_Channel_2 - TIM_Channel_1), tim_channel_ids_linear); + return TIM_Channel_1 + port->tch->timHw->channelIndex * (TIM_Channel_2 - TIM_Channel_1); +#endif +} + +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; + if (len < 1 || len > 21) { + // Edges closer than half a GCR bit or further apart than a whole frame + return DSHOT_TELEMETRY_INVALID; + } + } else { + len = 21 - bits; + if (len < 1) { + // All bits accounted for, no trailing run left to infer + break; + } + } + + 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; +} + +#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 = inverted ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH; + init.OCNIdleState = TIM_OCNIDLESTATE_SET; + 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)); + 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_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) + // Telemetry input capture widens the period to 0xffff (see pwmDshotSetDirectionInput); + // restore the DSHOT bit period now, bypassing the period buffer so it takes effect + // immediately, and reset the counter so a stale count above the new period doesn't + // have to run all the way round (TMR2/TMR5 are 32-bit) before the first compare. + tmr_period_buffer_enable(port->tch->timHw->tim, FALSE); + tmr_period_value_set(port->tch->timHw->tim, DSHOT_MOTOR_BITLENGTH - 1); + tmr_period_buffer_enable(port->tch->timHw->tim, TRUE); + tmr_counter_value_set(port->tch->timHw->tim, 0); + + 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 = 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 = 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); + 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 = 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 = inverted ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + 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) + // 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; + 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)); + +#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)); +#elif defined(AT32F43x) + // Widen the period 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. + tmr_period_buffer_enable(port->tch->timHw->tim, TRUE); + tmr_period_value_set(port->tch->timHw->tim, 0xffff); + + 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->telemetryInputActive = true; +} + +// 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); + } + } +} + +// Takes over the stream's completion interrupt from the timer driver on bidir ports: once +// the frame is out the port is turned round to capture the ESC reply, and +// pwmDshotDecodeTelemetry() turns it back before the next frame +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 + + // Same bookkeeping as the timer driver's handler (timerPWMDMAInProgress()) + if (port->tch->dmaState == TCH_DMA_ACTIVE) { + port->tch->dmaState = TCH_DMA_IDLE; + } + + if (!port->telemetryInputActive) { + pwmDshotSetDirectionInput(port); + dshotTelemetryInputStampUs = micros(); + dshotTelemetryPending = true; + } + + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); +} + +// 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 (!dshotTelemetryPending) { + return true; + } + + // Wait for the last reply of the frame before switching any port back (see + // dshotTelemetryInputStampUs) + if ((micros() - dshotTelemetryInputStampUs) < dshotTelemetryDeadtimeUs) { + return false; + } + + // Cleared before the loop: a completion IRQ landing while the loop runs re-arms it, so a + // port the loop has already passed is picked up by the next call instead of being left + // in input for the following frame + dshotTelemetryPending = false; + + for (int motorIndex = 0; motorIndex < getMotorCount(); motorIndex++) { + pwmOutputPort_t *port = motors[motorIndex].pwmPort; + if (!port || !port->configured || !port->telemetryInputActive) { + continue; + } + + 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 + + // Too few edges is no reply at all + uint16_t rawValue = DSHOT_TELEMETRY_NOEDGE; + if (edges > MIN_GCR_EDGES) { + // No cache maintenance: pwmOutputPorts is DMA_RAM, which the H7 MPU maps non-cacheable + 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 + // Nothing to publish before the first eRPM value (an EDT frame may come first) + escSensorData_t data; + if (getDshotEscSensorData(&data, motorIndex)) { + escSensorSetDshotData(motorIndex, data.rpm, data.temperature, data.voltage, data.current); + } +#endif + } + + pwmDshotSetDirectionOutput(port); + } + + // Re-armed during the loop: some port is in input again, hold the frame until it is back + return !dshotTelemetryPending; +} +#endif // USE_DSHOT_BIDIR + static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput) { // Try allocating new port @@ -429,9 +962,26 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, return NULL; } +#ifdef USE_DSHOT_BIDIR + if (enableOutput && useDshotTelemetry) { + // 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); + IOConfigGPIO(io, IOCFG_OUT_PP); + (timerHardware->output & TIMER_OUTPUT_INVERTED) ? IOHi(io) : IOLo(io); + dshotPinsDisconnected = true; + } +#endif + // 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; @@ -449,6 +999,15 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, // Only mark as DSHOT channel if DMA was set successfully ZERO_FARRAY(port->dmaBuffer); port->configured = true; +#ifdef USE_DSHOT_BIDIR + if (useDshotTelemetry) { + // Inverted signalling and per-frame direction switching; the port takes the + // stream's completion interrupt over from the timer driver + 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 } #endif @@ -473,11 +1032,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; } + // Frame reset slots: rewritten every time, a bidir port captures edges into this buffer + dmaBuffer[16] = 0; + dmaBuffer[17] = 0; } #endif static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) { + // The telemetry bit asks for a serial telemetry packet on the ESC's telemetry wire; + // the eRPM reply on the motor line is triggered by the inverted checksum alone, so + // bidir leaves the bit to the serial ESC sensor's round-robin requests uint16_t packet = (value << 1) | (requestTelemetry ? 1 : 0); // compute checksum @@ -487,6 +1052,11 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) csum ^= csum_data; // xor data by nibbles csum_data >>= 4; } +#ifdef USE_DSHOT_BIDIR + if (useDshotTelemetry) { + csum = ~csum; + } +#endif csum &= 0xf; // append checksum @@ -551,6 +1121,7 @@ static int getDShotCommandRepeats(dshotCommands_e cmd) { int repeats = 1; switch (cmd) { + case DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE: case DSHOT_CMD_SPIN_DIRECTION_NORMAL: case DSHOT_CMD_SPIN_DIRECTION_REVERSED: repeats = 10; @@ -562,7 +1133,8 @@ static int getDShotCommandRepeats(dshotCommands_e cmd) { return repeats; } -static bool executeDShotCommands(void){ +// NOINLINE: pwmCompleteMotorUpdate() is inlined into the ITCM scheduler; commands are rare +static bool NOINLINE executeDShotCommands(void){ timeUs_t tNow = micros(); @@ -606,9 +1178,21 @@ void pwmCompleteMotorUpdate(void) { return; } +#ifdef USE_DSHOT_BIDIR + if (useDshotTelemetry && !pwmDshotDecodeTelemetry()) { + return; + } +#endif + int motorCount = getMotorCount(); timeUs_t currentTimeUs = micros(); +#ifdef USE_DSHOT_BIDIR + if (useDshotTelemetry) { + escSensorFrameWindowUpdate(currentTimeUs / 1000); + } +#endif + // Enforce motor update rate if ((digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) { return; @@ -619,6 +1203,13 @@ void pwmCompleteMotorUpdate(void) { #ifdef USE_DSHOT if (isMotorProtocolDshot()) { +#ifdef USE_DSHOT_BIDIR + if (dshotPinsDisconnected) { + dshotConnectOutputs(); + dshotPinsDisconnected = false; + } +#endif + if (!executeDShotCommands()) { return; } @@ -672,6 +1263,9 @@ void pwmMotorPreconfigure(void) { // Keep track of initial motor protocol initMotorProtocol = motorConfig()->motorPwmProtocol; +#ifdef USE_DSHOT_BIDIR + useDshotTelemetry = motorConfig()->useDshotTelemetry && getMotorProtocolProperties(initMotorProtocol)->isDSHOT; +#endif #ifdef BRUSHED_MOTORS initMotorProtocol = PWM_TYPE_BRUSHED; // Override proto diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index afe1a1916e3..fec7d8cd0d0 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -27,6 +27,7 @@ #define MAX_PWM_OUTPUTS (MAX_PWM_OUTPUT_PORTS) #endif typedef enum { + DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE = 13, DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20, DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21, } dshotCommands_e; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 15339c4f39e..ad592f0add9 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -238,7 +238,9 @@ static const char *debugModeNames[DEBUG_COUNT] = { "TERRAIN_NAV", "ESC", "FW_TURN", - "MAG" + "MAG", + "RPM_FILTER", + "DYN_IDLE" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 778dce2e511..a79c29b5555 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -37,6 +37,7 @@ #include "drivers/time.h" #include "drivers/system.h" #include "drivers/pwm_output.h" +#include "drivers/bidir_dshot.h" #include "drivers/pwm_mapping.h" #include "sensors/sensors.h" @@ -593,6 +594,17 @@ bool emergInflightRearmEnabled(void) return false; // craft doesn't appear to be flying, don't allow emergency rearm } +#ifdef USE_DSHOT_BIDIR +// As in Betaflight: the ESC sends EDT frames only after being asked and forgets it on power-up, +// so ask on every arm, while the motors are still stopped and the ESC accepts commands +static void requestDshotExtendedTelemetry(void) +{ + if (isDshotTelemetryActive() && motorConfig()->useDshotEdt) { + sendDShotCommand(DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE); + } +} +#endif + void tryArm(void) { updateArmingStatus(); @@ -608,6 +620,9 @@ void tryArm(void) const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE); #endif if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) { +#ifdef USE_DSHOT_BIDIR + requestDshotExtendedTelemetry(); +#endif sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED); ENABLE_ARMING_FLAG(ARMED); ENABLE_FLIGHT_MODE(TURTLE_MODE); @@ -633,6 +648,9 @@ void tryArm(void) lastDisarmReason = DISARM_NONE; +#ifdef USE_DSHOT_BIDIR + requestDshotExtendedTelemetry(); +#endif ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 9ba5a8e442c..b83297e9744 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/bidir_dshot.h" #include "drivers/exti.h" #include "drivers/io.h" #include "drivers/flash.h" @@ -765,6 +766,9 @@ void init(void) #ifdef USE_DSHOT initDShotCommands(); #endif +#ifdef USE_DSHOT_BIDIR + initDshotTelemetry(); +#endif #ifdef USE_SERIAL_GIMBAL gimbalCommonInit(); @@ -797,7 +801,11 @@ void init(void) #ifdef USE_RPM_FILTER disableRpmFilters(); - if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) { + bool rpmSourceActive = STATE(ESC_SENSOR_ENABLED); +#ifdef USE_DSHOT_BIDIR + rpmSourceActive = rpmSourceActive || isDshotTelemetryActive(); +#endif + if (rpmSourceActive && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) { rpmFiltersInit(); setTaskEnabled(TASK_RPM_FILTER, true); } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 3bf520b977a..efa95e668c1 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -47,6 +47,7 @@ #include "drivers/compass/compass.h" #include "drivers/compass/compass_msp.h" #include "drivers/barometer/barometer_msp.h" +#include "drivers/bidir_dshot.h" #include "drivers/pitotmeter/pitotmeter_msp.h" #include "sensors/battery_sensor_fake.h" #include "drivers/bus_i2c.h" @@ -1957,18 +1958,40 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; #endif -#ifdef USE_ESC_SENSOR +#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_BIDIR) case MSP2_INAV_ESC_RPM: { uint8_t motorCount = getMotorCount(); + // Same source selection as rpmFilterUpdateTask(): bidirectional DSHOT keeps its + // own per-motor RPM, so it works on targets without USE_ESC_SENSOR too for (uint8_t i = 0; i < motorCount; i++){ - const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry - sbufWriteU32(dst, escState->rpm); + uint32_t rpm; +#ifdef USE_DSHOT_BIDIR + if (isDshotTelemetryActive()) { + rpm = lrintf(getDshotRpm(i)); + } else +#endif + { +#ifdef USE_ESC_SENSOR + rpm = getEscTelemetry(i)->rpm; +#else + rpm = 0; +#endif + } + sbufWriteU32(dst, rpm); + // Link quality of the last closed window; 255 when no reply was expected in it + // (boot, no telemetry source, a source without frame counting such as SRXL2) + const escFrameCounter_t *frames = escSensorFrameCounter(i); + const bool framesKnown = frames->lastWindowTotal > 0; + sbufWriteU8(dst, framesKnown ? frames->lastWindowSuccess : 255); + sbufWriteU16(dst, framesKnown ? frames->lastWindowTotal : 0); } } break; +#endif +#ifdef USE_ESC_SENSOR case MSP2_INAV_ESC_TELEM: { uint8_t motorCount = getMotorCount(); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 142f5f06c9b..7f88b1c4611 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -93,7 +93,7 @@ tables: "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "MAG_CALIB", "VTOL_TRANSITION", "VTOL_MC_PROTECT", "TERRAIN_NAV", "ESC", - "FW_TURN", "MAG"] + "FW_TURN", "MAG", "RPM_FILTER", "DYN_IDLE"] - name: vtol_mc_protection_mode values: ["OFF", "NAV", "NAV_AND_STABILIZED"] - name: aux_operator @@ -896,12 +896,59 @@ groups: condition: USE_MOTOR_SRXL2 min: 0 max: 9 + - name: dshot_bidir_enabled + description: "Enable bidirectional DShot telemetry on motor outputs. Required for RPM filtering without a separate ESC telemetry UART" + field: useDshotTelemetry + condition: USE_DSHOT_BIDIR + type: bool + default_value: OFF + - name: dshot_edt_enabled + description: "Enable extended DShot telemetry (temperature, voltage, current in addition to eRPM). The ESC is asked for it on every arm, so the values appear after the first arm" + field: useDshotEdt + condition: USE_DSHOT_BIDIR + type: bool + default_value: OFF - name: motor_poles field: motorPoleCount description: "The number of motor poles. Required to compute motor RPM" min: 4 max: 255 default_value: 14 + - name: dynamic_idle_min_rpm + description: "Multirotor only, ported from Betaflight, needs `dshot_bidir_enabled`. The slowest speed the motors are kept at once the throttle has been raised [RPM]; the low end of the motor output then moves with it, starting from the lowest DShot value instead of `throttle_idle`. Until the throttle has passed `airmode_throttle_threshold` once after arming the motors go no higher than `throttle_idle`. Betaflight's `dyn_idle_min_rpm` is in hundreds of RPM: 35 there is 3500 here. It starts once at least 80% of every motor's replies decode and falls back to `throttle_idle` below 50%. 0 disables it." + condition: USE_DSHOT_BIDIR + field: dynamicIdleMinRpm + default_value: 0 + min: 0 + max: 20000 + - name: dynamic_idle_p_gain + description: "P gain of `dynamic_idle_min_rpm`, on how far the slowest motor is below it" + condition: USE_DSHOT_BIDIR + field: dynamicIdlePGain + default_value: 50 + min: 1 + max: 250 + - name: dynamic_idle_i_gain + description: "I gain of `dynamic_idle_min_rpm`: it rises fast and falls slowly" + condition: USE_DSHOT_BIDIR + field: dynamicIdleIGain + default_value: 50 + min: 1 + max: 250 + - name: dynamic_idle_d_gain + description: "D gain of `dynamic_idle_min_rpm`, on how fast the slowest motor slows down" + condition: USE_DSHOT_BIDIR + field: dynamicIdleDGain + default_value: 50 + min: 0 + max: 250 + - name: dynamic_idle_max_increase + description: "The most `dynamic_idle_min_rpm` may raise the motors' low end, in tenths of a percent of the motor range: 150 is 15 %" + condition: USE_DSHOT_BIDIR + field: dynamicIdleMaxIncrease + default_value: 150 + min: 10 + max: 255 - name: PG_FAILSAFE_CONFIG type: failsafeConfig_t diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 2dccb22e3b2..c64134ed572 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -35,6 +35,7 @@ #include "config/config_reset.h" #include "drivers/pwm_output.h" +#include "drivers/bidir_dshot.h" #include "drivers/pwm_mapping.h" #include "drivers/time.h" @@ -76,6 +77,30 @@ static EXTENDED_FASTRAM int throttleDeadbandLow = 0; static EXTENDED_FASTRAM int throttleDeadbandHigh = 0; static EXTENDED_FASTRAM int throttleRangeMin = 0; static EXTENDED_FASTRAM int throttleRangeMax = 0; +#ifdef USE_DSHOT_BIDIR +#define DYNAMIC_IDLE_HZ_LPF 150 +// Decoded replies in the last 1 s window to start, and to keep going; fewer than DYNAMIC_IDLE_MIN_REPLIES is no link +#define DYNAMIC_IDLE_ENTER_PERCENT 80 +#define DYNAMIC_IDLE_LEAVE_PERCENT 50 +#define DYNAMIC_IDLE_MIN_REPLIES 100 +static EXTENDED_FASTRAM bool dynamicIdleOn; +static EXTENDED_FASTRAM bool dynamicIdleWasOn; +static EXTENDED_FASTRAM bool dynamicIdleLinkOk; +static EXTENDED_FASTRAM bool dynamicIdleThrottleRaised; +static EXTENDED_FASTRAM float dynamicIdleI; +static EXTENDED_FASTRAM float dynamicIdleLastHz; +static EXTENDED_FASTRAM pt1Filter_t dynamicIdleHzLpf; + +static inline bool isDynamicIdleOn(void) +{ + return dynamicIdleOn; +} +#else +static inline bool isDynamicIdleOn(void) +{ + return false; +} +#endif static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1; static EXTENDED_FASTRAM float throttleRateLimit = 0.0f; @@ -89,11 +114,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 #ifdef USE_MOTOR_SRXL2 @@ -101,6 +128,13 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .srxl2TelemetryRate = SETTING_ESC_SRXL2_TELEMETRY_RATE_DEFAULT, .srxl2Telemetry = SETTING_ESC_SRXL2_TELEMETRY_DEFAULT, #endif +#ifdef USE_DSHOT_BIDIR + .dynamicIdleMinRpm = SETTING_DYNAMIC_IDLE_MIN_RPM_DEFAULT, + .dynamicIdlePGain = SETTING_DYNAMIC_IDLE_P_GAIN_DEFAULT, + .dynamicIdleIGain = SETTING_DYNAMIC_IDLE_I_GAIN_DEFAULT, + .dynamicIdleDGain = SETTING_DYNAMIC_IDLE_D_GAIN_DEFAULT, + .dynamicIdleMaxIncrease = SETTING_DYNAMIC_IDLE_MAX_INCREASE_DEFAULT, +#endif ); PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_TIMER_OVERRIDE_CONFIG, 0); @@ -456,6 +490,106 @@ static uint16_t handleOutputScaling( } #endif #ifdef USE_DSHOT +#ifdef USE_DSHOT_BIDIR +// Betaflight's dynamic idle; returns the raise of the low end as a fraction of the motor range +static float dynamicIdleIncrease(float minHz, float dT) +{ + // Before the throttle has passed the airmode threshold once since arming, no higher than the static idle + if (rcCommand[THROTTLE] > rcControlsConfig()->airmodeThrottleThreshold) { + dynamicIdleThrottleRaised = true; + } + const float maxIncrease = dynamicIdleThrottleRaised ? motorConfig()->dynamicIdleMaxIncrease * 0.001f : currentBatteryProfile->motor.throttleIdle * 0.01f; + + float error = motorConfig()->dynamicIdleMinRpm / 60.0f - minHz; + // D on a smoothed copy of the speed, about 20 ms behind + const float smoothedHz = dynamicIdleLastHz + 40.0f * dT * (minHz - dynamicIdleLastHz); + const float d = (dynamicIdleLastHz - smoothedHz) * motorConfig()->dynamicIdleDGain * 0.0000003f / dT; + dynamicIdleLastHz = smoothedHz; + const float p = error * motorConfig()->dynamicIdlePGain * 0.00015f; + // I rises fast and falls slowly + error = MAX(-0.1f, error); + dynamicIdleI = constrainf(dynamicIdleI + error * motorConfig()->dynamicIdleIGain * 0.01f * dT, 0.0f, maxIncrease); + const float increase = constrainf(p + dynamicIdleI + d, 0.0f, maxIncrease); + + DEBUG_SET(DEBUG_DYN_IDLE, 0, MAX(-1000, lrintf(p * 10000))); + DEBUG_SET(DEBUG_DYN_IDLE, 1, lrintf(dynamicIdleI * 10000)); + DEBUG_SET(DEBUG_DYN_IDLE, 2, lrintf(d * 10000)); + DEBUG_SET(DEBUG_DYN_IDLE, 3, lrintf(minHz * 10.0f)); + + return increase; +} + +// Out of line, mixTable() is FAST_CODE +static NOINLINE bool applyDynamicIdle(float dT) +{ + if (!motorConfig()->dynamicIdleMinRpm || !STATE(MULTIROTOR) || !isMotorProtocolDigital() || !isDshotTelemetryActive()) { + return false; + } + + // A motor whose replies stop decoding keeps its last RPM: the static idle unless most of every motor's replies decode + const uint8_t minDecoded = dynamicIdleLinkOk ? DYNAMIC_IDLE_LEAVE_PERCENT : DYNAMIC_IDLE_ENTER_PERCENT; + float minHz = 1e6f; + for (int i = 0; i < motorCount; i++) { + // A motor this profile keeps stopped, such as a VTOL pusher, would read 0 Hz + if (currentMixer[i].throttle <= 0.0f) { + continue; + } + const escFrameCounter_t *frames = escSensorFrameCounter(i); + if (frames->lastWindowTotal < DYNAMIC_IDLE_MIN_REPLIES || frames->lastWindowSuccess < minDecoded) { + dynamicIdleLinkOk = false; + return false; + } + minHz = MIN(minHz, getMotorFrequencyHz(i)); + } + if (minHz == 1e6f) { + return false; + } + dynamicIdleLinkOk = true; + + // Back after a pause (link, profile, override): smoothing and D start from the speed now, not from before it + if (!dynamicIdleWasOn) { + pt1FilterReset(&dynamicIdleHzLpf, minHz); + dynamicIdleLastHz = minHz; + } + // getMotorFrequencyHz() is the raw last eRPM; Betaflight smooths each motor at 150 Hz before the minimum + minHz = pt1FilterApply3(&dynamicIdleHzLpf, minHz, dT); + + // The throttle starts at the static idle: spread it over the range above the dynamic one, never below 1 % + const int range = throttleRangeMax - motorConfig()->mincommand; + const float throttle = MAX((mixerThrottleCommand - throttleIdleValue) / (float)(throttleRangeMax - throttleIdleValue), 0.01f); + throttleRangeMin = MIN(motorConfig()->mincommand + 1 + lrintf(dynamicIdleIncrease(minHz, dT) * range), throttleRangeMax - 1); + mixerThrottleCommand = throttleRangeMin + throttle * (throttleRangeMax - throttleRangeMin); + return true; +} + +// At low stick without airmode the mixer is skipped and the motors idle: the dynamic idle sets that idle too +static NOINLINE void applyDynamicIdleWhenStopped(float dT) +{ + if (getMotorStatus() != MOTOR_STOPPED_USER || motorValueWhenStopped != throttleIdleValue || feature(FEATURE_REVERSIBLE_MOTORS)) { + return; + } + mixerThrottleCommand = throttleIdleValue; + throttleRangeMax = getMaxThrottle(); + if (applyDynamicIdle(dT)) { + dynamicIdleOn = true; + for (int i = 0; i < motorCount; i++) { + if (currentMixer[i].throttle > 0.0f) { + motor[i] = lrintf(mixerThrottleCommand); + } + } + } +} + +static NOINLINE void dynamicIdleReset(void) +{ + dynamicIdleThrottleRaised = false; + dynamicIdleLinkOk = false; + dynamicIdleI = 0.0f; + dynamicIdleLastHz = 0.0f; + pt1FilterInit(&dynamicIdleHzLpf, DYNAMIC_IDLE_HZ_LPF, 0.0f); +} +#endif + static void applyTurtleModeToMotors(void) { if (ARMING_FLAG(ARMED)) { @@ -566,12 +700,12 @@ void FAST_CODE writeMotors(void) } else { // While disarmed only the mixer stop value means motor off, so the - // motor test can drive a motor below the configured idle. Armed - // behaviour is unchanged: there the configured idle stays the - // threshold, so failsafe and turtle mode are not affected. + // motor test can drive a motor below the configured idle. Armed, + // the configured idle stays the threshold, except under dynamic + // idle, whose low end can be below it. motorValue = handleOutputScaling( motor[i], - ARMING_FLAG(ARMED) ? throttleIdleValue : (motorZeroCommand + 1), + (ARMING_FLAG(ARMED) && !isDynamicIdleOn()) ? throttleIdleValue : (motorZeroCommand + 1), DSHOT_DISARM_COMMAND, motorConfig()->mincommand, getMaxThrottle(), @@ -658,6 +792,10 @@ static int getReversibleMotorsThrottleDeadband(void) void FAST_CODE mixTable(float dT) { static float lastMixerThrottleCommand = 1000.0f; +#ifdef USE_DSHOT_BIDIR + dynamicIdleWasOn = dynamicIdleOn; + dynamicIdleOn = false; +#endif #ifdef USE_DSHOT if (FLIGHT_MODE(TURTLE_MODE)) { applyTurtleModeToMotors(); @@ -674,6 +812,13 @@ void FAST_CODE mixTable(float dT) for (int i = 0; i < motorCount; i++) { motor[i] = isDisarmed ? motor_disarmed[i] : motorValueWhenStopped; } +#ifdef USE_DSHOT_BIDIR + if (isDisarmed) { + dynamicIdleReset(); + } else { + applyDynamicIdleWhenStopped(dT); + } +#endif mixerThrottleCommand = motor[0]; lastMixerThrottleCommand = mixerThrottleCommand; return; @@ -790,6 +935,9 @@ void FAST_CODE mixTable(float dT) mixerThrottleCommand = rcCommand[THROTTLE]; throttleRangeMin = throttleIdleValue; throttleRangeMax = getMaxThrottle(); +#ifdef USE_DSHOT_BIDIR + dynamicIdleOn = applyDynamicIdle(dT); +#endif // Throttle scaling to limit max throttle when battery is full #ifdef USE_PROGRAMMING_FRAMEWORK @@ -954,7 +1102,9 @@ int16_t getThrottlePercent(bool useScaled) int16_t thr = constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX); if (useScaled) { - thr = (thr - throttleIdleValue) * 100 / (getMaxThrottle() - throttleIdleValue); + // Under dynamic idle the command is spread over the range above its moving low end + const int low = isDynamicIdleOn() ? throttleRangeMin : throttleIdleValue; + thr = (thr - low) * 100 / (getMaxThrottle() - low); } else { thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 43c31c9132d..78ae1a85434 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -93,6 +93,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 #ifdef USE_MOTOR_SRXL2 @@ -121,6 +123,12 @@ typedef struct motorConfig_s { uint8_t srxl2Telemetry; // read ESC telemetry off the SRXL2 link uint8_t srxl2TelemetryRate; // how often to ask the ESC for telemetry, as srxl2TelemetryRate_e #endif + // On every target, so turning USE_DSHOT_BIDIR on or off never moves the fields stored after these + uint16_t dynamicIdleMinRpm; // Slowest motor speed the dynamic idle keeps, 0 = off + uint8_t dynamicIdlePGain; + uint8_t dynamicIdleIGain; + uint8_t dynamicIdleDGain; + uint8_t dynamicIdleMaxIncrease; // Largest idle raise, in 0.1 % of the motor range } motorConfig_t; PG_DECLARE(motorConfig_t, motorConfig); diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index ffca6c8ea68..1cbdb6d6923 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -22,6 +22,8 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ +#include + #include "platform.h" #include "flight/rpm_filter.h" @@ -36,13 +38,13 @@ #include "common/maths.h" #include "common/filter.h" #include "flight/mixer.h" +#include "drivers/bidir_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 @@ -159,8 +161,7 @@ void rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequ void rpmFiltersInit(void) { - for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) - { + for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { pt1FilterInit(&motorFrequencyFilter[i], RPM_FILTER_RPM_LPF_HZ, US2S(RPM_FILTER_UPDATE_RATE_US)); } @@ -184,14 +185,32 @@ void rpmFilterUpdateTask(timeUs_t currentTimeUs) uint8_t motorCount = getMotorCount(); /* - * For each motor, read ERPM, filter it and update motor frequency + * For each motor, read its frequency, filter it and update the notches. Both sources + * hand over their latest raw value; the LPF runs here, at this task's fixed rate, so its + * cutoff does not depend on how often new frames happened to arrive */ 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 motorFrequency; +#ifdef USE_DSHOT_BIDIR + if (isDshotTelemetryActive()) { + motorFrequency = getMotorFrequencyHz(i); + } else +#endif + { +#ifdef USE_ESC_SENSOR + motorFrequency = (float)getEscTelemetry(i)->rpm / 60.0f; +#else + motorFrequency = 0.0f; +#endif + } + const float baseFrequency = pt1FilterApply(&motorFrequencyFilter[i], motorFrequency); rpmGyroUpdateFn(&gyroRpmFilters, i, baseFrequency); + + // Per-motor RPM as the notch sees it (after the LPF); blackbox has no per-motor field + if (i < DEBUG32_VALUE_COUNT) { + DEBUG_SET(DEBUG_RPM_FILTER, i, lrintf(baseFrequency * 60.0f)); + } } } diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 0611dd0ad44..88c60921e61 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -41,6 +41,7 @@ #include "config/parameter_group_ids.h" #include "flight/mixer.h" +#include "drivers/bidir_dshot.h" #include "drivers/pwm_output.h" #include "sensors/esc_sensor.h" #include "drivers/pwm_mapping.h" @@ -51,6 +52,30 @@ #include "fc/runtime_config.h" #include "fc/settings.h" +static escFrameCounter_t escSensorFrames[MAX_SUPPORTED_MOTORS]; +static timeMs_t escFrameWindowStartMs; + +escFrameCounter_t * escSensorFrameCounter(uint8_t esc) +{ + return &escSensorFrames[esc]; +} + +// NOINLINE: escSensorUpdate() is inlined into the ITCM scheduler; this runs once per window +void NOINLINE escSensorFrameWindowUpdate(timeMs_t currentTimeMs) +{ + if (currentTimeMs - escFrameWindowStartMs < ESC_FRAME_WINDOW_MS) { + return; + } + + escFrameWindowStartMs = currentTimeMs; + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + escFrameCounter_t *counter = &escSensorFrames[i]; + counter->lastWindowTotal = counter->total; + counter->lastWindowSuccess = counter->total ? (uint32_t)counter->valid * 100 / counter->total : 0; + counter->total = 0; + counter->valid = 0; + } +} #if defined(USE_ESC_SENSOR) @@ -130,6 +155,7 @@ static bool escSensorDecodeFrame(void) if (bufferPosition >= TELEMETRY_FRAME_SIZE) { uint8_t checksum = crc8_update(0, telemetryBuffer, TELEMETRY_FRAME_SIZE - 1); if (checksum == telemetryBuffer[TELEMETRY_FRAME_SIZE - 1]) { + escSensorFrames[escSensorMotor].valid++; escSensorData[escSensorMotor].dataAge = 0; escSensorData[escSensorMotor].temperature = telemetryBuffer[0]; escSensorData[escSensorMotor].voltage = ((uint16_t)telemetryBuffer[1]) << 8 | telemetryBuffer[2]; @@ -157,6 +183,32 @@ 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; +} + escSensorData_t * escSensorGetData(void) { /* @@ -249,6 +301,16 @@ bool escSensorInitialize(void) } #endif + escSensorInitData(); + +#ifdef USE_DSHOT_BIDIR + // Telemetry on the motor line: fed by the motor driver, no port to open + if (isDshotTelemetryActive()) { + ENABLE_STATE(ESC_SENSOR_ENABLED); + return true; + } +#endif + // FUNCTION_ESCSERIAL is shared between SERIALSHOT and ESC_SENSOR telemetry // They are mutually exclusive serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_ESCSERIAL); @@ -261,10 +323,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; @@ -272,6 +330,25 @@ bool escSensorInitialize(void) void escSensorUpdate(timeUs_t currentTimeUs) { +#ifdef USE_DSHOT_BIDIR + if (isDshotTelemetryActive()) { + // The motor driver refreshes the data with every decoded frame. Age it at the serial + // poll rate, so a silent ESC drops out of the combined values the same way (the serial + // poll timer doubles as the aging clock, nothing polls here) + const timeMs_t currentTimeMs = currentTimeUs / 1000; + if (currentTimeMs - escTriggerTimeMs >= ESC_REQUEST_TIMEOUT_MS) { + escTriggerTimeMs = currentTimeMs; + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + if (escSensorData[i].dataAge < ESC_DATA_INVALID) { + escSensorData[i].dataAge++; + escSensorDataNeedsUpdate = true; + } + } + } + return; + } +#endif + #ifdef USE_MOTOR_SRXL2 if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) { /* One ESC per port, so escSensorData[i] belongs to the i-th assigned @@ -326,6 +403,8 @@ void escSensorUpdate(timeUs_t currentTimeUs) const timeMs_t currentTimeMs = currentTimeUs / 1000; + escSensorFrameWindowUpdate(currentTimeMs); + switch (escSensorState) { case ESC_SENSOR_WAIT_STARTUP: if (currentTimeMs > ESC_BOOTTIME_MS) { @@ -338,6 +417,9 @@ void escSensorUpdate(timeUs_t currentTimeUs) if (!escSensorConfig()->listenOnly) { pwmRequestMotorTelemetry(escSensorMotor); } + // One reply expected per request (per listen window with listenOnly); a timeout + // or a CRC error just never makes it to valid + escSensorFrames[escSensorMotor].total++; bufferPosition = 0; escTriggerTimeMs = currentTimeMs; escSensorState = ESC_SENSOR_WAITING; @@ -378,3 +460,12 @@ void escSensorUpdate(timeUs_t currentTimeUs) } #endif + +#ifndef USE_ESC_SENSOR +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 6bccd340960..702f68938d5 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -32,6 +32,21 @@ typedef struct { uint32_t rpm; } escSensorData_t; +#define ESC_FRAME_WINDOW_MS 1000 + +// Telemetry link quality of one ESC over fixed back-to-back windows, shared by the serial +// ESC sensor and bidirectional DSHOT (only one source is ever active). total counts every +// reply expected (serial: a missing one too; bidir DSHOT: only replies with edges, as in +// Betaflight), valid those that decoded. The last closed window is kept, so the reported +// value moves once per window; lastWindowTotal == 0 means nothing was expected (boot, a +// source without frame counting such as SRXL2) and lastWindowSuccess is meaningless then +typedef struct { + uint16_t total; + uint16_t valid; + uint16_t lastWindowTotal; + uint8_t lastWindowSuccess; // percent +} escFrameCounter_t; + typedef struct escSensorConfig_s { uint16_t currentOffset; // offset consumed by the flight controller / VTX / cam / ... in mA uint8_t listenOnly; @@ -47,5 +62,11 @@ bool escSensorInitialize(void); void escSensorUpdate(timeUs_t currentTimeUs); escSensorData_t * escSensorGetData(void); escSensorData_t * getEscTelemetry(uint8_t esc); +escFrameCounter_t * escSensorFrameCounter(uint8_t esc); +// Closes the counting window of every ESC once ESC_FRAME_WINDOW_MS have passed; the active +// source calls it from wherever it runs regularly +void escSensorFrameWindowUpdate(timeMs_t currentTimeMs); 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); int getTelemetryMotorCount(void); diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 183b44a3908..80629eb05c6 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -185,7 +185,26 @@ extern uint8_t __config_end; #endif #endif -#ifdef USE_ESC_SENSOR +// DSHOT variants, all on top of USE_DSHOT: +// - USE_DSHOT_DMAR: one burst DMA stream per timer (target choice) +// - USE_DSHOT_BIDIR: bidirectional DSHOT (eRPM telemetry on the motor line, +// dshot_bidir_enabled). Needs a DMA stream per motor channel for the direction +// switching, so it excludes USE_DSHOT_DMAR. Implemented by the timer/DMA motor +// driver (pwm_output.c); RP2350 drives its motors from a PIO program that only +// transmits, so the setting is hidden there and stays off. STM32F7 is excluded +// as well: bidirectional DSHOT is only supported on STM32H7 and AT32. +#if defined(USE_DSHOT_DMAR) && !defined(USE_DSHOT) +#error "USE_DSHOT_DMAR requires USE_DSHOT" +#endif +#if defined(USE_DSHOT) && !defined(USE_DSHOT_DMAR) && !defined(RP2350) && !defined(STM32F7) +#define USE_DSHOT_BIDIR +#endif +#if defined(USE_DSHOT_BIDIR) && (!defined(USE_DSHOT) || defined(USE_DSHOT_DMAR)) +#error "USE_DSHOT_BIDIR requires USE_DSHOT and excludes USE_DSHOT_DMAR" +#endif + +// RPM filter sources: serial ESC telemetry or bidirectional DSHOT +#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_BIDIR) #define USE_RPM_FILTER #endif diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index ae3dcf9b7f1..cb6e5c750d8 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -64,7 +64,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 +183,14 @@ typedef struct uint16_t getMotorAveragePeriod(void) { -#if defined( USE_ESC_SENSOR_TELEMETRY) || defined( USE_DSHOT_TELEMETRY) +#if defined(USE_ESC_SENSOR) 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