From d3e9ee88a00cad62f44a3272b409d7214f03aeeb Mon Sep 17 00:00:00 2001
From: nanthony21 <nickmanthony@hotmail.com>
Date: Sat, 10 Mar 2018 19:09:56 -0600
Subject: [PATCH] Use filtered throttle for battery estimation instead of raw.
---
src/modules/systemlib/battery.cpp | 18 +++++++++++++++---
src/modules/systemlib/battery.h | 4 +++-
2 files changed, 18 insertions(+), 4 deletions(-)
diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp
index 188fd7163a..2302515707 100644
--- a/src/modules/systemlib/battery.cpp
+++ b/src/modules/systemlib/battery.cpp
@@ -71,9 +71,10 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
reset(battery_status);
battery_status->timestamp = timestamp;
filterVoltage(voltage_v);
+ filterThrottle(throttle_normalized);
filterCurrent(current_a);
sumDischarged(timestamp, current_a);
- estimateRemaining(_voltage_filtered_v, _current_filtered_a, throttle_normalized, armed);
+ estimateRemaining(_voltage_filtered_v, _current_filtered_a, _throttle_filtered, armed);
computeScale();
if (_battery_initialized) {
@@ -126,6 +127,17 @@ Battery::filterCurrent(float current_a)
}
}
+void Battery::filterThrottle(float throttle){
+ if (!_battery_initialized) {
+ _throttle_filtered = throttle;
+ }
+
+ const float filtered_next = _throttle_filtered * 0.99f + throttle * 0.01f;
+
+ if (PX4_ISFINITE(filtered_next)){
+ _throttle_filtered = filtered_next;
+ }
+}
void
Battery::sumDischarged(hrt_abstime timestamp, float current_a)
@@ -150,7 +162,7 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a)
}
void
-Battery::estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed)
+Battery::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed)
{
// remaining battery capacity based on voltage
float cell_voltage = voltage_v / _n_cells.get();
@@ -161,7 +173,7 @@ Battery::estimateRemaining(float voltage_v, float current_a, float throttle_norm
} else {
// assume linear relation between throttle and voltage drop
- cell_voltage += throttle_normalized * _v_load_drop.get();
+ cell_voltage += throttle * _v_load_drop.get();
}
_remaining_voltage = math::gradual(cell_voltage, _v_empty.get(), _v_charged.get(), 0.f, 1.f);
diff --git a/src/modules/systemlib/battery.h b/src/modules/systemlib/battery.h
index fa9a4b1e5e..b007c43863 100644
--- a/src/modules/systemlib/battery.h
+++ b/src/modules/systemlib/battery.h
@@ -88,9 +88,10 @@ public:
private:
void filterVoltage(float voltage_v);
+ void filterThrottle(float throttle);
void filterCurrent(float current_a);
void sumDischarged(hrt_abstime timestamp, float current_a);
- void estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed);
+ void estimateRemaining(float voltage_v, float current_a, float throttle, bool armed);
void determineWarning(bool connected);
void computeScale();
@@ -108,6 +109,7 @@ private:
bool _battery_initialized = false;
float _voltage_filtered_v = -1.f;
+ float _throttle_filtered = -1.f;
float _current_filtered_a = -1.f;
float _discharged_mah = 0.f;
float _discharged_mah_loop = 0.f;
--
GitLab