From 6103a554a3a289fd9187b807770f8ffe9fd3b4ed Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 11 Oct 2015 11:36:45 +0200
Subject: [PATCH] UAS: Fix battery status lowpass and only publish values when
 battery measurements are valid

---
 src/uas/UAS.cc | 23 ++++++++++++++---------
 1 file changed, 14 insertions(+), 9 deletions(-)

diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index cbca01e5d..f593b7a1f 100644
--- a/src/uas/UAS.cc
+++ b/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,11 +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 != UINT16_MAX) {
+            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)
@@ -514,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;
                 }
@@ -1557,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;
 }
 
 /**
-- 
2.22.0