Dshot RPM Telemetry Refactoring

src/main/blackbox/blackbox.c

@@ -1165,7 +1165,7 @@ static void loadMainState(timeUs_t currentTimeUs)
 
 #ifdef USE_DSHOT_TELEMETRY
     for (int i = 0; i < motorCount; i++) {
-        blackboxCurrent->erpm[i] = getDshotTelemetry(i);
+        blackboxCurrent->erpm[i] = getDshotErpm(i);
     }
 #endif
 

src/main/build/debug.c

@@ -18,8 +18,6 @@
  * If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <stdint.h>
-
 #include "platform.h"
 
 #include "debug.h"

src/main/cli/cli.c

@@ -6119,8 +6119,8 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline)
 #endif
 
         for (uint8_t i = 0; i < getMotorCount(); i++) {
-            const uint16_t erpm = getDshotTelemetry(i);
-            const uint16_t rpm = erpmToRpm(erpm);
+            const uint16_t erpm = getDshotErpm(i);
+            const uint16_t rpm = lrintf(getDshotRpm(i));
 
             cliPrintf("%5d   %c%c%c%c%c %6d %6d %6d",
                     i + 1,

src/main/common/time.h

@@ -43,6 +43,8 @@ typedef uint32_t timeUs_t;
 #define TIMEZONE_OFFSET_MINUTES_MIN -780  // -13 hours
 #define TIMEZONE_OFFSET_MINUTES_MAX 780   // +13 hours
 
+#define SECONDS_PER_MINUTE          60.0f
+
 static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); }
 static inline int32_t cmpTimeCycles(uint32_t a, uint32_t b) { return (int32_t)(a - b); }
 
@@ -101,4 +103,5 @@ bool rtcSetDateTime(dateTime_t *dt);
 
 void rtcPersistWrite(int16_t offsetMinutes);
 bool rtcPersistRead(rtcTime_t *t);
-#endif
+
+#endif // USE_RTC_TIME

src/main/drivers/dshot.c

@@ -22,9 +22,9 @@
  * Follows the extended dshot telemetry documentation found at https://github.com/bird-sanctuary/extended-dshot-telemetry
  */
 
-#include <stdbool.h>
-#include <stdint.h>
+#include <float.h>
 #include <math.h>
+#include <stdbool.h>
 #include <string.h>
 
 #include "platform.h"
@@ -34,6 +34,7 @@
 #include "build/debug.h"
 #include "build/atomic.h"
 
+#include "common/filter.h"
 #include "common/maths.h"
 
 #include "config/feature.h"
@@ -46,9 +47,14 @@
 
 #include "flight/mixer.h"
 
+#include "pg/rpm_filter.h"
+
 #include "rx/rx.h"
+
 #include "dshot.h"
 
+#define ERPM_PER_LSB            100.0f
+
 void dshotInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow)
 {
     float outputLimitOffset = DSHOT_RANGE * (1 - outputLimit);
@@ -134,9 +140,30 @@ FAST_CODE uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb)
 
 #ifdef USE_DSHOT_TELEMETRY
 
-
 FAST_DATA_ZERO_INIT dshotTelemetryState_t dshotTelemetryState;
 
+FAST_DATA_ZERO_INIT static pt1Filter_t motorFreqLpf[MAX_SUPPORTED_MOTORS];
+FAST_DATA_ZERO_INIT static float motorFrequencyHz[MAX_SUPPORTED_MOTORS];
+FAST_DATA_ZERO_INIT static float minMotorFrequencyHz;
+FAST_DATA_ZERO_INIT static float erpmToHz;
+FAST_DATA_ZERO_INIT static float dshotRpmAverage;
+FAST_DATA_ZERO_INIT static float dshotRpm[MAX_SUPPORTED_MOTORS];
+
+void initDshotTelemetry(const timeUs_t looptimeUs)
+{
+    // if bidirectional DShot is not available
+    if (!motorConfig()->dev.useDshotTelemetry) {
+        return;
+    }
+
+    // init LPFs for RPM data
+    for (int i = 0; i < getMotorCount(); i++) {
+        pt1FilterInit(&motorFreqLpf[i], pt1FilterGain(rpmFilterConfig()->rpm_filter_lpf_hz, looptimeUs * 1e-6f));
+    }
+
+    erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (motorConfig()->motorPoleCount / 2.0f);
+}
+
 static uint32_t dshot_decode_eRPM_telemetry_value(uint16_t value)
 {
     // eRPM range
@@ -259,41 +286,78 @@ static void dshotUpdateTelemetryData(uint8_t motorIndex, dshotTelemetryType_t ty
     }
 }
 
-uint16_t getDshotTelemetry(uint8_t index)
+FAST_CODE_NOINLINE void updateDshotTelemetry(void)
 {
-    // Process telemetry in case it haven´t been processed yet
-    if (dshotTelemetryState.rawValueState == DSHOT_RAW_VALUE_STATE_NOT_PROCESSED) {
-        const unsigned motorCount = motorDeviceCount();
-        uint32_t erpmTotal = 0;
-        uint32_t rpmSamples = 0;
-
-        // Decode all telemetry data now to discharge interrupt from this task
-        for (uint8_t k = 0; k < motorCount; k++) {
-            dshotTelemetryType_t type;
-            uint32_t value;
-
-            dshot_decode_telemetry_value(k, &value, &type);
-
-            if (value != DSHOT_TELEMETRY_INVALID) {
-                dshotUpdateTelemetryData(k, type, value);
-
-                if (type == DSHOT_TELEMETRY_TYPE_eRPM) {
-                    erpmTotal += value;
-                    rpmSamples++;
-                }
+    if (!motorConfig()->dev.useDshotTelemetry) {
+        return;
+    }
+
+    // Only process telemetry in case it hasn´t been processed yet
+    if (dshotTelemetryState.rawValueState != DSHOT_RAW_VALUE_STATE_NOT_PROCESSED) {
+        return;
+    }
+
+    const unsigned motorCount = motorDeviceCount();
+    uint32_t erpmTotal = 0;
+    uint32_t rpmSamples = 0;
+
+    // Decode all telemetry data now to discharge interrupt from this task
+    for (uint8_t k = 0; k < motorCount; k++) {
+        dshotTelemetryType_t type;
+        uint32_t value;
+
+        dshot_decode_telemetry_value(k, &value, &type);
+
+        if (value != DSHOT_TELEMETRY_INVALID) {
+            dshotUpdateTelemetryData(k, type, value);
+
+            if (type == DSHOT_TELEMETRY_TYPE_eRPM) {
+                dshotRpm[k] = erpmToRpm(value);
+                erpmTotal += value;
+                rpmSamples++;
             }
         }
+    }
 
-        // Update average
-        if (rpmSamples > 0) {
-            dshotTelemetryState.averageErpm = (uint16_t)(erpmTotal / rpmSamples);
-        }
+    // Update average
+    if (rpmSamples > 0) {
+        dshotRpmAverage = erpmToRpm(erpmTotal) / (float)rpmSamples;
+    }
 
-        // Set state to processed
-        dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_PROCESSED;
+    // update filtered rotation speed of motors for features (e.g. "RPM filter")
+    minMotorFrequencyHz = FLT_MAX;
+    for (int motor = 0; motor < getMotorCount(); motor++) {
+        motorFrequencyHz[motor] = pt1FilterApply(&motorFreqLpf[motor], erpmToHz * getDshotErpm(motor));
+        minMotorFrequencyHz = MIN(minMotorFrequencyHz, motorFrequencyHz[motor]);
     }
 
-    return dshotTelemetryState.motorState[index].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM];
+    // Set state to processed
+    dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_PROCESSED;
+}
+
+uint16_t getDshotErpm(uint8_t motorIndex)
+{
+    return dshotTelemetryState.motorState[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM];
+}
+
+float getDshotRpm(uint8_t motorIndex)
+{
+    return dshotRpm[motorIndex];
+}
+
+float getDshotRpmAverage(void)
+{
+    return dshotRpmAverage;
+}
+
+float getMotorFrequencyHz(uint8_t motorIndex)
+{
+    return motorFrequencyHz[motorIndex];
+}
+
+float getMinMotorFrequencyHz(void)
+{
+    return minMotorFrequencyHz;
 }
 
 bool isDshotMotorTelemetryActive(uint8_t motorIndex)
@@ -320,21 +384,15 @@ void dshotCleanTelemetryData(void)
     memset(&dshotTelemetryState, 0, sizeof(dshotTelemetryState));
 }
 
-
-uint32_t getDshotAverageRpm(void)
-{
-    return erpmToRpm(dshotTelemetryState.averageErpm);
-}
-
 #endif // USE_DSHOT_TELEMETRY
 
 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
 
 // Used with serial esc telem as well as dshot telem
-uint32_t erpmToRpm(uint16_t erpm)
+float erpmToRpm(uint32_t erpm)
 {
-    //  rpm = (erpm * 100) / (motorConfig()->motorPoleCount / 2)
-    return (erpm * 200) / motorConfig()->motorPoleCount;
+    // rpm = (erpm * ERPM_PER_LSB) / (motorConfig()->motorPoleCount / 2)
+    return erpm * erpmToHz * SECONDS_PER_MINUTE;
 }
 
 #endif // USE_ESC_SENSOR || USE_DSHOT_TELEMETRY

src/main/drivers/dshot.h

@@ -20,6 +20,9 @@
 
 #pragma once
 
+#include <stdbool.h>
+#include <stdint.h>
+
 #include "common/time.h"
 
 #include "pg/motor.h"
@@ -66,13 +69,14 @@ typedef enum dshotTelemetryType_e {
     DSHOT_TELEMETRY_TYPE_DEBUG2         = 5,
     DSHOT_TELEMETRY_TYPE_DEBUG3         = 6,
     DSHOT_TELEMETRY_TYPE_STATE_EVENTS   = 7,
-    DSHOT_TELEMETRY_TYPE_COUNT          = 8
+    DSHOT_TELEMETRY_TYPE_COUNT
 } dshotTelemetryType_t;
 
 typedef enum dshotRawValueState_e {
-    DSHOT_RAW_VALUE_STATE_INVALID = 0,
+    DSHOT_RAW_VALUE_STATE_INVALID       = 0,
     DSHOT_RAW_VALUE_STATE_NOT_PROCESSED = 1,
-    DSHOT_RAW_VALUE_STATE_PROCESSED = 2
+    DSHOT_RAW_VALUE_STATE_PROCESSED     = 2,
+    DSHOT_RAW_VALUE_STATE_COUNT
 } dshotRawValueState_t;
 
 typedef struct dshotProtocolControl_s {
@@ -103,7 +107,6 @@ typedef struct dshotTelemetryState_s {
     uint32_t readCount;
     dshotTelemetryMotorState_t motorState[MAX_SUPPORTED_MOTORS];
     uint32_t inputBuffer[MAX_GCR_EDGES];
-    uint16_t averageErpm;
     dshotRawValueState_t rawValueState;
 } dshotTelemetryState_t;
 
@@ -112,15 +115,23 @@ extern dshotTelemetryState_t dshotTelemetryState;
 #ifdef USE_DSHOT_TELEMETRY_STATS
 void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool packetValid, timeMs_t currentTimeMs);
 #endif
-#endif
+#endif // USE_DSHOT_TELEMETRY
+
+void initDshotTelemetry(const timeUs_t looptimeUs);
+void updateDshotTelemetry(void);
+
+uint16_t getDshotErpm(uint8_t motorIndex);
+float getDshotRpm(uint8_t motorIndex);
+float getDshotRpmAverage(void);
+float getMotorFrequencyHz(uint8_t motorIndex);
+float getMinMotorFrequencyHz(void);
 
-uint16_t getDshotTelemetry(uint8_t index);
-uint32_t erpmToRpm(uint16_t erpm);
-uint32_t getDshotAverageRpm(void);
 bool isDshotMotorTelemetryActive(uint8_t motorIndex);
 bool isDshotTelemetryActive(void);
+void dshotCleanTelemetryData(void);
+
+float erpmToRpm(uint32_t erpm);
 
 int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex);
 
 void validateAndfixMotorOutputReordering(uint8_t *array, const unsigned size);
-void dshotCleanTelemetryData(void);

src/main/fc/core.c

@@ -1270,8 +1270,10 @@ FAST_CODE bool pidLoopReady(void)
 
 FAST_CODE void taskFiltering(timeUs_t currentTimeUs)
 {
+#ifdef USE_DSHOT_TELEMETRY
+    updateDshotTelemetry();  // decode and update Dshot telemetry
+#endif
     gyroFiltering(currentTimeUs);
-
 }
 
 // Function for loop trigger

src/main/fc/init.c

@@ -54,6 +54,7 @@
 #include "drivers/camera_control_impl.h"
 #include "drivers/compass/compass.h"
 #include "drivers/dma.h"
+#include "drivers/dshot.h"
 #include "drivers/exti.h"
 #include "drivers/flash.h"
 #include "drivers/inverter.h"
@@ -694,6 +695,11 @@ void init(void)
     // Now reset the targetLooptime as it's possible for the validation to change the pid_process_denom
     gyroSetTargetLooptime(pidConfig()->pid_process_denom);
 
+#ifdef USE_DSHOT_TELEMETRY
+    // Initialize the motor frequency filter now that we have a target looptime
+    initDshotTelemetry(gyro.targetLooptime);
+#endif
+
     // Finally initialize the gyro filtering
     gyroInitFilters();
 

src/main/flight/mixer.c

@@ -220,7 +220,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
         if (mixerRuntime.dynIdleMinRps > 0.0f) {
             const float maxIncrease = isAirmodeActivated()
                 ? mixerRuntime.dynIdleMaxIncrease : mixerRuntime.dynIdleStartIncrease;
-            float minRps = getMinMotorFrequency();
+            float minRps = getMinMotorFrequencyHz();
             DEBUG_SET(DEBUG_DYN_IDLE, 3, lrintf(minRps * 10.0f));
             float rpsError = mixerRuntime.dynIdleMinRps - minRps;
             // PT1 type lowpass delay and smoothing for D