Merge pull request #11772 from damosvil/use_calc_esc_rpm_to_erpm_to_rpm

Unify all eRPM to RPM calculations into one single function

src/main/cli/cli.c

@@ -165,7 +165,6 @@ bool cliMode = false;
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
 #include "sensors/compass.h"
-#include "sensors/esc_sensor.h"
 #include "sensors/gyro.h"
 #include "sensors/gyro_init.h"
 #include "sensors/sensors.h"
@@ -6169,6 +6168,9 @@ 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);
+
             cliPrintf("%5d   %c%c%c%c%c %6d %6d %6d",
                     i + 1,
                     ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_eRPM)) ? 'R' : '-'),
@@ -6176,9 +6178,7 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline)
                     ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_VOLTAGE)) ? 'V' : '-'),
                     ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) ? 'C' : '-'),
                     ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS)) ? 'S' : '-'),
-                    dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM] * 100,
+                    erpm * 100, rpm, rpm / 60);
-                    dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM] * 100 * 2 / motorConfig()->motorPoleCount,
-                    dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM] * 100 * 2 / motorConfig()->motorPoleCount / 60);
 
 #ifdef USE_DSHOT_TELEMETRY_STATS
             if (isDshotMotorTelemetryActive(i)) {

src/main/drivers/dshot.c

@@ -40,6 +40,8 @@
 #include "drivers/dshot_command.h"
 #include "drivers/nvic.h"
 
+#include "flight/mixer.h"
+
 #include "rx/rx.h"
 #include "dshot.h"
 
@@ -188,6 +190,17 @@ FAST_CODE void dshotUpdateTelemetryData(uint8_t motorIndex, dshotTelemetryType_t
     }
 }
 
+uint32_t erpmToRpm(uint16_t erpm)
+{
+    //  rpm = (erpm * 100) / (motorConfig()->motorPoleCount / 2)
+    return (erpm * 200) / motorConfig()->motorPoleCount;
+}
+
+uint32_t getDshotAverageRpm(void)
+{
+    return dshotTelemetryState.averageRpm;
+}
+
 #endif // USE_DSHOT_TELEMETRY
 
 #ifdef USE_DSHOT_TELEMETRY_STATS

src/main/drivers/dshot.h

@@ -95,6 +95,7 @@ typedef struct dshotTelemetryState_s {
     uint32_t readCount;
     dshotTelemetryMotorState_t motorState[MAX_SUPPORTED_MOTORS];
     uint32_t inputBuffer[MAX_GCR_EDGES];
+    uint32_t averageRpm;
 } dshotTelemetryState_t;
 
 extern dshotTelemetryState_t dshotTelemetryState;
@@ -105,6 +106,8 @@ void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool pac
 #endif
 
 uint16_t getDshotTelemetry(uint8_t index);
+uint32_t erpmToRpm(uint16_t erpm);
+uint32_t getDshotAverageRpm(void);
 bool isDshotMotorTelemetryActive(uint8_t motorIndex);
 bool isDshotTelemetryActive(void);
 

src/main/drivers/dshot_bitbang.c

@@ -495,6 +495,9 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
 
 static bool bbUpdateStart(void)
 {
+    uint32_t totalErpm;
+    uint32_t totalMotorsWithErpmData;
+
 #ifdef USE_DSHOT_TELEMETRY
     if (useDshotTelemetry) {
 #ifdef USE_DSHOT_TELEMETRY_STATS
@@ -507,6 +510,9 @@ static bool bbUpdateStart(void)
             return false;
         }
 
+        totalErpm = 0;
+        totalMotorsWithErpmData = 0;
+
         for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
             dshotTelemetryType_t type;
 #ifdef USE_DSHOT_CACHE_MGMT
@@ -543,6 +549,10 @@ static bool bbUpdateStart(void)
             dshotTelemetryState.readCount++;
 
             if (value != DSHOT_TELEMETRY_INVALID) {
+                if (type == DSHOT_TELEMETRY_TYPE_eRPM) {
+                    totalErpm += value;
+                    totalMotorsWithErpmData++;
+                }
                 dshotUpdateTelemetryData(motorIndex, type, value);
                 if (motorIndex < 4) {
                     DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, value);
@@ -554,6 +564,11 @@ static bool bbUpdateStart(void)
             updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], value != DSHOT_TELEMETRY_INVALID, currentTimeMs);
 #endif
         }
+
+        // Calculate average when possible
+        if (totalMotorsWithErpmData > 0) {
+            dshotTelemetryState.averageRpm = erpmToRpm(totalErpm / totalMotorsWithErpmData);
+        }
     }
 #endif
     for (int i = 0; i < usedMotorPorts; i++) {

src/main/drivers/pwm_output_dshot_shared.c

@@ -187,6 +187,8 @@ FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
 #endif
     const timeUs_t currentUs = micros();
     dshotTelemetryType_t type;
+    uint32_t totalErpm = 0;
+    uint32_t totalMotorsWithErpmData = 0;
 
     for (int i = 0; i < dshotPwmDevice.count; i++) {
         timeDelta_t usSinceInput = cmpTimeUs(currentUs, inputStampUs);
@@ -220,6 +222,10 @@ FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
                 bool validTelemetryPacket = false;
 #endif
                 if (value != DSHOT_TELEMETRY_INVALID) {
+                    if (type == DSHOT_TELEMETRY_TYPE_eRPM) {
+                        totalErpm += value;
+                        totalMotorsWithErpmData++;
+                    }
                     dshotUpdateTelemetryData(value, type, value);
                     if (i < 4) {
                         DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value);
@@ -240,6 +246,10 @@ FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
         }
         pwmDshotSetDirectionOutput(&dmaMotors[i]);
     }
+
+    if (totalMotorsWithErpmData > 0) {
+        dshotTelemetryState.averageRpm = erpmToRpm(totalErpm / totalMotorsWithErpmData);
+    }
     inputStampUs = 0;
     dshotEnableChannels(dshotPwmDevice.count);
     return true;

src/main/msp/msp.c

@@ -139,7 +139,6 @@
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
 #include "sensors/compass.h"
-#include "sensors/esc_sensor.h"
 #include "sensors/gyro.h"
 #include "sensors/gyro_init.h"
 #include "sensors/rangefinder.h"
@@ -1211,7 +1210,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
 
 #ifdef USE_DSHOT_TELEMETRY
             if (motorConfig()->dev.useDshotTelemetry) {
-                rpm = getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount;
+                rpm = erpmToRpm(getDshotTelemetry(i));
                 rpmDataAvailable = true;
                 invalidPct = 10000; // 100.00%
 
@@ -1247,7 +1246,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
             if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
                 escSensorData_t *escData = getEscSensorData(i);
                 if (!rpmDataAvailable) {  // We want DSHOT telemetry RPM data (if available) to have precedence
-                    rpm = calcEscRpm(escData->rpm);
+                    rpm = erpmToRpm(escData->rpm);
                     rpmDataAvailable = true;
                 }
                 escTemperature = escData->temperature;

src/main/osd/osd.c

@@ -94,7 +94,6 @@
 
 #include "sensors/acceleration.h"
 #include "sensors/battery.h"
-#include "sensors/esc_sensor.h"
 #include "sensors/sensors.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
@@ -502,19 +501,14 @@ static void osdResetStats(void)
 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
 static int32_t getAverageEscRpm(void)
 {
-#ifdef USE_DSHOT_TELEMETRY
-    if (motorConfig()->dev.useDshotTelemetry) {
-        uint32_t rpm = 0;
-        for (int i = 0; i < getMotorCount(); i++) {
-            rpm += getDshotTelemetry(i);
-        }
-        rpm = rpm / getMotorCount();
-        return rpm * 100 * 2 / motorConfig()->motorPoleCount;
-    }
-#endif
 #ifdef USE_ESC_SENSOR
     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
-        return calcEscRpm(osdEscDataCombined->rpm);
+        return erpmToRpm(osdEscDataCombined->rpm);
+    }
+#endif
+#ifdef USE_DSHOT_TELEMETRY
+    if (motorConfig()->dev.useDshotTelemetry) {
+        return getDshotAverageRpm();
     }
 #endif
     return 0;

src/main/osd/osd_elements.c

@@ -159,7 +159,6 @@
 #include "sensors/adcinternal.h"
 #include "sensors/barometer.h"
 #include "sensors/battery.h"
-#include "sensors/esc_sensor.h"
 #include "sensors/sensors.h"
 
 #ifdef USE_GPS_PLUS_CODES
@@ -259,12 +258,12 @@ static int getEscRpm(int i)
 {
 #ifdef USE_DSHOT_TELEMETRY
     if (motorConfig()->dev.useDshotTelemetry) {
-        return 100.0f / (motorConfig()->motorPoleCount / 2.0f) * getDshotTelemetry(i);
+        return erpmToRpm(getDshotTelemetry(i));
     }
 #endif
 #ifdef USE_ESC_SENSOR
     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
-        return calcEscRpm(getEscSensorData(i)->rpm);
+        return erpmToRpm(getEscSensorData(i)->rpm);
     }
 #endif
     return 0;

src/main/osd/osd_warnings.c

@@ -273,7 +273,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
             const char motorNumber = '1' + i;
             // if everything is OK just display motor number else R, T or C
             char warnFlag = motorNumber;
-            if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) {
+            if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && erpmToRpm(escData->rpm) <= (uint32_t)osdConfig()->esc_rpm_alarm) {
                 warnFlag = 'R';
             }
             if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) {

src/main/sensors/esc_sensor.c

@@ -271,7 +271,7 @@ static uint8_t decodeEscFrame(void)
         frameStatus = ESC_SENSOR_FRAME_COMPLETE;
 
         if (escSensorMotor < 4) {
-            DEBUG_SET(DEBUG_ESC_SENSOR_RPM, escSensorMotor, calcEscRpm(escSensorData[escSensorMotor].rpm) / 10); // output actual rpm/10 to fit in 16bit signed.
+            DEBUG_SET(DEBUG_ESC_SENSOR_RPM, escSensorMotor, erpmToRpm(escSensorData[escSensorMotor].rpm) / 10); // output actual rpm/10 to fit in 16bit signed.
             DEBUG_SET(DEBUG_ESC_SENSOR_TMP, escSensorMotor, escSensorData[escSensorMotor].temperature);
         }
     } else {
@@ -361,8 +361,4 @@ void escSensorProcess(timeUs_t currentTimeUs)
     }
 }
 
-int calcEscRpm(int erpm)
-{
-    return (erpm * 100) / (motorConfig()->motorPoleCount / 2);
-}
 #endif

src/main/sensors/esc_sensor.h

@@ -54,5 +54,3 @@ void startEscDataRead(uint8_t *frameBuffer, uint8_t frameLength);
 uint8_t getNumberEscBytesRead(void);
 
 uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen);
-
-int calcEscRpm(int erpm);

src/main/telemetry/frsky_hub.c

@@ -44,6 +44,7 @@
 #include "drivers/sensor.h"
 #include "drivers/serial.h"
 #include "drivers/time.h"
+#include "drivers/dshot.h"
 
 #include "config/config.h"
 #include "fc/rc_controls.h"
@@ -189,7 +190,7 @@ static void sendThrottleOrBatterySizeAsRpm(void)
 #if defined(USE_ESC_SENSOR_TELEMETRY)
     escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
     if (escData) {
-        data = escData->dataAge < ESC_DATA_INVALID ? (calcEscRpm(escData->rpm) / 10) : 0;
+        data = escData->dataAge < ESC_DATA_INVALID ? (erpmToRpm(escData->rpm) / 10) : 0;
     }
 #else
     if (ARMING_FLAG(ARMED)) {

src/main/telemetry/smartport.c

@@ -45,6 +45,7 @@
 #include "drivers/compass/compass.h"
 #include "drivers/sensor.h"
 #include "drivers/time.h"
+#include "drivers/dshot.h"
 
 #include "config/config.h"
 #include "fc/controlrate_profile.h"
@@ -656,7 +657,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
             case FSSP_DATAID_RPM        :
                 escData = getEscSensorData(ESC_SENSOR_COMBINED);
                 if (escData != NULL) {
-                    smartPortSendPackage(id, calcEscRpm(escData->rpm));
+                    smartPortSendPackage(id, erpmToRpm(escData->rpm));
                     *clearToSend = false;
                 }
                 break;
@@ -670,7 +671,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
             case FSSP_DATAID_RPM8       :
                 escData = getEscSensorData(id - FSSP_DATAID_RPM1);
                 if (escData != NULL) {
-                    smartPortSendPackage(id, calcEscRpm(escData->rpm));
+                    smartPortSendPackage(id, erpmToRpm(escData->rpm));
                     *clearToSend = false;
                 }
                 break;

src/main/telemetry/srxl.c

@@ -193,16 +193,9 @@ uint16_t getMotorAveragePeriod(void)
 #endif
 
 #if defined(USE_DSHOT_TELEMETRY)
-    if (useDshotTelemetry) {
+    // Calculate this way when no rpm from esc data
-        uint16_t motors = getMotorCount();
+    if (useDshotTelemetry && rpm == 0) {
-
+        rpm = getDshotAverageRpm();
-        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