diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index bc77c281c79..a06814a6846 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -84,7 +84,7 @@ PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, .listenOnly = SETTING_ESC_SENSOR_LISTEN_ONLY_DEFAULT, ); -static int getTelemetryMotorCount(void) +int getTelemetryMotorCount(void) { if (escSensorConfig()->listenOnly) { return 1; diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index a5958c85c16..6bccd340960 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -48,3 +48,4 @@ void escSensorUpdate(timeUs_t currentTimeUs); escSensorData_t * escSensorGetData(void); escSensorData_t * getEscTelemetry(uint8_t esc); uint32_t computeRpm(int16_t erpm); +int getTelemetryMotorCount(void); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index e8214fc8c1f..686d2b1cbc5 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -335,7 +335,7 @@ int24_t rpm_value[]; // 1 - 19 RPM values with negative ones representing static bool crsfRpm(sbuf_t *dst) { const uint8_t MAX_CRSF_RPM_VALUES = 19; // CRSF protocol limit: 1-19 RPM values - uint8_t motorCount = getMotorCount(); + uint8_t motorCount = getTelemetryMotorCount(); if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { // Enforce protocol limit @@ -350,7 +350,7 @@ static bool crsfRpm(sbuf_t *dst) for (uint8_t i = 0; i < motorCount; i++) { const escSensorData_t *escState = getEscTelemetry(i); - crsfSerialize24(dst, (escState) ? escState->rpm : 0); + crsfSerialize24(dst, escState->dataAge <= ESC_DATA_MAX_AGE ? escState->rpm : 0); } return true; } @@ -371,11 +371,11 @@ static bool crsfTemperature(sbuf_t *dst) int16_t temperatures[20]; #ifdef USE_ESC_SENSOR - uint8_t motorCount = getMotorCount(); + uint8_t motorCount = getTelemetryMotorCount(); if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { for (uint8_t i = 0; i < motorCount && tempCount < MAX_CRSF_TEMPS; i++) { const escSensorData_t *escState = getEscTelemetry(i); - temperatures[tempCount++] = (escState) ? escState->temperature * 10 : TEMPERATURE_INVALID_VALUE; + temperatures[tempCount++] = escState->dataAge <= ESC_DATA_MAX_AGE ? (int16_t)(escState->temperature * 10) : TEMPERATURE_INVALID_VALUE; } } #endif