From 0dd6d4d47a0a642511729e0f618238753657377c Mon Sep 17 00:00:00 2001 From: iso9660 Date: Mon, 1 Aug 2022 23:51:11 +0200 Subject: [PATCH] Unified all eRPM calculations into one single function Fixed some review findings Unified dshot average rpm calculations into one single function Renamed calcEscRpm to erpmToRpm, and moved function to dshot.c Removed unused esc_sensor.h header file from dshot.c Removed esc_sensor.h header from modules that no longer needs it Average RPM calculated by demmand only when rpm data is updated Renamed rpm to averageRpm and fixed a bug Update average rpm when telemetry data is received Removed blank line Fixed review findings Fixed return values for erpmToRpm and getDshotAverageRpm so rpm value doesn't truncate Restored osd_esc_rmp_alarm setting. This setting is used to set an alarm to notify when rpm goes down a specified threshold. Rpm can go over 109krpm (1s 26kv motors setup), but a low rpm alarm doesn't have to be set to a so high value so at this time [0-32767] seems an acceptable range Rebased to master --- src/main/cli/cli.c | 8 ++++---- src/main/drivers/dshot.c | 13 +++++++++++++ src/main/drivers/dshot.h | 3 +++ src/main/drivers/dshot_bitbang.c | 15 +++++++++++++++ src/main/drivers/pwm_output_dshot_shared.c | 10 ++++++++++ src/main/msp/msp.c | 5 ++--- src/main/osd/osd.c | 18 ++++++------------ src/main/osd/osd_elements.c | 5 ++--- src/main/osd/osd_warnings.c | 2 +- src/main/sensors/esc_sensor.c | 6 +----- src/main/sensors/esc_sensor.h | 2 -- src/main/telemetry/frsky_hub.c | 3 ++- src/main/telemetry/smartport.c | 5 +++-- src/main/telemetry/srxl.c | 13 +++---------- 14 files changed, 65 insertions(+), 43 deletions(-) diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 4a9390bffb..c6e1aabe46 100644 --- a/src/main/cli/cli.c +++ b/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, - 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); + erpm * 100, rpm, rpm / 60); #ifdef USE_DSHOT_TELEMETRY_STATS if (isDshotMotorTelemetryActive(i)) { diff --git a/src/main/drivers/dshot.c b/src/main/drivers/dshot.c index e206022b01..75c8cec1b7 100644 --- a/src/main/drivers/dshot.c +++ b/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 diff --git a/src/main/drivers/dshot.h b/src/main/drivers/dshot.h index 2d0094ead2..3ba7b1e2a3 100644 --- a/src/main/drivers/dshot.h +++ b/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); diff --git a/src/main/drivers/dshot_bitbang.c b/src/main/drivers/dshot_bitbang.c index 78b968f5b8..42dd8c04a4 100644 --- a/src/main/drivers/dshot_bitbang.c +++ b/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++) { diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/main/drivers/pwm_output_dshot_shared.c index 3952eda10c..b438b57f4a 100644 --- a/src/main/drivers/pwm_output_dshot_shared.c +++ b/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; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 694e9dff10..a7f13a8b81 100644 --- a/src/main/msp/msp.c +++ b/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; diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index 317b54fedc..a5dd7e08e5 100644 --- a/src/main/osd/osd.c +++ b/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; diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 660fd0d471..b07ebc69a3 100644 --- a/src/main/osd/osd_elements.c +++ b/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; diff --git a/src/main/osd/osd_warnings.c b/src/main/osd/osd_warnings.c index 452b477bd8..ad0fe3a728 100644 --- a/src/main/osd/osd_warnings.c +++ b/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) { diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 5444766f62..ebd2666c20 100644 --- a/src/main/sensors/esc_sensor.c +++ b/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 diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index bbee665f63..fd9f52b6d7 100644 --- a/src/main/sensors/esc_sensor.h +++ b/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); diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index b5b3ed3150..eeffd33a3a 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/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)) { diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 16b2e8ed31..bc00bc2606 100644 --- a/src/main/telemetry/smartport.c +++ b/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; diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index ec443aa25a..12fe832242 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -193,16 +193,9 @@ uint16_t getMotorAveragePeriod(void) #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 - } + // Calculate this way when no rpm from esc data + if (useDshotTelemetry && rpm == 0) { + rpm = getDshotAverageRpm(); } #endif