diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9e99eb946c..1f65037a73 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -111,7 +111,7 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig) } } - motorConfig->motorPolesCount = 14; // Most brushes motors that we use are 14 poles + motorConfig->motorPoleCount = 14; // Most brushes motors that we use are 14 poles } PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 5d437ef6d4..b0f4939593 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -101,7 +101,7 @@ typedef struct motorConfig_s { uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs - uint8_t motorPolesCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry + uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry } motorConfig_t; PG_DECLARE(motorConfig_t, motorConfig); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index a06ff7a02f..126a27a530 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -554,7 +554,7 @@ const clivalue_t valueTable[] = { { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmProtocol) }, { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 200, 32000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmRate) }, { "motor_pwm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmInversion) }, - { "motor_poles", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 4, 32 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, motorPolesCount) }, + { "motor_poles", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 4, UINT8_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, motorPoleCount) }, // PG_THROTTLE_CORRECTION_CONFIG { "thr_corr_value", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 150 }, PG_THROTTLE_CORRECTION_CONFIG, offsetof(throttleCorrectionConfig_t, throttle_correction_value) }, diff --git a/src/main/io/osd.c b/src/main/io/osd.c index fb206cb85c..9965be478b 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -673,7 +673,7 @@ static bool osdDrawSingleElement(uint8_t item) 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 && escData->rpm <= osdConfig()->esc_rpm_alarm) { + if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) { warnFlag = 'R'; } if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) { @@ -820,7 +820,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_ESC_RPM: if (feature(FEATURE_ESC_SENSOR)) { - tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : (escDataCombined->rpm * 100) / (motorConfig()->motorPolesCount / 2)); + tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : calcEscRpm(escDataCombined->rpm)); } break; #endif diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 55ffb4f2c7..9a3787b337 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -264,7 +264,7 @@ static uint8_t decodeEscFrame(void) frameStatus = ESC_SENSOR_FRAME_COMPLETE; - DEBUG_SET(DEBUG_ESC_SENSOR_RPM, escSensorMotor, (escSensorData[escSensorMotor].rpm * 10) / (motorConfig()->motorPolesCount / 2)); // output actual rpm/10 to fit in 16bit signed. + 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_TMP, escSensorMotor, escSensorData[escSensorMotor].temperature); } else { frameStatus = ESC_SENSOR_FRAME_FAILED; @@ -350,4 +350,9 @@ void escSensorProcess(timeUs_t currentTimeUs) break; } } + +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 62b8f730d0..ffc096105c 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -52,3 +52,5 @@ 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 04cd3c2190..1196fcb4cf 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -186,7 +186,7 @@ static void sendThrottleOrBatterySizeAsRpm(void) #if defined(USE_ESC_SENSOR) escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); if (escData) { - data = escData->dataAge < ESC_DATA_INVALID ? escData->rpm : 0; + data = escData->dataAge < ESC_DATA_INVALID ? (calcEscRpm(escData->rpm) / 10) : 0; } #else if (ARMING_FLAG(ARMED)) { diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index f8b286a9d6..317ff2b597 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -530,7 +530,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear case FSSP_DATAID_RPM : escData = getEscSensorData(ESC_SENSOR_COMBINED); if (escData != NULL) { - smartPortSendPackage(id, escData->rpm); + smartPortSendPackage(id, calcEscRpm(escData->rpm)); *clearToSend = false; } break; @@ -544,7 +544,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear case FSSP_DATAID_RPM8 : escData = getEscSensorData(id - FSSP_DATAID_RPM1); if (escData != NULL) { - smartPortSendPackage(id, escData->rpm); + smartPortSendPackage(id, calcEscRpm(escData->rpm)); *clearToSend = false; } break;