battery_status: factor in total current consumption in battery remaining estimation

src/lib/battery/battery.cpp

@@ -134,7 +134,7 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
 	_current_filter_a.update(current_a);
 	_throttle_filter.update(throttle_normalized);
 	sumDischarged(timestamp, current_a);
-	estimateRemaining(_voltage_filter_v.getState(), _current_filter_a.getState(), _throttle_filter.getState());
+	estimateStateOfCharge(_voltage_filter_v.getState(), _current_filter_a.getState(), _throttle_filter.getState());
 	computeScale();
 
 	if (_battery_initialized) {
@@ -150,7 +150,7 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
 		_battery_status.current_filtered_a = _current_filter_a.getState();
 		_battery_status.discharged_mah = _discharged_mah;
 		_battery_status.warning = _warning;
-		_battery_status.remaining = _remaining;
+		_battery_status.remaining = _state_of_charge;
 		_battery_status.connected = connected;
 		_battery_status.source = source;
 		_battery_status.priority = priority;
@@ -198,7 +198,7 @@ void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
 	_last_timestamp = timestamp;
 }
 
-void Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
+void Battery::estimateStateOfCharge(const float voltage_v, const float current_a, const float throttle)
 {
 	// remaining battery capacity based on voltage
 	float cell_voltage = voltage_v / _params.n_cells;
@@ -212,27 +212,30 @@ void Battery::estimateRemaining(const float voltage_v, const float current_a, co
 		cell_voltage += throttle * _params.v_load_drop;
 	}
 
-	_remaining_voltage = math::gradual(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
+	_state_of_charge_volt_based = math::gradual(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
 
 	// choose which quantity we're using for final reporting
 	if (_params.capacity > 0.f) {
 		// if battery capacity is known, fuse voltage measurement with used capacity
 		if (!_battery_initialized) {
 			// initialization of the estimation state
-			_remaining = _remaining_voltage;
+			_state_of_charge = _state_of_charge_volt_based;
 
 		} 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);
+			const float weight_v = 3e-4f * (1 - _state_of_charge_volt_based);
-			_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
+			_state_of_charge = (1 - weight_v) * _state_of_charge + weight_v * _state_of_charge_volt_based;
 			// directly apply current capacity slope calculated using current
-			_remaining -= _discharged_mah_loop / _params.capacity;
+			_state_of_charge -= _discharged_mah_loop / _params.capacity;
-			_remaining = math::max(_remaining, 0.f);
+			_state_of_charge = math::max(_state_of_charge, 0.f);
+
+			const float state_of_charge_current_based = math::max(1.f - _discharged_mah / _params.capacity, 0.f);
+			_state_of_charge = math::min(state_of_charge_current_based, _state_of_charge);
+
 		}
 
 	} else {
-		// else use voltage
+		_state_of_charge = _state_of_charge_volt_based;
-		_remaining = _remaining_voltage;
 	}
 }
 
@@ -240,13 +243,13 @@ void Battery::determineWarning(bool connected)
 {
 	if (connected) {
 		// propagate warning state only if the state is higher, otherwise remain in current warning state
-		if (_remaining < _params.emergen_thr) {
+		if (_state_of_charge < _params.emergen_thr) {
 			_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
 
-		} else if (_remaining < _params.crit_thr) {
+		} else if (_state_of_charge < _params.crit_thr) {
 			_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
 
-		} else if (_remaining < _params.low_thr) {
+		} else if (_state_of_charge < _params.low_thr) {
 			_warning = battery_status_s::BATTERY_WARNING_LOW;
 
 		} else {
@@ -260,7 +263,7 @@ void Battery::computeScale()
 	const float voltage_range = (_params.v_charged - _params.v_empty);
 
 	// reusing capacity calculation to get single cell voltage before drop
-	const float bat_v = _params.v_empty + (voltage_range * _remaining_voltage);
+	const float bat_v = _params.v_empty + (voltage_range * _state_of_charge_volt_based);
 
 	_scale = _params.v_charged / bat_v;
 

src/lib/battery/battery.h

@@ -196,7 +196,7 @@ protected:
 
 private:
 	void sumDischarged(const hrt_abstime &timestamp, float current_a);
-	void estimateRemaining(const float voltage_v, const float current_a, const float throttle);
+	void estimateStateOfCharge(const float voltage_v, const float current_a, const float throttle);
 	void determineWarning(bool connected);
 	void computeScale();
 
@@ -208,8 +208,8 @@ private:
 	AlphaFilter<float> _throttle_filter;
 	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 _state_of_charge_volt_based{-1.f};	// [0,1]
-	float _remaining{-1.f};			///< normalized battery charge level, selected based on config param
+	float _state_of_charge{-1.f};				// [0,1]
 	float _scale{1.f};
 	uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE};
 	hrt_abstime _last_timestamp{0};