From 5769b3021ead06bb07e54007f8394c5d6f755db5 Mon Sep 17 00:00:00 2001 From: Jan Post Date: Sun, 3 Dec 2023 05:16:35 +0100 Subject: [PATCH] Dshot RPM Telemetry Refactoring (#13012) --- src/main/blackbox/blackbox.c | 2 +- src/main/build/debug.c | 2 - src/main/cli/cli.c | 4 +- src/main/common/time.h | 5 +- src/main/drivers/dshot.c | 138 ++++++++++++++++++++-------- src/main/drivers/dshot.h | 29 ++++-- src/main/fc/core.c | 4 +- src/main/fc/init.c | 6 ++ src/main/flight/mixer.c | 2 +- src/main/flight/rpm_filter.c | 35 +------ src/main/flight/rpm_filter.h | 1 - src/main/msp/msp.c | 8 +- src/main/osd/osd.c | 10 +- src/main/osd/osd_elements.c | 4 +- src/main/osd/osd_warnings.c | 2 +- src/main/sensors/esc_sensor.c | 3 +- src/main/sensors/gyro_init.c | 4 +- src/main/target/SITL/sitl.c | 1 + src/main/telemetry/frsky_hub.c | 2 +- src/main/telemetry/smartport.c | 4 +- src/main/telemetry/srxl.c | 3 +- src/test/unit/arming_prevention_unittest.cc | 1 + 22 files changed, 162 insertions(+), 108 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index d707e8fc9..880cdbb6e 100644 --- a/src/main/blackbox/blackbox.c +++ b/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 diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 9b5594a0a..0d32ec78a 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -18,8 +18,6 @@ * If not, see . */ -#include - #include "platform.h" #include "debug.h" diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 83a914cb4..9153f3b42 100644 --- a/src/main/cli/cli.c +++ b/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, diff --git a/src/main/common/time.h b/src/main/common/time.h index 29f4262bf..cd6a9e40a 100644 --- a/src/main/common/time.h +++ b/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 diff --git a/src/main/drivers/dshot.c b/src/main/drivers/dshot.c index fc686cdf7..87dd3c5a8 100644 --- a/src/main/drivers/dshot.c +++ b/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 -#include +#include #include +#include #include #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 diff --git a/src/main/drivers/dshot.h b/src/main/drivers/dshot.h index 1a91c9fcd..1d1e05742 100644 --- a/src/main/drivers/dshot.h +++ b/src/main/drivers/dshot.h @@ -20,6 +20,9 @@ #pragma once +#include +#include + #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); diff --git a/src/main/fc/core.c b/src/main/fc/core.c index ebeea263d..5ee46e679 100644 --- a/src/main/fc/core.c +++ b/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 diff --git a/src/main/fc/init.c b/src/main/fc/init.c index cd055c638..604dd5c41 100644 --- a/src/main/fc/init.c +++ b/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(); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6fac8650f..27c56e7f3 100644 --- a/src/main/flight/mixer.c +++ b/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 diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 54b8aade0..df665c7e3 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -19,7 +19,6 @@ */ -#include #include #include "platform.h" @@ -45,8 +44,6 @@ #include "rpm_filter.h" #define RPM_FILTER_DURATION_S 0.001f // Maximum duration allowed to update all RPM notches once -#define SECONDS_PER_MINUTE 60.0f -#define ERPM_PER_LSB 100.0f typedef struct rpmFilter_s { @@ -66,11 +63,6 @@ typedef struct rpmFilter_s { // Singleton FAST_DATA_ZERO_INIT static rpmFilter_t rpmFilter; -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; - // batch processing of RPM notches FAST_DATA_ZERO_INIT static int notchUpdatesPerIteration; FAST_DATA_ZERO_INIT static int motorIndex; @@ -81,7 +73,6 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs) { motorIndex = 0; harmonicIndex = 0; - minMotorFrequencyHz = 0; rpmFilter.numHarmonics = 0; // disable RPM Filtering // if bidirectional DShot is not available @@ -89,13 +80,6 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs) return; } - // init LPFs for RPM data - for (int i = 0; i < getMotorCount(); i++) { - pt1FilterInit(&motorFreqLpf[i], pt1FilterGain(config->rpm_filter_lpf_hz, looptimeUs * 1e-6f)); - } - - erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (motorConfig()->motorPoleCount / 2.0f); - // if RPM Filtering is configured to be off if (!config->rpm_filter_harmonics) { return; @@ -132,14 +116,8 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void) return; } - // update motor RPM data - minMotorFrequencyHz = FLT_MAX; - for (int motor = 0; motor < getMotorCount(); motor++) { - motorFrequencyHz[motor] = pt1FilterApply(&motorFreqLpf[motor], erpmToHz * getDshotTelemetry(motor)); - minMotorFrequencyHz = MIN(minMotorFrequencyHz, motorFrequencyHz[motor]); - if (motor < 4) { - DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequencyHz[motor]); - } + for (int motor = 0; motor < getMotorCount() && motor < DEBUG16_VALUE_COUNT; motor++) { + DEBUG_SET(DEBUG_RPM_FILTER, motor, lrintf(getMotorFrequencyHz(motor))); } if (!isRpmFilterEnabled()) { @@ -149,13 +127,13 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void) // update RPM notches for (int i = 0; i < notchUpdatesPerIteration; i++) { - // Only bother updating notches which can have an effect on filtered output + // Only bother updating notches which have an effect on filtered output if (rpmFilter.weights[harmonicIndex] > 0.0f) { // select current notch on ROLL biquadFilter_t *template = &rpmFilter.notch[0][motorIndex][harmonicIndex]; - const float frequencyHz = constrainf((harmonicIndex + 1) * motorFrequencyHz[motorIndex], rpmFilter.minHz, rpmFilter.maxHz); + const float frequencyHz = constrainf((harmonicIndex + 1) * getMotorFrequencyHz(motorIndex), rpmFilter.minHz, rpmFilter.maxHz); const float marginHz = frequencyHz - rpmFilter.minHz; float weight = 1.0f; @@ -213,9 +191,4 @@ bool isRpmFilterEnabled(void) return rpmFilter.numHarmonics > 0; } -float getMinMotorFrequency(void) -{ - return minMotorFrequencyHz; -} - #endif // USE_RPM_FILTER diff --git a/src/main/flight/rpm_filter.h b/src/main/flight/rpm_filter.h index dbf17f00a..409f9cfe5 100644 --- a/src/main/flight/rpm_filter.h +++ b/src/main/flight/rpm_filter.h @@ -30,4 +30,3 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs); void rpmFilterUpdate(void); float rpmFilterApply(const int axis, float value); bool isRpmFilterEnabled(void); -float getMinMotorFrequency(void); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 4b930ca3b..a9003ee51 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1123,7 +1123,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t // Added in API version 1.46 // Write CPU temp -#ifdef USE_ADC_INTERNAL +#ifdef USE_ADC_INTERNAL sbufWriteU16(dst, getCoreTemperatureCelsius()); #else sbufWriteU16(dst, 0); @@ -1236,7 +1236,7 @@ case MSP_NAME: #ifdef USE_DSHOT_TELEMETRY if (motorConfig()->dev.useDshotTelemetry) { - rpm = erpmToRpm(getDshotTelemetry(i)); + rpm = lrintf(getDshotRpm(i)); rpmDataAvailable = true; invalidPct = 10000; // 100.00% @@ -1272,7 +1272,7 @@ case MSP_NAME: if (featureIsEnabled(FEATURE_ESC_SENSOR)) { escSensorData_t *escData = getEscSensorData(i); if (!rpmDataAvailable) { // We want DSHOT telemetry RPM data (if available) to have precedence - rpm = erpmToRpm(escData->rpm); + rpm = lrintf(erpmToRpm(escData->rpm)); rpmDataAvailable = true; } escTemperature = escData->temperature; @@ -1495,7 +1495,7 @@ case MSP_NAME: sbufWriteU8(dst, getMotorCount()); for (int i = 0; i < getMotorCount(); i++) { sbufWriteU8(dst, dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE]); - sbufWriteU16(dst, getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount); + sbufWriteU16(dst, lrintf(getDshotRpm(i))); } } else diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index f4fb27190..08b416f46 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -599,12 +599,12 @@ static int32_t getAverageEscRpm(void) { #ifdef USE_ESC_SENSOR if (featureIsEnabled(FEATURE_ESC_SENSOR)) { - return erpmToRpm(osdEscDataCombined->rpm); + return lrintf(erpmToRpm(osdEscDataCombined->rpm)); } #endif #ifdef USE_DSHOT_TELEMETRY if (motorConfig()->dev.useDshotTelemetry) { - return getDshotAverageRpm(); + return lrintf(getDshotRpmAverage()); } #endif return 0; @@ -854,7 +854,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow) osdDisplayStatisticLabel(midCol, displayRow, osdConfig()->stat_show_cell_value ? "END AVG CELL" : "END BATTERY", buff); return true; - case OSD_STAT_BATTERY: + case OSD_STAT_BATTERY: { const uint16_t statsVoltage = getStatsVoltage(); osdPrintFloat(buff, SYM_NONE, statsVoltage / 100.0f, "", 2, true, SYM_VOLT); @@ -862,7 +862,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow) return true; } break; - + case OSD_STAT_MIN_RSSI: itoa(stats.min_rssi, buff, 10); strcat(buff, "%"); @@ -884,7 +884,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow) return true; } break; - + case OSD_STAT_WATT_HOURS_DRAWN: if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) { osdPrintFloat(buff, SYM_NONE, getWhDrawn(), "", 2, true, SYM_NONE); diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 648329427..df467fa23 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -266,12 +266,12 @@ static int getEscRpm(int i) { #ifdef USE_DSHOT_TELEMETRY if (motorConfig()->dev.useDshotTelemetry) { - return erpmToRpm(getDshotTelemetry(i)); + return lrintf(getDshotRpm(i)); } #endif #ifdef USE_ESC_SENSOR if (featureIsEnabled(FEATURE_ESC_SENSOR)) { - return erpmToRpm(getEscSensorData(i)->rpm); + return lrintf(erpmToRpm(getEscSensorData(i)->rpm)); } #endif return 0; diff --git a/src/main/osd/osd_warnings.c b/src/main/osd/osd_warnings.c index e8b29ebb6..d7748b8a9 100644 --- a/src/main/osd/osd_warnings.c +++ b/src/main/osd/osd_warnings.c @@ -282,7 +282,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 && erpmToRpm(escData->rpm) <= (uint32_t)osdConfig()->esc_rpm_alarm) { + if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && (uint32_t)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 ebd2666c2..06bed65e2 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -18,6 +18,7 @@ * If not, see . */ +#include #include #include #include @@ -271,7 +272,7 @@ static uint8_t decodeEscFrame(void) frameStatus = ESC_SENSOR_FRAME_COMPLETE; if (escSensorMotor < 4) { - 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_RPM, escSensorMotor, lrintf(erpmToRpm(escSensorData[escSensorMotor].rpm) / 10.0f)); // output actual rpm/10 to fit in 16bit signed. DEBUG_SET(DEBUG_ESC_SENSOR_TMP, escSensorMotor, escSensorData[escSensorMotor].temperature); } } else { diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 00c3abf51..4e6faaa10 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -711,8 +711,8 @@ void gyroSetTargetLooptime(uint8_t pidDenom) { activePidLoopDenom = pidDenom; if (gyro.sampleRateHz) { - gyro.sampleLooptime = 1e6 / gyro.sampleRateHz; - gyro.targetLooptime = activePidLoopDenom * 1e6 / gyro.sampleRateHz; + gyro.sampleLooptime = 1e6f / gyro.sampleRateHz; + gyro.targetLooptime = activePidLoopDenom * 1e6f / gyro.sampleRateHz; } else { gyro.sampleLooptime = 0; gyro.targetLooptime = 0; diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c index 4b9f7bf9b..37dfac14b 100644 --- a/src/main/target/SITL/sitl.c +++ b/src/main/target/SITL/sitl.c @@ -18,6 +18,7 @@ * If not, see . */ +#include #include #include #include diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 6c91319f3..7ea8393b0 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -192,7 +192,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 ? (erpmToRpm(escData->rpm) / 10) : 0; + data = escData->dataAge < ESC_DATA_INVALID ? lrintf(erpmToRpm(escData->rpm) / 10.0f) : 0; } #else if (ARMING_FLAG(ARMED)) { diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index b6c3e8c69..fd3ce3194 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -658,7 +658,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear case FSSP_DATAID_RPM : escData = getEscSensorData(ESC_SENSOR_COMBINED); if (escData != NULL) { - smartPortSendPackage(id, erpmToRpm(escData->rpm)); + smartPortSendPackage(id, lrintf(erpmToRpm(escData->rpm))); *clearToSend = false; } break; @@ -672,7 +672,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear case FSSP_DATAID_RPM8 : escData = getEscSensorData(id - FSSP_DATAID_RPM1); if (escData != NULL) { - smartPortSendPackage(id, erpmToRpm(escData->rpm)); + smartPortSendPackage(id, lrintf(erpmToRpm(escData->rpm))); *clearToSend = false; } break; diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index 319a1edf9..947e39207 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -18,6 +18,7 @@ * If not, see . */ +#include #include #include #include @@ -196,7 +197,7 @@ uint16_t getMotorAveragePeriod(void) #if defined(USE_DSHOT_TELEMETRY) // Calculate this way when no rpm from esc data if (useDshotTelemetry && rpm == 0) { - rpm = getDshotAverageRpm(); + rpm = lrintf(getDshotRpmAverage()); } #endif diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 8fcaa2c67..cc345147c 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#include #include extern "C" { -- 2.11.4.GIT