Merge pull request #1949 from mavlink/battery_fix

Battery indication fixes

src/Vehicle/Vehicle.cc

@@ -77,7 +77,7 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType,
     , _latitude(DEFAULT_LAT)
     , _longitude(DEFAULT_LON)
     , _refreshTimer(new QTimer(this))
-    , _batteryVoltage(0.0)
+    , _batteryVoltage(-1.0f)
     , _batteryPercent(0.0)
     , _batteryConsumed(-1.0)
     , _currentHeartbeatTimeout(0)

src/uas/UAS.cc

@@ -73,7 +73,7 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle) : UASInterface(),
     tickLowpassVoltage(12.0f),
     warnLevelPercent(UAS_DEFAULT_BATTERY_WARNLEVEL),
     currentVoltage(12.6f),
-    lpVoltage(12.0f),
+    lpVoltage(-1.0f),
     currentCurrent(0.4f),
     chargeLevel(-1),
     lowBattAlarm(false),
@@ -495,10 +495,11 @@ void UAS::receiveMessage(mavlink_message_t message)
             emit loadChanged(this,state.load/10.0f);
             emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time);
 
+            if (state.voltage_battery > 0.0f && state.voltage_battery != UINT16_MAX) {
                 // Battery charge/time remaining/voltage calculations
                 currentVoltage = state.voltage_battery/1000.0f;
-            lpVoltage = filterVoltage(currentVoltage);
-            tickLowpassVoltage = tickLowpassVoltage*0.8f + 0.2f*currentVoltage;
+                filterVoltage(currentVoltage);
+                tickLowpassVoltage = tickLowpassVoltage * 0.8f + 0.2f * currentVoltage;
 
                 // We don't want to tick above the threshold
                 if (tickLowpassVoltage > tickVoltage)
@@ -513,10 +514,10 @@ void UAS::receiveMessage(mavlink_message_t message)
                         && (currentVoltage > 3.3f)
                         /* warn only if current voltage is really still lower by a reasonable amount */
                         && ((currentVoltage - 0.2f) < tickVoltage)
-                    /* warn only every 12 seconds */
-                    && (QGC::groundTimeUsecs() - lastVoltageWarning) > 12000000)
+                        /* warn only every 20 seconds */
+                        && (QGC::groundTimeUsecs() - lastVoltageWarning) > 20000000)
                 {
-                _say(QString("Voltage warning for system %1: %2 volts").arg(getUASID()).arg(lpVoltage, 0, 'f', 1, QChar(' ')));
+                    _say(QString("Low battery system %1: %2 volts").arg(getUASID()).arg(lpVoltage, 0, 'f', 1, QChar(' ')));
                     lastVoltageWarning = QGC::groundTimeUsecs();
                     lastTickVoltageValue = tickLowpassVoltage;
                 }
@@ -525,6 +526,8 @@ void UAS::receiveMessage(mavlink_message_t message)
                 chargeLevel = state.battery_remaining;
 
                 emit batteryChanged(this, lpVoltage, currentCurrent, getChargeLevel(), 0);
+            }
+
             emit valueChanged(uasId, name.arg("battery_remaining"), "%", getChargeLevel(), time);
             emit valueChanged(uasId, name.arg("battery_voltage"), "V", currentVoltage, time);
 
@@ -1554,9 +1557,14 @@ quint64 UAS::getUnixTime(quint64 time)
 /**
  * @param value battery voltage
  */
-float UAS::filterVoltage(float value) const
+float UAS::filterVoltage(float value)
 {
-    return lpVoltage * 0.6f + value * 0.4f;
+    if (lpVoltage < 0.0f) {
+        lpVoltage = value;
+    }
+
+    lpVoltage = lpVoltage * 0.6f + value * 0.4f;
+    return lpVoltage;
 }
 
 /**

src/uas/UAS.h

@@ -85,7 +85,7 @@ public:
     /** @brief The time interval the robot is switched on */
     quint64 getUptime() const;
     /** @brief Add one measurement and get low-passed voltage */
-    float filterVoltage(float value) const;
+    float filterVoltage(float value);
 
     Q_PROPERTY(double localX READ getLocalX WRITE setLocalX NOTIFY localXChanged)
     Q_PROPERTY(double localY READ getLocalY WRITE setLocalY NOTIFY localYChanged)

src/ui/toolbar/MainToolBar.qml

@@ -479,7 +479,7 @@ Rectangle {
 
                 QGCLabel {
                     visible: batteryStatus.visible && activeVehicle.batteryConsumed < 0.0
-                    text: activeVehicle.batteryVoltage.toFixed(1) + 'V';
+                    text: (activeVehicle.batteryVoltage > 0) ? activeVehicle.batteryVoltage.toFixed(1) + 'V' : '---';
                     font.pixelSize: ScreenTools.smallFontPixelSize
                     font.weight: Font.DemiBold
                     anchors.right: parent.right
@@ -495,7 +495,7 @@ Rectangle {
                     anchors.rightMargin:    getProportionalDimmension(6)
                     visible: batteryStatus.visible && activeVehicle.batteryConsumed >= 0.0
                     QGCLabel {
-                        text: activeVehicle.batteryVoltage.toFixed(1) + 'V';
+                        text: (activeVehicle.batteryVoltage > 0) ? activeVehicle.batteryVoltage.toFixed(1) + 'V' : '---';
                         width: getProportionalDimmension(30)
                         horizontalAlignment: Text.AlignRight
                         font.pixelSize: ScreenTools.smallFontPixelSize
@@ -503,7 +503,7 @@ Rectangle {
                         color: colorWhite
                     }
                     QGCLabel {
-                        text: activeVehicle.batteryConsumed.toFixed(0) + 'mAh';
+                        text: (activeVehicle.batteryConsumed > 0) ? activeVehicle.batteryConsumed.toFixed(0) + 'mAh' : '---';
                         width: getProportionalDimmension(30)
                         horizontalAlignment: Text.AlignRight
                         font.pixelSize: ScreenTools.smallFontPixelSize