Merge pull request #1989 from mikeller/make_esc_feedback_more_resilient

Made ESC feedback and vbat calculation more resilient.

src/main/fc/fc_msp.c

@@ -737,7 +737,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_ANALOG:
-        sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
+        sbufWriteU8(dst, (uint8_t)constrain(getVbat(), 0, 255));
         sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
         sbufWriteU16(dst, rssi);
         if(batteryConfig()->multiwiiCurrentMeterOutput) {

src/main/fc/serial_cli.c

@@ -3164,7 +3164,7 @@ static void cliStatus(char *cmdline)
 
     cliPrintf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery - %s), CPU:%d%%\r\n",
         millis() / 1000,
-        vbat,
+        getVbat(),
         batteryCellCount,
         getBatteryStateString(),
         constrain(averageSystemLoadPercent, 0, 100)

src/main/io/dashboard.c

@@ -458,7 +458,7 @@ void showBatteryPage(void)
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 
     if (feature(FEATURE_VBAT)) {
-        tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", vbat / 10, vbat % 10, batteryCellCount);
+        tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getVbat() / 10, getVbat() % 10, batteryCellCount);
         padLineBuffer();
         i2c_OLED_set_line(rowIndex++);
         i2c_OLED_send_string(lineBuffer);

src/main/io/osd.c

@@ -161,7 +161,7 @@ static void osdDrawSingleElement(uint8_t item)
         case OSD_MAIN_BATT_VOLTAGE:
         {
             buff[0] = SYM_BATT_5;
-            sprintf(buff + 1, "%d.%1dV", vbat / 10, vbat % 10);
+            sprintf(buff + 1, "%d.%1dV", getVbat() / 10, getVbat() % 10);
             break;
         }
 
@@ -362,7 +362,7 @@ static void osdDrawSingleElement(uint8_t item)
 
         case OSD_POWER:
         {
-            sprintf(buff, "%dW", amperage * vbat / 1000);
+            sprintf(buff, "%dW", amperage * getVbat() / 1000);
             break;
         }
 
@@ -500,7 +500,7 @@ void osdUpdateAlarms(void)
     else
         pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG;
 
-    if (vbat <= (batteryWarningVoltage - 1))
+    if (getVbat() <= (batteryWarningVoltage - 1))
         pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG;
     else
         pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG;
@@ -556,8 +556,8 @@ static void osdUpdateStats(void)
     if (stats.max_speed < value)
         stats.max_speed = value;
 
-    if (stats.min_voltage > vbat)
+    if (stats.min_voltage > getVbat())
-        stats.min_voltage = vbat;
+        stats.min_voltage = getVbat();
 
     value = amperage / 100;
     if (stats.max_current < value)

src/main/rx/jetiexbus.c

@@ -526,7 +526,7 @@ void handleJetiExBusTelemetry(void)
         }
 
         if((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) {
-            jetiExSensors[EX_VOLTAGE].value = vbat;
+            jetiExSensors[EX_VOLTAGE].value = getVbat();
             jetiExSensors[EX_CURRENT].value = amperage;
             jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt;
             jetiExSensors[EX_CAPACITY].value = mAhDrawn;

src/main/sensors/battery.c

@@ -50,6 +50,8 @@
 
 #define ADCVREF 3300   // in mV
 
+#define MAX_ESC_BATTERY_AGE 10
+
 // Battery monitoring stuff
 uint8_t batteryCellCount;
 uint16_t batteryWarningVoltage;
@@ -85,8 +87,8 @@ static void updateBatteryVoltage(void)
 
     #ifdef USE_ESC_SENSOR
     if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) {
-        escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
+        escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
-        vbatLatest = escData.stale ? 0 : escData.voltage / 10;
+        vbatLatest = escData->dataAge <= MAX_ESC_BATTERY_AGE ? escData->voltage / 10 : 0;
         if (debugMode == DEBUG_BATTERY) {
             debug[0] = -1;
         }
@@ -294,10 +296,10 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
         case CURRENT_SENSOR_ESC:
             #ifdef USE_ESC_SENSOR
             if (feature(FEATURE_ESC_SENSOR)) {
-                escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
+                escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
-                if (!escData.stale) {
+                if (escData->dataAge <= MAX_ESC_BATTERY_AGE) {
-                    amperageLatest = escData.current;
+                    amperageLatest = escData->current;
-                    mAhDrawn = escData.consumption;
+                    mAhDrawn = escData->consumption;
                 } else {
                     amperageLatest = 0;
                     mAhDrawn = 0;
@@ -340,3 +342,8 @@ uint8_t calculateBatteryPercentage(void)
 
     return batteryPercentage;
 }
+
+uint16_t getVbat(void)
+{
+    return vbat;
+}

src/main/sensors/battery.h

@@ -70,7 +70,6 @@ typedef enum {
     BATTERY_NOT_PRESENT
 } batteryState_e;
 
-extern uint16_t vbat;
 extern uint16_t vbatRaw;
 extern uint16_t vbatLatest;
 extern uint8_t batteryCellCount;
@@ -91,3 +90,4 @@ int32_t currentMeterToCentiamps(uint16_t src);
 
 float calculateVbatPidCompensation(void);
 uint8_t calculateBatteryPercentage(void);
+uint16_t getVbat(void);

src/main/sensors/esc_sensor.c

@@ -73,19 +73,20 @@ set debug_mode = DEBUG_ESC_TELEMETRY in cli
 enum {
     DEBUG_ESC_MOTOR_INDEX = 0,
     DEBUG_ESC_NUM_TIMEOUTS = 1,
-    DEBUG_ESC_TEMPERATURE = 2,
+    DEBUG_ESC_NUM_CRC_ERRORS = 2,
-    DEBUG_ESC_RPM = 3
+    DEBUG_ESC_DATA_AGE = 3,
 };
 
 typedef enum {
-    ESC_SENSOR_FRAME_PENDING = 1 << 0,     // 1
+    ESC_SENSOR_FRAME_PENDING = 0,
-    ESC_SENSOR_FRAME_COMPLETE = 1 << 1     // 2
+    ESC_SENSOR_FRAME_COMPLETE = 1,
+    ESC_SENSOR_FRAME_FAILED = 2
 } escTlmFrameState_t;
 
 typedef enum {
-    ESC_SENSOR_TRIGGER_WAIT = 0,
+    ESC_SENSOR_TRIGGER_STARTUP = 0,
-    ESC_SENSOR_TRIGGER_READY = 1 << 0,     // 1
+    ESC_SENSOR_TRIGGER_READY = 1,
-    ESC_SENSOR_TRIGGER_PENDING = 1 << 1,   // 2
+    ESC_SENSOR_TRIGGER_PENDING = 2
 } escSensorTriggerState_t;
 
 #define ESC_SENSOR_BAUDRATE 115200
@@ -96,71 +97,79 @@ typedef enum {
 static bool tlmFrameDone = false;
 static uint8_t tlm[ESC_SENSOR_BUFFSIZE] = { 0, };
 static uint8_t tlmFramePosition = 0;
+
 static serialPort_t *escSensorPort = NULL;
+
 static escSensorData_t escSensorData[MAX_SUPPORTED_MOTORS];
-static uint32_t escTriggerTimestamp = -1;
-static uint32_t escLastResponseTimestamp;
-static uint8_t timeoutRetryCount = 0;
-static uint8_t totalRetryCount = 0;
 
+static escSensorTriggerState_t escSensorTriggerState = ESC_SENSOR_TRIGGER_STARTUP;
+static uint32_t escTriggerTimestamp;
 static uint8_t escSensorMotor = 0;      // motor index
-static bool escSensorEnabled = false;
-static escSensorTriggerState_t escSensorTriggerState = ESC_SENSOR_TRIGGER_WAIT;
 
-static void escSensorDataReceive(uint16_t c);
+static escSensorData_t combinedEscSensorData;
-static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed);
+static bool combinedDataNeedsUpdate = true;
-static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen);
+
-static void selectNextMotor(void);
+static uint16_t totalTimeoutCount = 0;
+static uint16_t totalCrcErrorCount = 0;
 
 bool isEscSensorActive(void)
 {
-    return escSensorEnabled;
+    return escSensorPort != NULL;
 }
 
-escSensorData_t getEscSensorData(uint8_t motorNumber)
+escSensorData_t *getEscSensorData(uint8_t motorNumber)
 {
     if (motorNumber < getMotorCount()) {
-        return escSensorData[motorNumber];
+        return &escSensorData[motorNumber];
-    }
+    } else if (motorNumber == ESC_SENSOR_COMBINED) {
+        if (combinedDataNeedsUpdate) {
+            combinedEscSensorData.dataAge = 0;
+            combinedEscSensorData.temperature = 0;
+            combinedEscSensorData.voltage = 0;
+            combinedEscSensorData.current = 0;
+            combinedEscSensorData.consumption = 0;
+            combinedEscSensorData.rpm = 0;
 
-    escSensorData_t combinedEscSensorData = {
-        .stale = true,
-        .temperature = 0,
-        .voltage = 0,
-        .current = 0,
-        .consumption = 0,
-        .rpm = 0
-    };
-    if (motorNumber == ESC_SENSOR_COMBINED) {
-        unsigned int activeSensors = 0;
             for (int i = 0; i < getMotorCount(); i = i + 1) {
-            if (!escSensorData[i].stale) {
+                combinedEscSensorData.dataAge = MAX(combinedEscSensorData.dataAge, escSensorData[i].dataAge);
                 combinedEscSensorData.temperature = MAX(combinedEscSensorData.temperature, escSensorData[i].temperature);
                 combinedEscSensorData.voltage += escSensorData[i].voltage;
                 combinedEscSensorData.current += escSensorData[i].current;
                 combinedEscSensorData.consumption += escSensorData[i].consumption;
                 combinedEscSensorData.rpm += escSensorData[i].rpm;
-                activeSensors = activeSensors + 1;
+            }
+
+            combinedEscSensorData.voltage = combinedEscSensorData.voltage / getMotorCount();
+            combinedEscSensorData.rpm = combinedEscSensorData.rpm / getMotorCount();
+
+            combinedDataNeedsUpdate = false;
+
+            DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_DATA_AGE, combinedEscSensorData.dataAge);
+        }
+
+        return &combinedEscSensorData;
+    } else {
+        return NULL;
     }
 }
 
-        if (activeSensors > 0) {
+// Receive ISR callback
-            combinedEscSensorData.stale = false;
+static void escSensorDataReceive(uint16_t c)
-            combinedEscSensorData.voltage = combinedEscSensorData.voltage / activeSensors;
-            combinedEscSensorData.rpm = combinedEscSensorData.rpm / activeSensors;
-
-            DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_TEMPERATURE, combinedEscSensorData.temperature);
-            DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_RPM, combinedEscSensorData.rpm);
-        }
-    }
-
-    return combinedEscSensorData;
-}
-
-static void resetEscSensorData(void)
 {
-    for (int i; i < MAX_SUPPORTED_MOTORS; i = i + 1) {
+    // KISS ESC sends some data during startup, ignore this for now (maybe future use)
-        escSensorData[i].stale = true;
+    // startup data could be firmware version and serialnumber
+
+    if (escSensorTriggerState == ESC_SENSOR_TRIGGER_STARTUP || tlmFrameDone) {
+        return;
+    }
+
+    tlm[tlmFramePosition] = (uint8_t)c;
+
+    if (tlmFramePosition == ESC_SENSOR_BUFFSIZE - 1) {
+        tlmFrameDone = true;
+        tlmFramePosition = 0;
+    } else {
+        tlmFramePosition++;
     }
 }
 
@@ -176,143 +185,13 @@ bool escSensorInit(void)
     // Initialize serial port
     escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_ESC_SENSOR, escSensorDataReceive, ESC_SENSOR_BAUDRATE, MODE_RX, options);
 
-    if (escSensorPort) {
+    for (int i; i < MAX_SUPPORTED_MOTORS; i = i + 1) {
-        escSensorEnabled = true;
+        escSensorData[i].dataAge = ESC_DATA_INVALID;
     }
 
-    resetEscSensorData();
-
     return escSensorPort != NULL;
 }
 
-static void freeEscSensorPort(void)
-{
-    closeSerialPort(escSensorPort);
-    escSensorPort = NULL;
-    escSensorEnabled = false;
-}
-
-// Receive ISR callback
-static void escSensorDataReceive(uint16_t c)
-{
-    // KISS ESC sends some data during startup, ignore this for now (maybe future use)
-    // startup data could be firmware version and serialnumber
-
-    if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) return;
-
-    tlm[tlmFramePosition] = (uint8_t)c;
-
-    if (tlmFramePosition == ESC_SENSOR_BUFFSIZE - 1) {
-        tlmFrameDone = true;
-        tlmFramePosition = 0;
-    } else {
-        tlmFramePosition++;
-    }
-}
-
-static uint8_t escSensorFrameStatus(void)
-{
-    uint8_t frameStatus = ESC_SENSOR_FRAME_PENDING;
-    uint16_t chksum, tlmsum;
-
-    if (!tlmFrameDone) {
-        return frameStatus;
-    }
-
-    tlmFrameDone = false;
-
-    // Get CRC8 checksum
-    chksum = get_crc8(tlm, ESC_SENSOR_BUFFSIZE - 1);
-    tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1];     // last byte contains CRC value
-
-    if (chksum == tlmsum) {
-        escSensorData[escSensorMotor].stale = false;
-        escSensorData[escSensorMotor].temperature = tlm[0];
-        escSensorData[escSensorMotor].voltage = tlm[1] << 8 | tlm[2];
-        escSensorData[escSensorMotor].current = tlm[3] << 8 | tlm[4];
-        escSensorData[escSensorMotor].consumption = tlm[5] << 8 | tlm[6];
-        escSensorData[escSensorMotor].rpm = tlm[7] << 8 | tlm[8];
-
-        frameStatus = ESC_SENSOR_FRAME_COMPLETE;
-    }
-
-    return frameStatus;
-}
-
-void escSensorProcess(timeUs_t currentTimeUs)
-{
-    const timeMs_t currentTimeMs = currentTimeUs / 1000;
-
-    if (!escSensorEnabled) {
-        return;
-    }
-
-    // Wait period of time before requesting telemetry (let the system boot first)
-    if (currentTimeMs < ESC_BOOTTIME) {
-        return;
-    }
-    else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) {
-        // Ready for starting requesting telemetry
-        escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
-        escSensorMotor = 0;
-        escTriggerTimestamp = currentTimeMs;
-        escLastResponseTimestamp = escTriggerTimestamp;
-    }
-    else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) {
-        DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1);
-
-        motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
-        motor->requestTelemetry = true;
-        escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;
-    }
-    else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_PENDING) {
-
-        if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs) {
-            // ESC did not repond in time, retry
-            timeoutRetryCount++;
-            escTriggerTimestamp = currentTimeMs;
-            escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
-
-            if (timeoutRetryCount == 4) {
-                // Not responding after 3 times, skip motor
-                escSensorData[escSensorMotor].stale = true;
-                selectNextMotor();
-            }
-
-            DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_TIMEOUTS, ++totalRetryCount);
-        }
-
-        // Get received frame status
-        uint8_t state = escSensorFrameStatus();
-
-        if (state == ESC_SENSOR_FRAME_COMPLETE) {
-            selectNextMotor();
-            escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
-            escLastResponseTimestamp = currentTimeMs;
-        }
-    }
-
-    if (escLastResponseTimestamp + 10000 < currentTimeMs) {
-        // ESCs did not respond for 10 seconds
-        // Disable ESC telemetry and reset voltage and current to let the use know something is wrong
-        freeEscSensorPort();
-
-        resetEscSensorData();
-    }
-}
-
-static void selectNextMotor(void)
-{
-    escSensorMotor++;
-    if (escSensorMotor == getMotorCount()) {
-        escSensorMotor = 0;
-    }
-    timeoutRetryCount = 0;
-    escTriggerTimestamp = millis();
-}
-
-//-- CRC
-
 static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
 {
     uint8_t crc_u = crc;
@@ -332,4 +211,110 @@ static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen)
     return (crc);
 }
 
+static uint8_t decodeEscFrame(void)
+{
+    if (!tlmFrameDone) {
+        return ESC_SENSOR_FRAME_PENDING;
+    }
+
+    // Get CRC8 checksum
+    uint16_t chksum = get_crc8(tlm, ESC_SENSOR_BUFFSIZE - 1);
+    uint16_t tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1];     // last byte contains CRC value
+    uint8_t frameStatus;
+    if (chksum == tlmsum) {
+        escSensorData[escSensorMotor].dataAge = 0;
+        escSensorData[escSensorMotor].temperature = tlm[0];
+        escSensorData[escSensorMotor].voltage = tlm[1] << 8 | tlm[2];
+        escSensorData[escSensorMotor].current = tlm[3] << 8 | tlm[4];
+        escSensorData[escSensorMotor].consumption = tlm[5] << 8 | tlm[6];
+        escSensorData[escSensorMotor].rpm = tlm[7] << 8 | tlm[8];
+
+        combinedDataNeedsUpdate = true;
+
+        frameStatus = ESC_SENSOR_FRAME_COMPLETE;
+    } else {
+        frameStatus = ESC_SENSOR_FRAME_FAILED;
+    }
+
+    tlmFrameDone = false;
+
+    return frameStatus;
+}
+
+static void increaseDataAge(void)
+{
+    if (escSensorData[escSensorMotor].dataAge < ESC_DATA_INVALID) {
+        escSensorData[escSensorMotor].dataAge++;
+
+        combinedDataNeedsUpdate = true;
+    }
+}
+
+static void selectNextMotor(void)
+{
+    escSensorMotor++;
+    if (escSensorMotor == getMotorCount()) {
+        escSensorMotor = 0;
+    }
+}
+
+void escSensorProcess(timeUs_t currentTimeUs)
+{
+    const timeMs_t currentTimeMs = currentTimeUs / 1000;
+
+    if (!escSensorPort) {
+        return;
+    }
+
+    switch (escSensorTriggerState) {
+        case ESC_SENSOR_TRIGGER_STARTUP:
+            // Wait period of time before requesting telemetry (let the system boot first)
+            if (currentTimeMs >= ESC_BOOTTIME) {
+                escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
+            }
+
+            break;
+        case ESC_SENSOR_TRIGGER_READY:
+            escTriggerTimestamp = currentTimeMs;
+
+            motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
+            motor->requestTelemetry = true;
+            escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;
+
+            DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1);
+
+            break;
+        case ESC_SENSOR_TRIGGER_PENDING:
+            if (currentTimeMs < escTriggerTimestamp + ESC_REQUEST_TIMEOUT) {
+                uint8_t state = decodeEscFrame();
+                switch (state) {
+                    case ESC_SENSOR_FRAME_COMPLETE:
+                        selectNextMotor();
+                        escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
+
+                        break;
+                    case ESC_SENSOR_FRAME_FAILED:
+                        increaseDataAge();
+
+                        selectNextMotor();
+                        escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
+
+                        DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_CRC_ERRORS, ++totalCrcErrorCount);
+                        break;
+                    case ESC_SENSOR_FRAME_PENDING:
+                        break;
+                }
+            } else {
+                // Move on to next ESC, we'll come back to this one
+                increaseDataAge();
+
+                selectNextMotor();
+                escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
+
+                DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_TIMEOUTS, ++totalTimeoutCount);
+            }
+
+            break;
+    }
+}
 #endif

src/main/sensors/esc_sensor.h

@@ -20,7 +20,7 @@
 #include "common/time.h"
 
 typedef struct {
-    bool stale;
+    uint8_t dataAge;
     int8_t temperature;
     int16_t voltage;
     int16_t current;
@@ -28,10 +28,12 @@ typedef struct {
     int16_t rpm;
 } escSensorData_t;
 
+#define ESC_DATA_INVALID 255
+
 bool escSensorInit(void);
 void escSensorProcess(timeUs_t currentTime);
 
 #define ESC_SENSOR_COMBINED 255
 
-escSensorData_t getEscSensorData(uint8_t motorNumber);
+escSensorData_t *getEscSensorData(uint8_t motorNumber);
 

src/main/target/COLIBRI_RACE/i2c_bst.c

@@ -677,7 +677,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
 #endif
             break;
         case BST_ANALOG:
-            bstWrite8((uint8_t)constrain(vbat, 0, 255));
+            bstWrite8((uint8_t)constrain(getVbat(), 0, 255));
             bstWrite16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
             bstWrite16(rssi);
             if(batteryConfig()->multiwiiCurrentMeterOutput) {

src/main/telemetry/crsf.c

@@ -175,7 +175,7 @@ void crsfFrameBatterySensor(sbuf_t *dst)
     // use sbufWrite since CRC does not include frame length
     sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
     crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
-    crsfSerialize16(dst, vbat); // vbat is in units of 0.1V
+    crsfSerialize16(dst, getVbat()); // vbat is in units of 0.1V
 #ifdef CLEANFLIGHT
     const amperageMeter_t *amperageMeter = getAmperageMeter(batteryConfig()->amperageMeterSource);
     const int16_t amperage = constrain(amperageMeter->amperage, -0x8000, 0x7FFF) / 10; // send amperage in 0.01 A steps, range is -320A to 320A

src/main/telemetry/frsky.c

@@ -205,8 +205,8 @@ static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadba
     UNUSED(rxConfig);
     UNUSED(deadband3d_throttle);
 
-    escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
+    escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
-    serialize16(escData.stale ? 0 : escData.rpm);
+    serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->rpm : 0);
 #else
     if (ARMING_FLAG(ARMED)) {
         throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
@@ -224,8 +224,8 @@ static void sendTemperature1(void)
 {
     sendDataHead(ID_TEMPRATURE1);
 #if defined(USE_ESC_SENSOR)
-    escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
+    escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
-    serialize16(escData.stale ? 0 : escData.temperature);
+    serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->temperature : 0);
 #elif defined(BARO)
     serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf
 #else
@@ -388,7 +388,7 @@ static void sendVoltage(void)
      * The actual value sent for cell voltage has resolution of 0.002 volts
      * Since vbat has resolution of 0.1 volts it has to be multiplied by 50
      */
-    cellVoltage = ((uint32_t)vbat * 100 + batteryCellCount) / (batteryCellCount * 2);
+    cellVoltage = ((uint32_t)getVbat() * 100 + batteryCellCount) / (batteryCellCount * 2);
 
     // Cell number is at bit 9-12
     payload = (currentCell << 4);
@@ -416,9 +416,9 @@ static void sendVoltageAmp(void)
          * Use new ID 0x39 to send voltage directly in 0.1 volts resolution
          */
         sendDataHead(ID_VOLTAGE_AMP);
-        serialize16(vbat);
+        serialize16(getVbat());
     } else {
-        uint16_t voltage = (vbat * 110) / 21;
+        uint16_t voltage = (getVbat() * 110) / 21;
         uint16_t vfasVoltage;
         if (telemetryConfig->frsky_vfas_cell_voltage) {
             vfasVoltage = voltage / batteryCellCount;

src/main/telemetry/hott.c

@@ -242,10 +242,10 @@ static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage)
 
 static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
 {
-    hottEAMMessage->main_voltage_L = vbat & 0xFF;
+    hottEAMMessage->main_voltage_L = getVbat() & 0xFF;
-    hottEAMMessage->main_voltage_H = vbat >> 8;
+    hottEAMMessage->main_voltage_H = getVbat() >> 8;
-    hottEAMMessage->batt1_voltage_L = vbat & 0xFF;
+    hottEAMMessage->batt1_voltage_L = getVbat() & 0xFF;
-    hottEAMMessage->batt1_voltage_H = vbat >> 8;
+    hottEAMMessage->batt1_voltage_H = getVbat() >> 8;
 
     updateAlarmBatteryStatus(hottEAMMessage);
 }

src/main/telemetry/ltm.c

@@ -190,7 +190,7 @@ static void ltm_sframe(void)
     if (failsafeIsActive())
         lt_statemode |= 2;
     ltm_initialise_packet('S');
-    ltm_serialise_16(vbat * 100);    //vbat converted to mv
+    ltm_serialise_16(getVbat() * 100);    //vbat converted to mv
     ltm_serialise_16(0);             //  current, not implemented
     ltm_serialise_8((uint8_t)((rssi * 254) / 1023));        // scaled RSSI (uchar)
     ltm_serialise_8(0);              // no airspeed

src/main/telemetry/mavlink.c

@@ -223,7 +223,7 @@ void mavlinkSendSystemStatus(void)
         // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
         0,
         // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
-        feature(FEATURE_VBAT) ? vbat * 100 : 0,
+        feature(FEATURE_VBAT) ? getVbat() * 100 : 0,
         // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
         feature(FEATURE_VBAT) ? amperage : -1,
         // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery

src/main/telemetry/smartport.c

@@ -639,9 +639,9 @@ void handleSmartPortTelemetry(void)
                 if (feature(FEATURE_VBAT) && batteryCellCount > 0) {
                     uint16_t vfasVoltage;
                     if (telemetryConfig->frsky_vfas_cell_voltage) {
-                        vfasVoltage = vbat / batteryCellCount;
+                        vfasVoltage = getVbat() / batteryCellCount;
                     } else {
-                        vfasVoltage = vbat;
+                        vfasVoltage = getVbat();
                     }
                     smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts
                     smartPortHasRequest = 0;
@@ -810,7 +810,7 @@ void handleSmartPortTelemetry(void)
 #endif
             case FSSP_DATAID_A4         :
                 if (feature(FEATURE_VBAT) && batteryCellCount > 0) {
-                    smartPortSendPackage(id, vbat * 10 / batteryCellCount ); // given in 0.1V, convert to volts
+                    smartPortSendPackage(id, getVbat() * 10 / batteryCellCount ); // given in 0.1V, convert to volts
                     smartPortHasRequest = 0;
                 }
                 break;

src/main/telemetry/srxl.c

@@ -161,7 +161,7 @@ void srxlFrameRpm(sbuf_t *dst)
     srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_RPM);
     srxlSerialize8(dst, SRXL_FRAMETYPE_SID);
     srxlSerialize16(dst, 0xFFFF); // pulse leading edges
-    srxlSerialize16(dst, vbat * 10);   // vbat is in units of 0.1V
+    srxlSerialize16(dst, getVbat() * 10);   // vbat is in units of 0.1V
     srxlSerialize16(dst, 0x7FFF); // temperature
     srxlSerialize8(dst, 0xFF);    // dbmA
     srxlSerialize8(dst, 0xFF);    // dbmB
@@ -200,8 +200,8 @@ void srxlFramePowerBox(sbuf_t *dst)
 {
     srxlSerialize8(dst, SRXL_FRAMETYPE_POWERBOX);
     srxlSerialize8(dst, SRXL_FRAMETYPE_SID);
-    srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V - vbat1
+    srxlSerialize16(dst, getVbat() * 10); // vbat is in units of 0.1V - vbat1
-    srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V - vbat2
+    srxlSerialize16(dst, getVbat() * 10); // vbat is in units of 0.1V - vbat2
     srxlSerialize16(dst, amperage / 10);
     srxlSerialize16(dst, 0xFFFF);
 

src/test/unit/telemetry_crsf_unittest.cc

@@ -54,6 +54,7 @@ extern "C" {
     #include "flight/gps_conversion.h"
 
     bool airMode;
+    uint16_t vbat;
     serialPort_t *telemetrySharedPort;
 }
 
@@ -282,7 +283,6 @@ uint16_t GPS_distanceToHome;        // distance to home point in meters
 uint16_t GPS_altitude;              // altitude in m
 uint16_t GPS_speed;                 // speed in 0.1m/s
 uint16_t GPS_ground_course = 0;     // degrees * 10
-uint16_t vbat;
 
 int32_t amperage;
 int32_t mAhDrawn;
@@ -314,6 +314,6 @@ uint8_t calculateBatteryCapacityRemainingPercentage(void) {return 67;}
 uint8_t calculateBatteryPercentage(void) {return 67;}
 batteryState_e getBatteryState(void) {return BATTERY_OK;}
 bool isAirmodeActive(void) {return airMode;}
-
+uint16_t getVbat(void) { return vbat; }
 }
 

src/test/unit/telemetry_hott_unittest.cc

@@ -165,8 +165,8 @@ int32_t GPS_coord[2];
 uint16_t GPS_speed;                 // speed in 0.1m/s
 uint16_t GPS_distanceToHome;        // distance to home point in meters
 uint16_t GPS_altitude;              // altitude in 0.1m
-uint16_t vbat;
 int16_t GPS_directionToHome;        // direction to home or hol point in degrees
+uint16_t vbat;
 
 int32_t amperage;
 int32_t mAhDrawn;
@@ -256,5 +256,9 @@ batteryState_e getBatteryState(void)
 	return BATTERY_OK;
 }
 
+uint16_t getVbat(void)
+{
+    return vbat;
+}
 }