Battery Estimation Corrections

src/modules/simulator/simulator_mavlink.cpp

@@ -367,23 +367,16 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 					batt_sim_start = now;
 				}
 
-				unsigned cellcount = _battery.cell_count();
+				float ibatt = -1.0f; // no current sensor in simulation
+				const float minimum_percentage = 0.5f; // change this value if you want to simulate low battery reaction
 
-				float vbatt = _battery.full_cell_voltage() ;
-				float ibatt = -1.0f;
+				/* Simulate the voltage of a linearly draining battery but stop at the minimum percentage */
+				float battery_percentage = (now - batt_sim_start) / discharge_interval_us;
+				battery_percentage = math::min(battery_percentage, minimum_percentage);
+				float vbatt = math::gradual(battery_percentage, 0.f, 1.f, _battery.full_cell_voltage(), _battery.empty_cell_voltage());
+				vbatt *= _battery.cell_count();
 
-				float discharge_v = _battery.full_cell_voltage() - _battery.empty_cell_voltage();
-
-				vbatt = (_battery.full_cell_voltage() - (discharge_v * ((now - batt_sim_start) / discharge_interval_us)))  * cellcount;
-
-				float batt_voltage_loaded = _battery.empty_cell_voltage() - 0.05f;
-
-				if (!PX4_ISFINITE(vbatt) || (vbatt < (cellcount * batt_voltage_loaded))) {
-					vbatt = cellcount * batt_voltage_loaded;
-				}
-
-				// TODO: don't hard-code throttle.
-				const float throttle = 0.5f;
+				const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable
 				_battery.updateBatteryStatus(now, vbatt, ibatt, true, true, 0, throttle, armed, &_battery_status);
 
 				// publish the battery voltage

src/modules/systemlib/battery.cpp

@@ -40,25 +40,26 @@
  */
 
 #include "battery.h"
+#include <mathlib/mathlib.h>
 
 Battery::Battery() :
 	SuperBlock(nullptr, "BAT"),
-	_param_v_empty(this, "V_EMPTY"),
-	_param_v_full(this, "V_CHARGED"),
-	_param_n_cells(this, "N_CELLS"),
-	_param_capacity(this, "CAPACITY"),
-	_param_v_load_drop(this, "V_LOAD_DROP"),
-	_param_r_internal(this, "R_INTERNAL"),
-	_param_low_thr(this, "LOW_THR"),
-	_param_crit_thr(this, "CRIT_THR"),
-	_param_emergency_thr(this, "EMERGEN_THR"),
-	_voltage_filtered_v(-1.0f),
-	_current_filtered_a(-1.0f),
-	_discharged_mah(0.0f),
-	_remaining_voltage(1.0f),
-	_remaining_capacity(1.0f),
-	_remaining(1.0f),
-	_scale(1.0f),
+	_v_empty(this, "V_EMPTY"),
+	_v_charged(this, "V_CHARGED"),
+	_n_cells(this, "N_CELLS"),
+	_capacity(this, "CAPACITY"),
+	_v_load_drop(this, "V_LOAD_DROP"),
+	_r_internal(this, "R_INTERNAL"),
+	_low_thr(this, "LOW_THR"),
+	_crit_thr(this, "CRIT_THR"),
+	_emergency_thr(this, "EMERGEN_THR"),
+	_voltage_filtered_v(-1.f),
+	_current_filtered_a(-1.f),
+	_discharged_mah(0.f),
+	_discharged_mah_loop(0.f),
+	_remaining_voltage(1.f),
+	_remaining(2.f),
+	_scale(1.f),
 	_warning(battery_status_s::BATTERY_WARNING_NONE),
 	_last_timestamp(0)
 {
@@ -74,10 +75,10 @@ void
 Battery::reset(battery_status_s *battery_status)
 {
 	memset(battery_status, 0, sizeof(*battery_status));
-	battery_status->current_a = -1.0f;
-	battery_status->remaining = 1.0f;
-	battery_status->scale = 1.0f;
-	battery_status->cell_count = _param_n_cells.get();
+	battery_status->current_a = -1.f;
+	battery_status->remaining = 1.f;
+	battery_status->scale = 1.f;
+	battery_status->cell_count = _n_cells.get();
 	// TODO: check if it is sane to reset warning to NONE
 	battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
 	battery_status->connected = false;
@@ -94,7 +95,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 	filterVoltage(voltage_v);
 	filterCurrent(current_a);
 	sumDischarged(timestamp, current_a);
-	estimateRemaining(voltage_v, current_a, throttle_normalized, armed);
+	estimateRemaining(_voltage_filtered_v, _current_filtered_a, throttle_normalized, armed);
 	determineWarning(connected);
 	computeScale();
 
@@ -116,7 +117,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 void
 Battery::filterVoltage(float voltage_v)
 {
-	if (_voltage_filtered_v < 0.0f) {
+	if (_voltage_filtered_v < 0.f) {
 		_voltage_filtered_v = voltage_v;
 	}
 
@@ -131,7 +132,7 @@ Battery::filterVoltage(float voltage_v)
 void
 Battery::filterCurrent(float current_a)
 {
-	if (_current_filtered_a < 0.0f) {
+	if (_current_filtered_a < 0.f) {
 		_current_filtered_a = current_a;
 	}
 
@@ -148,16 +149,19 @@ void
 Battery::sumDischarged(hrt_abstime timestamp, float current_a)
 {
 	// Not a valid measurement
-	if (current_a < 0.0f) {
+	if (current_a < 0.f) {
 		// Because the measurement was invalid we need to stop integration
 		// and re-initialize with the next valid measurement
 		_last_timestamp = 0;
 		return;
 	}
 
-	// Ignore first update because we don't know dT.
+	// Ignore first update because we don't know dt.
 	if (_last_timestamp != 0) {
-		_discharged_mah += current_a * ((float)(timestamp - _last_timestamp)) / 1e3f / 3600.0f;
+		const float dt = (timestamp - _last_timestamp) / 1e6;
+		// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
+		_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
+		_discharged_mah += _discharged_mah_loop;
 	}
 
 	_last_timestamp = timestamp;
@@ -166,54 +170,35 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a)
 void
 Battery::estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed)
 {
-	const float bat_r = _param_r_internal.get();
-
-	// remaining charge estimate based on voltage and internal resistance (drop under load)
-	float bat_v_empty_dynamic = _param_v_empty.get();
-
-	if (bat_r >= 0.0f) {
-		bat_v_empty_dynamic -= current_a * bat_r;
+	// correct battery voltage locally for load drop to avoid estimation fluctuations
+	if (_r_internal.get() >= 0.f) {
+		voltage_v += _r_internal.get() * current_a;
 
 	} else {
-		// assume 10% voltage drop of the full drop range with motors idle
-		const float thr = (armed) ? ((fabsf(throttle_normalized) + 0.1f) / 1.1f) : 0.0f;
-
-		bat_v_empty_dynamic -= _param_v_load_drop.get() * thr;
+		// assume quadratic relation between throttle and current
+		// good assumption if throttle represents RPM
+		voltage_v += throttle_normalized * throttle_normalized * _v_load_drop.get();
 	}
 
-	// the range from full to empty is the same for batteries under load and without load,
-	// since the voltage drop applies to both the full and empty state
-	const float voltage_range = (_param_v_full.get() - _param_v_empty.get());
-
 	// remaining battery capacity based on voltage
-	const float rvoltage = (voltage_v - (_param_n_cells.get() * bat_v_empty_dynamic))
-			       / (_param_n_cells.get() * voltage_range);
-	const float rvoltage_filt = _remaining_voltage * 0.99f + rvoltage * 0.01f;
-
-	if (PX4_ISFINITE(rvoltage_filt)) {
-		_remaining_voltage = rvoltage_filt;
-	}
-
-	// remaining battery capacity based on used current integrated time
-	const float rcap = 1.0f - _discharged_mah / _param_capacity.get();
-	const float rcap_filt = _remaining_capacity * 0.99f + rcap * 0.01f;
-
-	if (PX4_ISFINITE(rcap_filt)) {
-		_remaining_capacity = rcap_filt;
-	}
-
-	// limit to sane values
-	_remaining_voltage = (_remaining_voltage < 0.0f) ? 0.0f : _remaining_voltage;
-	_remaining_voltage = (_remaining_voltage > 1.0f) ? 1.0f : _remaining_voltage;
-
-	_remaining_capacity = (_remaining_capacity < 0.0f) ? 0.0f : _remaining_capacity;
-	_remaining_capacity = (_remaining_capacity > 1.0f) ? 1.0f : _remaining_capacity;
+	const float cell_voltage = voltage_v / _n_cells.get();
+	_remaining_voltage = math::gradual(cell_voltage, _v_empty.get(), _v_charged.get(), 0.f, 1.f);
 
 	// choose which quantity we're using for final reporting
-	if (_param_capacity.get() > 0.0f) {
-		// if battery capacity is known, use discharged current for estimate,
-		// but don't show more than voltage estimate
-		_remaining = fminf(_remaining_voltage, _remaining_capacity);
+	if (_capacity.get() > 0.f) {
+		// if battery capacity is known, fuse voltage measurement with used capacity
+		if (_remaining > 1.f) {
+			// initialization of the estimation state
+			_remaining = _remaining_voltage;
+
+		} else {
+			// The lower the voltage the more adjust the estimate with it to avoid deep discharge
+			const float weight_v = 3e-4f * (1 - _remaining_voltage);
+			_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
+			// directly apply current capacity slope calculated using current
+			_remaining -= _discharged_mah_loop / _capacity.get();
+			_remaining = math::max(_remaining, 0.f);
+		}
 
 	} else {
 		// else use voltage
@@ -226,13 +211,13 @@ Battery::determineWarning(bool connected)
 {
 	if (connected) {
 		// propagate warning state only if the state is higher, otherwise remain in current warning state
-		if (_remaining < _param_emergency_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
+		if (_remaining < _emergency_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
 			_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
 
-		} else if (_remaining < _param_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
+		} else if (_remaining < _crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
 			_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
 
-		} else if (_remaining < _param_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
+		} else if (_remaining < _low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
 			_warning = battery_status_s::BATTERY_WARNING_LOW;
 		}
 	}
@@ -241,17 +226,17 @@ Battery::determineWarning(bool connected)
 void
 Battery::computeScale()
 {
-	const float voltage_range = (_param_v_full.get() - _param_v_empty.get());
+	const float voltage_range = (_v_charged.get() - _v_empty.get());
 
 	// reusing capacity calculation to get single cell voltage before drop
-	const float bat_v = _param_v_empty.get() + (voltage_range * _remaining_voltage);
+	const float bat_v = _v_empty.get() + (voltage_range * _remaining_voltage);
 
-	_scale = _param_v_full.get() / bat_v;
+	_scale = _v_charged.get() / bat_v;
 
 	if (_scale > 1.3f) { // Allow at most 30% compensation
 		_scale = 1.3f;
 
-	} else if (!PX4_ISFINITE(_scale) || _scale < 1.0f) { // Shouldn't ever be more than the power at full battery
-		_scale = 1.0f;
+	} else if (!PX4_ISFINITE(_scale) || _scale < 1.f) { // Shouldn't ever be more than the power at full battery
+		_scale = 1.f;
 	}
 }

src/modules/systemlib/battery.h

@@ -68,17 +68,17 @@ class Battery : public control::SuperBlock
 	/**
 	 * Get the battery cell count
 	 */
-	int cell_count() { return _param_n_cells.get(); }
+	int cell_count() { return _n_cells.get(); }
 
 	/**
 	 * Get the empty voltage per cell
 	 */
-	float empty_cell_voltage() { return _param_v_empty.get(); }
+	float empty_cell_voltage() { return _v_empty.get(); }
 
 	/**
 	 * Get the full voltage per cell
 	 */
-	float full_cell_voltage() { return _param_v_full.get(); }
+	float full_cell_voltage() { return _v_charged.get(); }
 
 	/**
 	 * Update current battery status message.
@@ -103,21 +103,21 @@ class Battery : public control::SuperBlock
 	void determineWarning(bool connected);
 	void computeScale();
 
-	control::BlockParamFloat _param_v_empty;
-	control::BlockParamFloat _param_v_full;
-	control::BlockParamInt _param_n_cells;
-	control::BlockParamFloat _param_capacity;
-	control::BlockParamFloat _param_v_load_drop;
-	control::BlockParamFloat _param_r_internal;
-	control::BlockParamFloat _param_low_thr;
-	control::BlockParamFloat _param_crit_thr;
-	control::BlockParamFloat _param_emergency_thr;
+	control::BlockParamFloat _v_empty;
+	control::BlockParamFloat _v_charged;
+	control::BlockParamInt _n_cells;
+	control::BlockParamFloat _capacity;
+	control::BlockParamFloat _v_load_drop;
+	control::BlockParamFloat _r_internal;
+	control::BlockParamFloat _low_thr;
+	control::BlockParamFloat _crit_thr;
+	control::BlockParamFloat _emergency_thr;
 
 	float _voltage_filtered_v;
 	float _current_filtered_a;
 	float _discharged_mah;
+	float _discharged_mah_loop;
 	float _remaining_voltage;		///< normalized battery charge level remaining based on voltage
-	float _remaining_capacity;		///< normalized battery charge level remaining based on capacity
 	float _remaining;			///< normalized battery charge level, selected based on config param
 	float _scale;
 	uint8_t _warning;

src/modules/systemlib/battery_params.c

@@ -56,7 +56,7 @@
  * @increment 0.01
  * @reboot_required true
  */
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f);
 
 /**
  * Full cell voltage (5C load)