diff --git a/docs/Blackbox.md b/docs/Blackbox.md
index 55b0f19c103..732428e1b97 100644
--- a/docs/Blackbox.md
+++ b/docs/Blackbox.md
@@ -194,6 +194,7 @@ Available debug modes include:
- `GPS` - GPS debugging
- `ALTITUDE` - Altitude estimation debugging
- `MAG` - Raw, uncalibrated magnetometer samples (useful for external compass calibration)
+- `RPM_FILTER` - Raw per-motor eRPM/100 (same unit as Betaflight's `eRPM[]` blackbox field, before the RPM filter's smoothing), motors 1-8 in `debug[0]`-`debug[7]`; RPM = value * 100 / (`motor_poles` / 2). Needs the RPM filter enabled, so bidirectional DShot or ESC telemetry. `src/utils/blackbox_rpm_to_erpm.py` renames the fields to `eRPM[]` for PIDtoolbox
- And 20+ other modes for specific subsystems
To use debug mode logging:
diff --git a/docs/Settings.md b/docs/Settings.md
index a0bdc187935..b0f66f277aa 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -803,6 +803,7 @@ Defines debug values exposed in debug variables (developer / debugging setting)
| ESC | |
| FW_TURN | |
| MAG | |
+| RPM_FILTER | |
---
@@ -944,6 +945,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.
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..0975b163f85 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);
@@ -2152,6 +2154,7 @@ static bool blackboxWriteSysinfo(void)
#endif
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider);
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->motorPwmProtocol);
+ BLACKBOX_PRINT_HEADER_LINE("motor_poles", "%d", motorConfig()->motorPoleCount);
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", getEscUpdateFrequency());
BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode);
BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 23caa6b6fa7..6adfd58cf72 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -87,6 +87,7 @@ typedef enum {
DEBUG_ESC,
DEBUG_FW_TURN,
DEBUG_MAG,
+ DEBUG_RPM_FILTER,
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..d99a2ed87f9
--- /dev/null
+++ b/src/main/drivers/bidir_dshot.c
@@ -0,0 +1,179 @@
+/*
+ * 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)
+
+// A silent ESC (reset, brownout, desync) must not leave the RPM notches on its last speed.
+// Replies come with every frame, so this is far longer than any regular gap, EDT included
+#define DSHOT_ERPM_TIMEOUT_MS 100
+
+typedef struct {
+ uint16_t telemetryData[DSHOT_TELEMETRY_STORED_TYPE_COUNT];
+ uint8_t telemetryTypes; // bit per dshotTelemetryType_e seen since boot
+ timeMs_t lastErpmMs; // millis(), not micros(): a dead ESC must not turn fresh on wrap
+} 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;
+ }
+ if (type == DSHOT_TELEMETRY_TYPE_ERPM) {
+ motorTelemetry[motorIndex].lastErpmMs = millis();
+ }
+
+ return rawValue;
+}
+
+float getMotorFrequencyHz(uint8_t motorIndex)
+{
+ const dshotTelemetryMotorState_t *state = &motorTelemetry[motorIndex];
+ if (millis() - state->lastErpmMs > DSHOT_ERPM_TIMEOUT_MS) {
+ return 0.0f;
+ }
+
+ return state->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..4ecdd37115e
--- /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); 0 once it is stale
+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..303f090881d 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -26,13 +26,16 @@
#include "build/atomic.h"
#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/io_impl.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 +73,20 @@
#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
#define MAX_DMA_TIMERS 8
+#ifdef USE_DSHOT_BIDIR
+/* Telemetry reply window, measured from the frame start: the 18-slot frame, then the ESC's
+ * ~30 us turnaround and its 21-bit GCR reply at 5/4 of the DSHOT bit rate (17 slots).
+ * DSHOT_TELEMETRY_DEADTIME_US covers the turnaround plus margin for ESC timing variation. */
+#define DSHOT_TELEMETRY_DEADTIME_US 45
+#define DSHOT_TELEMETRY_WINDOW_SLOTS (DSHOT_DMA_BUFFER_SIZE + 17)
+#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 +132,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 +170,19 @@ 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 in the main loop just before the frames start, so
+ * the completion ISR has no clock to read. 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 reply window is
+ * therefore waited out once, for all ports, before any of them is touched */
+static timeUs_t dshotTelemetryFrameStampUs = 0;
+static timeUs_t dshotTelemetryDeadtimeUs = 0;
+// DEBUG_ESC: cycle counter at the frame start, debug[motor] = cycles until its capture is armed
+static uint32_t dshotFrameStartCycles = 0;
+#endif
+
static circularBuffer_t commandsCircularBuffer;
static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE];
static currentExecutingCommand_t currentExecutingCommand;
@@ -167,6 +199,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 +340,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,8 +493,407 @@ static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer)
}
#endif
-static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput)
+#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(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(AT32F43x)
+// Per-channel DMA request bit in DIER; the LL and SPL drivers only offer fixed-channel variants
+static inline void dshotDmaReqEnable(TIM_TypeDef *tim, uint32_t dmaSource)
+{
+ SET_BIT(tim->DIER, dmaSource);
+}
+
+static inline void dshotDmaReqDisable(TIM_TypeDef *tim, uint32_t dmaSource)
+{
+ CLEAR_BIT(tim->DIER, dmaSource);
+}
+
+// Register slices of this port's timer channel for the direction switch: the channel's
+// byte in CCMR1/CCMR2 and its nibble in CCER. Derived on the fly, the shifts cost a few cycles.
+typedef struct {
+ __typeof__(((TIM_TypeDef *)0)->CCMR1) *ccmr; // CCMRx is 16 bits wide on F4, 32 on F7/H7
+ uint32_t ccmrShift;
+ uint32_t ccmrMask; // the channel's byte plus its OCxM[3] bit
+ uint32_t ccerShift;
+ uint32_t ccerMask;
+} dshotChannelRegs_t;
+
+static dshotChannelRegs_t dshotChannelRegs(const pwmOutputPort_t *port)
+{
+ const uint32_t channelIndex = port->tch->timHw->channelIndex;
+ TIM_TypeDef *tim = port->tch->timHw->tim;
+ dshotChannelRegs_t regs;
+ regs.ccmr = (channelIndex < 2) ? &tim->CCMR1 : &tim->CCMR2;
+ regs.ccmrShift = 8 * (channelIndex & 1);
+ regs.ccmrMask = 0xFF << regs.ccmrShift;
+#ifdef TIM_CCMR1_OC1M_3
+ // F7/H7 keep a fourth OCxM bit outside the channel byte; F4 has none
+ regs.ccmrMask |= TIM_CCMR1_OC1M_3 << regs.ccmrShift;
+#endif
+ regs.ccerShift = 4 * channelIndex;
+ regs.ccerMask = 0xF << regs.ccerShift;
+ return regs;
+}
+
+// CCMRx channel byte for capture: CCxS=01 (TI direct), ICxPSC=0, ICxF=0010 (fDTS, N=4)
+#define DSHOT_CCMR_INPUT (TIM_CCMR1_CC1S_0 | TIM_CCMR1_IC1F_1)
+// CCMRx channel byte for output: CCxS=00, OCxM=110 (PWM1); preload is added once CCR is zeroed
+#define DSHOT_CCMR_OUTPUT (TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1)
+// CCER channel nibble for capture: both edges, channel on
+#define DSHOT_CCER_INPUT (TIM_CCER_CC1E | TIM_CCER_CC1P | TIM_CCER_CC1NP)
+#else
+// The channel identifiers are evenly spaced, so channelIndex maps onto them linearly
+static uint32_t dshotTimChannel(const pwmOutputPort_t *port)
+{
+ 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);
+}
+#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;
+}
+
+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(AT32F43x)
+ // 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;
+
+ // Channel back to PWM1 output. CCxS is only writable with the channel off, and CCR is
+ // zeroed before OCxPE goes back on so the stale capture value can't give one bit period
+ // of output before the first update event. Idle state (CR2 OISx) is untouched.
+ const dshotChannelRegs_t regs = dshotChannelRegs(port);
+ CLEAR_BIT(tim->CCER, regs.ccerMask);
+ MODIFY_REG(*regs.ccmr, regs.ccmrMask, DSHOT_CCMR_OUTPUT << regs.ccmrShift);
+ *port->ccr = 0;
+ SET_BIT(*regs.ccmr, TIM_CCMR1_OC1PE << regs.ccmrShift);
+ uint32_t ccer;
+ if (port->tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) {
+ ccer = TIM_CCER_CC1NE | (inverted ? TIM_CCER_CC1NP : 0);
+ } else {
+ ccer = TIM_CCER_CC1E | (inverted ? TIM_CCER_CC1P : 0);
+ }
+ MODIFY_REG(tim->CCER, regs.ccerMask, ccer << regs.ccerShift);
+
+ // Stream back to memory-to-peripheral; timerPWMPrepareDMA() sets count and addresses and
+ // enables it for the next frame. The capture may still be draining (the decode loop only
+ // cleared EN), so wait for EN to read 0 before touching CR, then clear all five event
+ // flags: EN=1 is ignored while any of them is set (RM0433).
+ DMA_Stream_TypeDef *stream = port->tch->dma->ref;
+ for (uint32_t timeout = 10000; timeout && (stream->CR & DMA_SxCR_EN); timeout--) { }
+ DMA_CLEAR_FLAG(port->tch->dma, DMA_IT_TCIF | DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_DMEIF | DMA_IT_FEIF);
+ stream->CR = (stream->CR & ~(DMA_SxCR_DIR | DMA_SxCR_EN)) | DMA_SxCR_DIR_0;
+ port->telemetryInputActive = false;
+#else
+ // 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;
+#endif
+}
+
+FAST_CODE static void pwmDshotSetDirectionInput(pwmOutputPort_t *port)
+{
+#if !defined(AT32F43x)
+ // Runs in the DMA completion ISR for every motor in turn, so it only writes registers:
+ // no HAL/LL init calls, and the stream is reconfigured in place (hardware clears its EN
+ // bit at end of transfer, the wait below only covers the flag/EN ordering).
+ // pwmOutputPorts lives in non-cacheable DMA_RAM, so read tch once.
+ TCH_t *tch = port->tch;
+ TIM_TypeDef *tim = tch->timHw->tim;
+ const dshotChannelRegs_t regs = dshotChannelRegs(port);
+
+ // 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.
+ // ARPE is set, so this lands at the next update event, within one bit period.
+ 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.
+ // Drive it as a plain GPIO output at the preset ODR level meanwhile. Only MODER changes;
+ // pull, speed and AF stay as dshotConnectOutputs() set them.
+ const IO_t io = IOGetByTag(tch->timHw->tag);
+ GPIO_TypeDef *gpio = IO_GPIO(io);
+ const uint32_t moderShift = 2 * IO_GPIOPinIdx(io);
+ MODIFY_REG(gpio->MODER, GPIO_MODER_MODE0_Msk << moderShift, GPIO_MODER_MODE0_0 << moderShift);
+#endif
+
+ // Channel to input capture, both edges, filter N=4. CCxS is only writable with the channel off.
+ CLEAR_BIT(tim->CCER, regs.ccerMask);
+ MODIFY_REG(*regs.ccmr, regs.ccmrMask, DSHOT_CCMR_INPUT << regs.ccmrShift);
+ MODIFY_REG(tim->CCER, regs.ccerMask, DSHOT_CCER_INPUT << regs.ccerShift);
+
+#if defined(STM32H7)
+ MODIFY_REG(gpio->MODER, GPIO_MODER_MODE0_Msk << moderShift, GPIO_MODER_MODE0_1 << moderShift);
+#endif
+
+ // Stream to peripheral-to-memory: addresses, FIFO and request stay from the output frame.
+ // Clear all five event flags first, EN=1 is ignored while any of them is set (RM0433).
+ DMA_Stream_TypeDef *stream = tch->dma->ref;
+ for (uint32_t timeout = 1000; timeout && (stream->CR & DMA_SxCR_EN); timeout--) { }
+ DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF | DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_DMEIF | DMA_IT_FEIF);
+ const uint32_t cr = (stream->CR & ~(DMA_SxCR_DIR | DMA_SxCR_EN)) | DMA_SxCR_TCIE;
+ stream->CR = cr;
+ stream->NDTR = GCR_TELEMETRY_INPUT_LEN;
+ stream->CR = cr | DMA_SxCR_EN;
+ dshotDmaReqEnable(tim, dshotDmaSource(port));
+#else
+ // 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);
+#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. Runs once per motor per
+// frame, back to back, so it lives in ITCM where available.
+FAST_CODE static void pwmDshotDmaIrqHandler(DMA_t descriptor)
+{
+ if (!DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
+ return;
+ }
+
+ // userParam is the motor index (see motorConfigDshot())
+ pwmOutputPort_t *port = motors[descriptor->userParam].pwmPort;
+
+#if defined(AT32F43x)
+ dma_channel_enable(port->tch->dma->ref, FALSE);
+ tmr_dma_request_enable(port->tch->timHw->tim, dshotDmaSource(port), FALSE);
+#else
+ // The stream disabled itself at end of transfer (normal mode). The DMA request must go
+ // too: CCR is 0 after the frame, so compare matches keep a request pending and the stream
+ // would serve it as a bogus first capture the moment it is re-enabled for input.
+ dshotDmaReqDisable(port->tch->timHw->tim, dshotDmaSource(port));
+#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);
+ dshotTelemetryPending = true;
+
+ // DEBUG_ESC: debug[motor] = cycles from the frame start to this capture being armed
+ // (H7: /480 = us, F7: /216)
+ if (debugMode == DEBUG_ESC && descriptor->userParam < DEBUG32_VALUE_COUNT) {
+ debug[descriptor->userParam] = ticks() - dshotFrameStartCycles;
+ }
+ }
+
+ 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 out the reply window of the frame before switching any port back (see
+ // dshotTelemetryFrameStampUs)
+ if ((micros() - dshotTelemetryFrameStampUs) < 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(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 - port->tch->dma->ref->NDTR;
+ dshotDmaReqDisable(port->tch->timHw->tim, dshotDmaSource(port));
+ CLEAR_BIT(port->tch->dma->ref->CR, DMA_SxCR_EN);
+#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, uint8_t motorIndex)
+{
+ UNUSED(motorIndex);
// Try allocating new port
pwmOutputPort_t * port = pwmOutConfig(timerHardware, OWNER_MOTOR, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput);
@@ -429,9 +901,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 +938,25 @@ 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 * (DSHOT_TELEMETRY_WINDOW_SLOTS * DSHOT_MOTOR_BITLENGTH) / dshotHz;
+#if defined(USE_HAL_DRIVER)
+ // FIFO threshold 1/4 for both directions (the F7/H7 timer driver set FULL), so
+ // captured edges reach memory one word at a time and the decode never finds a
+ // partial reply still sitting in the FIFO. Set once here; the direction switch
+ // leaves FCR alone. The F4 driver runs the stream in direct mode, which already
+ // writes every word straight away.
+ port->tch->dma->ref->FCR = DMA_SxFCR_DMDIS;
+#endif
+ pwmDshotSetDirectionOutput(port);
+ // The handler gets the motor index: it reaches the port through motors[] and uses
+ // the index straight as the debug slot
+ dmaSetHandler(port->tch->dma, pwmDshotDmaIrqHandler, NVIC_PRIO_TIMER_DMA, motorIndex);
+ }
+#endif
}
#endif
@@ -473,11 +981,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 +1001,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 +1070,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 +1082,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 +1127,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 +1152,13 @@ void pwmCompleteMotorUpdate(void) {
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
+#ifdef USE_DSHOT_BIDIR
+ if (dshotPinsDisconnected) {
+ dshotConnectOutputs();
+ dshotPinsDisconnected = false;
+ }
+#endif
+
if (!executeDShotCommands()) {
return;
}
@@ -647,6 +1187,16 @@ void pwmCompleteMotorUpdate(void) {
}
}
+#ifdef USE_DSHOT_BIDIR
+ if (useDshotTelemetry) {
+ // Reference for the reply window; the completion ISRs don't read the clock
+ dshotTelemetryFrameStampUs = micros();
+ if (debugMode == DEBUG_ESC) {
+ dshotFrameStartCycles = ticks();
+ }
+ }
+#endif
+
// Start DMA on all timers
for (int index = 0; index < motorCount; index++) {
if (motors[index].pwmPort && motors[index].pwmPort->configured) {
@@ -672,6 +1222,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
@@ -764,7 +1317,7 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, bo
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
- motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, getDshotHz(initMotorProtocol), enableOutput);
+ motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, getDshotHz(initMotorProtocol), enableOutput, motorIndex);
break;
#endif
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..228d872e3d4 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -238,7 +238,8 @@ static const char *debugModeNames[DEBUG_COUNT] = {
"TERRAIN_NAV",
"ESC",
"FW_TURN",
- "MAG"
+ "MAG",
+ "RPM_FILTER"
};
/* 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..400bbeca6d6 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"]
- name: vtol_mc_protection_mode
values: ["OFF", "NAV", "NAV_AND_STABILIZED"]
- name: aux_operator
@@ -896,6 +896,18 @@ 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"
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 2dccb22e3b2..0ec3ac21994 100644
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -89,11 +89,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
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 43c31c9132d..d11fe1ae3af 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
diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c
index ffca6c8ea68..7e3c98e7d75 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,14 +38,17 @@
#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
+// Serial ESC telemetry ages by one per missed reply of that motor (SRXL2 only once its own
+// stale timeout passed): a single CRC error keeps the value, a silent ESC drops to 0
+#define RPM_FILTER_MAX_ESC_DATA_AGE 2
#define RPM_FILTER_HARMONICS 3
PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 1);
@@ -159,8 +164,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));
}
@@ -183,14 +187,35 @@ void rpmFilterUpdateTask(timeUs_t currentTimeUs)
UNUSED(currentTimeUs);
uint8_t motorCount = getMotorCount();
+ // Debug holds eRPM/100 like Betaflight's eRPM[] blackbox field, so log tools can reuse it
+ const float hzToErpm100 = 60.0f * (motorConfig()->motorPoleCount / 2.0f) / ERPM_PER_LSB;
/*
- * 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
+ const escSensorData_t *escData = getEscTelemetry(i);
+ motorFrequency = escData->dataAge <= RPM_FILTER_MAX_ESC_DATA_AGE ? (float)escData->rpm / 60.0f : 0.0f;
+#else
+ motorFrequency = 0.0f;
+#endif
+ }
+ // Raw telemetry before the LPF; blackbox has no per-motor field
+ if (i < DEBUG32_VALUE_COUNT) {
+ DEBUG_SET(DEBUG_RPM_FILTER, i, lrintf(motorFrequency * hzToErpm100));
+ }
+ const float baseFrequency = pt1FilterApply(&motorFrequencyFilter[i], motorFrequency);
rpmGyroUpdateFn(&gyroRpmFilters, i, baseFrequency);
}
}
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