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,36 +495,39 @@ 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);
 
-            // Battery charge/time remaining/voltage calculations
-            currentVoltage = state.voltage_battery/1000.0f;
-            lpVoltage = filterVoltage(currentVoltage);
-            tickLowpassVoltage = tickLowpassVoltage*0.8f + 0.2f*currentVoltage;
+            if (state.voltage_battery > 0.0f && state.voltage_battery != UINT16_MAX) {
+                // Battery charge/time remaining/voltage calculations
+                currentVoltage = state.voltage_battery/1000.0f;
+                filterVoltage(currentVoltage);
+                tickLowpassVoltage = tickLowpassVoltage * 0.8f + 0.2f * currentVoltage;
 
-            // We don't want to tick above the threshold
-            if (tickLowpassVoltage > tickVoltage)
-            {
-                lastTickVoltageValue = tickLowpassVoltage;
-            }
+                // We don't want to tick above the threshold
+                if (tickLowpassVoltage > tickVoltage)
+                {
+                    lastTickVoltageValue = tickLowpassVoltage;
+                }
 
-            if ((startVoltage > 0.0f) && (tickLowpassVoltage < tickVoltage) && (fabs(lastTickVoltageValue - tickLowpassVoltage) > 0.1f)
-                    /* warn if lower than treshold */
-                    && (lpVoltage < tickVoltage)
-                    /* warn only if we have at least the voltage of an empty LiPo cell, else we're sampling something wrong */
-                    && (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)
-            {
-                _say(QString("Voltage warning for system %1: %2 volts").arg(getUASID()).arg(lpVoltage, 0, 'f', 1, QChar(' ')));
-                lastVoltageWarning = QGC::groundTimeUsecs();
-                lastTickVoltageValue = tickLowpassVoltage;
-            }
+                if ((startVoltage > 0.0f) && (tickLowpassVoltage < tickVoltage) && (fabs(lastTickVoltageValue - tickLowpassVoltage) > 0.1f)
+                        /* warn if lower than treshold */
+                        && (lpVoltage < tickVoltage)
+                        /* warn only if we have at least the voltage of an empty LiPo cell, else we're sampling something wrong */
+                        && (currentVoltage > 3.3f)
+                        /* warn only if current voltage is really still lower by a reasonable amount */
+                        && ((currentVoltage - 0.2f) < tickVoltage)
+                        /* warn only every 20 seconds */
+                        && (QGC::groundTimeUsecs() - lastVoltageWarning) > 20000000)
+                {
+                    _say(QString("Low battery system %1: %2 volts").arg(getUASID()).arg(lpVoltage, 0, 'f', 1, QChar(' ')));
+                    lastVoltageWarning = QGC::groundTimeUsecs();
+                    lastTickVoltageValue = tickLowpassVoltage;
+                }
+
+                if (startVoltage == -1.0f && currentVoltage > 0.1f) startVoltage = currentVoltage;
+                chargeLevel = state.battery_remaining;
 
-            if (startVoltage == -1.0f && currentVoltage > 0.1f) startVoltage = currentVoltage;
-            chargeLevel = state.battery_remaining;
+                emit batteryChanged(this, lpVoltage, currentCurrent, getChargeLevel(), 0);
+            }
 
-            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