Handle unreliable PX4 support of BATTERY_STATUS

src/Vehicle/BatteryFact.json

@@ -59,7 +59,7 @@
     "name":             "chargeState",
     "shortDescription": "Charge State",
     "type":             "uint8",
-    "enumStrings":      "N/A,Normal Operation,Low Battery State,Critical Battery State,Emergency Battery State,Battery Failed,Battery Unhealthy",
+    "enumStrings":      "n/a,Normal Operation,Low Battery State,Critical Battery State,Emergency Battery State,Battery Failed,Battery Unhealthy",
     "enumValues":       "0,1,2,3,4,5,6",
     "decimalPlaces":    0
 }

src/Vehicle/Vehicle.cc

@@ -1531,12 +1531,40 @@ bool Vehicle::_apmArmingNotRequired()
             _parameterManager->getParameter(FactSystem::defaultComponentId, armingRequireParam)->rawValue().toInt() == 0;
 }
 
+void Vehicle::_batteryStatusWorker(int batteryId, double voltage, double current, double batteryRemainingPct)
+{
+    VehicleBatteryFactGroup* pBatteryFactGroup = nullptr;
+    if (batteryId == 0) {
+        pBatteryFactGroup = &_battery1FactGroup;
+    } else if (batteryId == 1) {
+        pBatteryFactGroup = &_battery2FactGroup;
+    } else {
+        return;
+    }
+
+    pBatteryFactGroup->voltage()->setRawValue(voltage);
+    pBatteryFactGroup->current()->setRawValue(current);
+    pBatteryFactGroup->instantPower()->setRawValue(voltage * current);
+    pBatteryFactGroup->percentRemaining()->setRawValue(batteryRemainingPct);
+
+    //-- Low battery warning
+    if (batteryId == 0 && !qIsNaN(batteryRemainingPct)) {
+        int warnThreshold = _settingsManager->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toInt();
+        if (batteryRemainingPct < warnThreshold &&
+                batteryRemainingPct < _lastAnnouncedLowBatteryPercent &&
+                _lastBatteryAnnouncement.elapsed() > (batteryRemainingPct < warnThreshold * 0.5 ? 15000 : 30000)) {
+            _say(tr("%1 low battery: %2 percent remaining").arg(_vehicleIdSpeech()).arg(static_cast<int>(batteryRemainingPct)));
+            _lastBatteryAnnouncement.start();
+            _lastAnnouncedLowBatteryPercent = static_cast<int>(batteryRemainingPct);
+        }
+    }
+}
+
 void Vehicle::_handleSysStatus(mavlink_message_t& message)
 {
     mavlink_sys_status_t sysStatus;
     mavlink_msg_sys_status_decode(&message, &sysStatus);
 
-
     if (_onboardControlSensorsPresent != sysStatus.onboard_control_sensors_present) {
         _onboardControlSensorsPresent = sysStatus.onboard_control_sensors_present;
         emit sensorsPresentBitsChanged(_onboardControlSensorsPresent);
@@ -1563,6 +1591,12 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message)
         emit unhealthySensorsChanged();
         emit sensorsUnhealthyBitsChanged(_onboardControlSensorsUnhealthy);
     }
+
+    // BATTERY_STATUS is currently unreliable on PX4 stack so we rely on SYS_STATUS for partial battery 0 information to work around it
+    _batteryStatusWorker(0 /* batteryId */,
+                         sysStatus.voltage_battery == UINT16_MAX ? qQNaN() : static_cast<double>(sysStatus.voltage_battery) / 1000.0,
+                         sysStatus.current_battery == -1 ? qQNaN() : static_cast<double>(sysStatus.current_battery) / 100.0,
+                         sysStatus.battery_remaining == -1 ? qQNaN() : sysStatus.battery_remaining);
 }
 
 void Vehicle::_handleBatteryStatus(mavlink_message_t& message)
@@ -1582,7 +1616,7 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message)
     int cellCount = 0;
     double voltage = qQNaN();
     for (int i=0; i<10; i++) {
-        double cellVoltage = bat_status.voltages[i] == UINT16_MAX ? qQNaN() : (double)bat_status.voltages[i] / 1000.0;
+        double cellVoltage = bat_status.voltages[i] == UINT16_MAX ? qQNaN() : static_cast<double>(bat_status.voltages[i]) / 1000.0;
         if (qIsNaN(cellVoltage)) {
             break;
         }
@@ -1599,24 +1633,14 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message)
     pBatteryFactGroup->temperature()->setRawValue(bat_status.temperature == INT16_MAX ? qQNaN() : static_cast<double>(bat_status.temperature) / 100.0);
     pBatteryFactGroup->mahConsumed()->setRawValue(bat_status.current_consumed == -1  ? qQNaN() : bat_status.current_consumed);
     pBatteryFactGroup->chargeState()->setRawValue(bat_status.charge_state);
-    pBatteryFactGroup->current()->setRawValue(bat_status.current_battery == -1 ? qQNaN() : (double)bat_status.current_battery / 100.0);
+    pBatteryFactGroup->timeRemaining()->setRawValue(bat_status.time_remaining == 0 ? qQNaN() : bat_status.time_remaining);
-    pBatteryFactGroup->voltage()->setRawValue(voltage);
-    pBatteryFactGroup->instantPower()->setRawValue(pBatteryFactGroup->voltage()->rawValue().toDouble() * pBatteryFactGroup->current()->rawValue().toDouble());
-    pBatteryFactGroup->timeRemaining()->setRawValue(bat_status.time_remaining);
-    pBatteryFactGroup->percentRemaining()->setRawValue(bat_status.battery_remaining);
 
-    //-- Low battery warning
+    // BATTERY_STATUS is currently unreliable on PX4 stack so we rely on SYS_STATUS for partial battery 0 information to work around it
-    if (bat_status.id == 0) {
+    if (bat_status.id != 0) {
-        if (bat_status.battery_remaining > 0) {
+        _batteryStatusWorker(bat_status.id,
-            int warnThreshold = _settingsManager->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toInt();
+                             voltage,
-            if (bat_status.battery_remaining < warnThreshold &&
+                             bat_status.current_battery == -1 ? qQNaN() : (double)bat_status.current_battery / 100.0,
-                    bat_status.battery_remaining < _lastAnnouncedLowBatteryPercent &&
+                             bat_status.battery_remaining == -1 ? qQNaN() : bat_status.battery_remaining);
-                    _lastBatteryAnnouncement.elapsed() > (bat_status.battery_remaining < warnThreshold * 0.5 ? 15000 : 30000)) {
-                _say(tr("%1 low battery: %2 percent remaining").arg(_vehicleIdSpeech()).arg(bat_status.battery_remaining));
-                _lastBatteryAnnouncement.start();
-                _lastAnnouncedLowBatteryPercent = bat_status.battery_remaining;
-            }
-        }
     }
 }
 
@@ -4278,6 +4302,8 @@ VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent)
     _temperatureFact.setRawValue        (qQNaN());
     _cellCountFact.setRawValue          (qQNaN());
     _instantPowerFact.setRawValue       (qQNaN());
+    _timeRemainingFact.setRawValue      (qQNaN());
+    _chargeStateFact.setRawValue        (MAV_BATTERY_CHARGE_STATE_UNDEFINED);
 }
 
 const char* VehicleWindFactGroup::_directionFactName =      "direction";

src/Vehicle/Vehicle.h

@@ -1348,6 +1348,7 @@ private:
     void _writeCsvLine                  ();
     void _flightTimerStart              ();
     void _flightTimerStop               ();
+    void _batteryStatusWorker           (int batteryId, double voltage, double current, double batteryRemainingPct);
 
     int     _id;                    ///< Mavlink system id
     int     _defaultComponentId;