From 9e776741d94b8ef11f5799448b018ca87d487f22 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Thu, 22 Sep 2022 10:32:12 +0200
Subject: [PATCH] battery: allow for external state of charge injection

---
 src/lib/battery/battery.cpp | 15 ++++++++++++---
 src/lib/battery/battery.h   |  6 +++++-
 2 files changed, 17 insertions(+), 4 deletions(-)

diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp
index dbb50e7dd3..8f8690a2f2 100644
--- a/src/lib/battery/battery.cpp
+++ b/src/lib/battery/battery.cpp
@@ -119,7 +119,13 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 	}
 
 	sumDischarged(timestamp, _current_a);
-	estimateStateOfCharge(_voltage_filter_v.getState(), _current_filter_a.getState());
+	_state_of_charge_volt_based =
+		calculateStateOfChargeVoltageBased(_voltage_filter_v.getState(), _current_filter_a.getState());
+
+	if (!_external_state_of_charge) {
+		estimateStateOfCharge();
+	}
+
 	computeScale();
 
 	if (_connected && _battery_initialized) {
@@ -192,7 +198,7 @@ void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
 	_last_timestamp = timestamp;
 }
 
-void Battery::estimateStateOfCharge(const float voltage_v, const float current_a)
+float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const float current_a)
 {
 	// remaining battery capacity based on voltage
 	float cell_voltage = voltage_v / _params.n_cells;
@@ -215,8 +221,11 @@ void Battery::estimateStateOfCharge(const float voltage_v, const float current_a
 		cell_voltage += throttle * _params.v_load_drop;
 	}
 
-	_state_of_charge_volt_based = math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
+	return math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
+}
 
+void Battery::estimateStateOfCharge()
+{
 	// choose which quantity we're using for final reporting
 	if (_params.capacity > 0.f && _battery_initialized) {
 		// if battery capacity is known, fuse voltage measurement with used capacity
diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h
index 24470b6a0d..9616a4e0cf 100644
--- a/src/lib/battery/battery.h
+++ b/src/lib/battery/battery.h
@@ -88,6 +88,7 @@ public:
 
 	void setPriority(const uint8_t priority) { _priority = priority; }
 	void setConnected(const bool connected) { _connected = connected; }
+	void setStateOfCharge(const float soc) { _state_of_charge = soc; _external_state_of_charge = true; }
 	void updateVoltage(const float voltage_v);
 	void updateCurrent(const float current_a);
 
@@ -144,7 +145,8 @@ protected:
 
 private:
 	void sumDischarged(const hrt_abstime &timestamp, float current_a);
-	void estimateStateOfCharge(const float voltage_v, const float current_a);
+	float calculateStateOfChargeVoltageBased(const float voltage_v, const float current_a);
+	void estimateStateOfCharge();
 	uint8_t determineWarning(float state_of_charge);
 	void computeScale();
 	float computeRemainingTime(float current_a);
@@ -153,6 +155,8 @@ private:
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
 
+	bool _external_state_of_charge{false}; ///< inticates that the soc is injected and not updated by this library
+
 	bool _connected{false};
 	const uint8_t _source;
 	uint8_t _priority{0};