From 611ab577d832d2e00332fbf3f58165340a364214 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Thu, 22 Feb 2018 14:24:20 +0100
Subject: [PATCH] Battery: Fix estimate initialization

On the Intel Aero and probably also other specific platforms
the first measured voltage values despite connected battery are unuasable.
The solution here is to start filtering and determining warning
only after a measurement above 2.1V was received.
---
 src/modules/systemlib/battery.cpp | 12 ++++++++----
 src/modules/systemlib/battery.h   |  5 +++--
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp
index 778708d017..7a8a8e940d 100644
--- a/src/modules/systemlib/battery.cpp
+++ b/src/modules/systemlib/battery.cpp
@@ -89,10 +89,14 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 	filterCurrent(current_a);
 	sumDischarged(timestamp, current_a);
 	estimateRemaining(_voltage_filtered_v, _current_filtered_a, throttle_normalized, armed);
-	determineWarning(connected);
 	computeScale();
 
+	if (_battery_initialized) {
+		determineWarning(connected);
+	}
+
 	if (_voltage_filtered_v > 2.1f) {
+		_battery_initialized = true;
 		battery_status->voltage_v = voltage_v;
 		battery_status->voltage_filtered_v = _voltage_filtered_v;
 		battery_status->scale = _scale;
@@ -110,7 +114,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 void
 Battery::filterVoltage(float voltage_v)
 {
-	if (_voltage_filtered_v < 0.f) {
+	if (!_battery_initialized) {
 		_voltage_filtered_v = voltage_v;
 	}
 
@@ -125,7 +129,7 @@ Battery::filterVoltage(float voltage_v)
 void
 Battery::filterCurrent(float current_a)
 {
-	if (_current_filtered_a < 0.f) {
+	if (!_battery_initialized) {
 		_current_filtered_a = current_a;
 	}
 
@@ -183,7 +187,7 @@ Battery::estimateRemaining(float voltage_v, float current_a, float throttle_norm
 	// choose which quantity we're using for final reporting
 	if (_capacity.get() > 0.f) {
 		// if battery capacity is known, fuse voltage measurement with used capacity
-		if (_remaining > 1.f) {
+		if (!_battery_initialized) {
 			// initialization of the estimation state
 			_remaining = _remaining_voltage;
 
diff --git a/src/modules/systemlib/battery.h b/src/modules/systemlib/battery.h
index 8fbb8b9735..bb0a754e97 100644
--- a/src/modules/systemlib/battery.h
+++ b/src/modules/systemlib/battery.h
@@ -113,12 +113,13 @@ private:
 	control::BlockParamFloat _crit_thr;
 	control::BlockParamFloat _emergency_thr;
 
+	bool _battery_initialized = false;
 	float _voltage_filtered_v = -1.f;
 	float _current_filtered_a = -1.f;
 	float _discharged_mah = 0.f;
 	float _discharged_mah_loop = 0.f;
-	float _remaining_voltage = 1.f;		///< normalized battery charge level remaining based on voltage
-	float _remaining = 2.f;			///< normalized battery charge level, selected based on config param
+	float _remaining_voltage = -1.f;		///< normalized battery charge level remaining based on voltage
+	float _remaining = -1.f;			///< normalized battery charge level, selected based on config param
 	float _scale = 1.f;
 	uint8_t _warning;
 	hrt_abstime _last_timestamp;