Merge pull request #5135 from codecae/battery_telemetry_init_fix

Corrected issue with telemetry init when battery cell estimation is undefined

src/main/sensors/battery.c

@@ -450,9 +450,9 @@ void batteryUpdateAlarms(void)
     }
 }
 
-bool isBatteryVoltageAvailable(void)
+bool isBatteryVoltageConfigured(void)
 {
-    return batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0;
+    return batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE;
 }
 
 uint16_t getBatteryVoltage(void)
@@ -475,7 +475,7 @@ uint16_t getBatteryAverageCellVoltage(void)
     return voltageMeter.filtered / batteryCellCount;
 }
 
-bool isAmperageAvailable(void)
+bool isAmperageConfigured(void)
 {
     return batteryConfig()->currentMeterSource != CURRENT_METER_NONE;
 }

src/main/sensors/battery.h

@@ -77,13 +77,13 @@ struct rxConfig_s;
 
 float calculateVbatPidCompensation(void);
 uint8_t calculateBatteryPercentageRemaining(void);
-bool isBatteryVoltageAvailable(void);
+bool isBatteryVoltageConfigured(void);
 uint16_t getBatteryVoltage(void);
 uint16_t getBatteryVoltageLatest(void);
 uint8_t getBatteryCellCount(void);
 uint16_t getBatteryAverageCellVoltage(void);
 
-bool isAmperageAvailable(void);
+bool isAmperageConfigured(void);
 int32_t getAmperage(void);
 int32_t getAmperageLatest(void);
 int32_t getMAhDrawn(void);

src/main/telemetry/crsf.c

@@ -399,7 +399,7 @@ void initCrsfTelemetry(void)
     if (sensors(SENSOR_ACC)) {
         crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX);
     }
-    if (isBatteryVoltageAvailable() || isAmperageAvailable()) {
+    if (isBatteryVoltageConfigured() || isAmperageConfigured()) {
         crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX);
     }
     crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX);

src/main/telemetry/frsky_hub.c

@@ -352,22 +352,26 @@ static void sendVario(void)
 static void sendVoltageCells(void)
 {
     static uint16_t currentCell;
+    uint32_t cellVoltage = 0;
+    const uint8_t cellCount = getBatteryCellCount();
 
-    uint8_t cellCount = getBatteryCellCount();
+    if (cellCount) {
-    currentCell %= cellCount;
+        currentCell %= cellCount;
-
+        /*
-    /*
+        * Format for Voltage Data for single cells is like this:
-     * Format for Voltage Data for single cells is like this:
+        *
-     *
+        *  llll llll cccc hhhh
-     *  llll llll cccc hhhh
+        *  l: Low voltage bits
-     *  l: Low voltage bits
+        *  h: High voltage bits
-     *  h: High voltage bits
+        *  c: Cell number (starting at 0)
-     *  c: Cell number (starting at 0)
+        *
-     *
+        * The actual value sent for cell voltage has resolution of 0.002 volts
-     * 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
-     * Since vbat has resolution of 0.1 volts it has to be multiplied by 50
+        */
-     */
+        cellVoltage = ((uint32_t)getBatteryVoltage() * 100 + cellCount) / (cellCount * 2);
-    uint32_t cellVoltage = ((uint32_t)getBatteryVoltage() * 100 + cellCount) / (cellCount * 2);
+    } else {
+        currentCell = 0;
+    }
 
     // Cell number is at bit 9-12
     uint16_t data = (currentCell << 4);
@@ -389,6 +393,7 @@ static void sendVoltageCells(void)
 static void sendVoltageAmp(void)
 {
     uint16_t voltage = getBatteryVoltage();
+    const uint8_t cellCount = getBatteryCellCount();
 
     if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {
         // Use new ID 0x39 to send voltage directly in 0.1 volts resolution
@@ -396,8 +401,8 @@ static void sendVoltageAmp(void)
     } else {
         // send in 0.2 volts resolution
         voltage *= 110 / 21;
-        if (telemetryConfig()->report_cell_voltage) {
+        if (telemetryConfig()->report_cell_voltage && cellCount) {
-            voltage /= getBatteryCellCount();
+            voltage /= cellCount;
         }
 
         frSkyHubWriteFrame(ID_VOLTAGE_AMP_BP, voltage / 100);
@@ -554,11 +559,11 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs)
         sendTemperature1();
         sendThrottleOrBatterySizeAsRpm();
 
-        if (isBatteryVoltageAvailable()) {
+        if (isBatteryVoltageConfigured()) {
             sendVoltageCells();
             sendVoltageAmp();
 
-            if (isAmperageAvailable()) {
+            if (isAmperageConfigured()) {
                 sendAmperage();
                 sendFuelLevel();
             }

src/main/telemetry/mavlink.c

@@ -208,6 +208,16 @@ void mavlinkSendSystemStatus(void)
     if (sensors(SENSOR_BARO)) onboardControlAndSensors |=  8200;
     if (sensors(SENSOR_GPS))  onboardControlAndSensors |= 16416;
 
+    uint16_t batteryVoltage = 0;
+    int16_t batteryAmperage = -1;
+    int8_t batteryRemaining = 100;
+
+    if (getBatteryState() < BATTERY_NOT_PRESENT) {
+        batteryVoltage = isBatteryVoltageConfigured() ? getBatteryVoltage() * 100 : batteryVoltage;
+        batteryAmperage = isAmperageConfigured() ? getAmperage() : batteryAmperage;
+        batteryRemaining = isBatteryVoltageConfigured() ? calculateBatteryPercentageRemaining() : batteryRemaining;
+    }
+
     mavlink_msg_sys_status_pack(0, 200, &mavMsg,
         // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present.
         //Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure,
@@ -222,11 +232,11 @@ 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)
-        isBatteryVoltageAvailable() ? getBatteryVoltage() * 100 : 0,
+        batteryVoltage,
         // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
-        isAmperageAvailable() ? getAmperage() : -1,
+        batteryAmperage,
         // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
-        isBatteryVoltageAvailable() ? calculateBatteryPercentageRemaining() : 100,
+        batteryRemaining,
         // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
         0,
         // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)

src/main/telemetry/smartport.c

@@ -376,10 +376,11 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
 
         switch (id) {
             case FSSP_DATAID_VFAS       :
-                if (isBatteryVoltageAvailable()) {
+                if (isBatteryVoltageConfigured()) {
                     uint16_t vfasVoltage;
                     if (telemetryConfig()->report_cell_voltage) {
-                        vfasVoltage = getBatteryVoltage() / getBatteryCellCount();
+                        const uint8_t cellCount = getBatteryCellCount();
+                        vfasVoltage = cellCount ? getBatteryVoltage() / cellCount : 0;
                     } else {
                         vfasVoltage = getBatteryVoltage();
                     }
@@ -388,7 +389,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                 }
                 break;
             case FSSP_DATAID_CURRENT    :
-                if (isAmperageAvailable()) {
+                if (isAmperageConfigured()) {
                     smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
                     *clearToSend = false;
                 }
@@ -401,7 +402,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                 }
                 break;
             case FSSP_DATAID_FUEL       :
-                if (isAmperageAvailable()) {
+                if (isAmperageConfigured()) {
                     smartPortSendPackage(id, getMAhDrawn()); // given in mAh, unknown requested unit
                     *clearToSend = false;
                 }
@@ -563,8 +564,10 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                 break;
 #endif
             case FSSP_DATAID_A4         :
-                if (isBatteryVoltageAvailable()) {
+                if (isBatteryVoltageConfigured()) {
-                    smartPortSendPackage(id, getBatteryVoltage() * 10 / getBatteryCellCount()); // given in 0.1V, convert to volts
+                    const uint8_t cellCount = getBatteryCellCount();
+                    const uint32_t vfasVoltage = cellCount ? (getBatteryVoltage() * 10 / cellCount) : 0; // given in 0.1V, convert to volts
+                    smartPortSendPackage(id, vfasVoltage);
                     *clearToSend = false;
                 }
                 break;

src/test/unit/telemetry_crsf_msp_unittest.cc

@@ -254,11 +254,11 @@ extern "C" {
     uint32_t micros(void) {return dummyTimeUs;}
     serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) {return NULL;}
     serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
-    bool isBatteryVoltageAvailable(void) { return true; }
+    bool isBatteryVoltageConfigured(void) { return true; }
     uint16_t getBatteryVoltage(void) {
         return testBatteryVoltage;
     }
-    bool isAmperageAvailable(void) { return true; }
+    bool isAmperageConfigured(void) { return true; }
     int32_t getAmperage(void) {
         return testAmperage;
     }

src/test/unit/telemetry_crsf_unittest.cc

@@ -336,7 +336,7 @@ int32_t getMAhDrawn(void){
 bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; }
 bool handleMspFrame(uint8_t *, int)  { return false; }
 void crsfScheduleMspResponse(void) {};
-bool isBatteryVoltageAvailable(void) { return true; }
+bool isBatteryVoltageConfigured(void) { return true; }
-bool isAmperageAvailable(void) { return true; }
+bool isAmperageConfigured(void) { return true; }
 
 }