mixer: dynamic idle from bidirectional DShot RPM

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:

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.

docs/development/msp/README.md

@@ -4180,16 +4180,18 @@ 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.
 
 ## <a id="msp2_inav_esc_rpm"></a>`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**  
 
 **Reply Payload:**
 |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.
 
 ## <a id="msp2_inav_esc_telem"></a>`MSP2_INAV_ESC_TELEM (8257 / 0x2041)`
 **Description:** Retrieves the full telemetry data structure reported by each ESC.  

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,

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

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);
 

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;
 

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <math.h>
+
+#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