SMBus battery (a.k.a. smart battery) enhancement. (#14496)

 * Enhancement: State of health, and max_error value is added. Both shows battery health of SMBUS smart battery.
 * Enhancement: BAT_C_MULT parameter is introduced. This is for high-current capable SMBUS-based battery.
As SMBUS only provides 16-bit for current, it could only be +-32768mA which is about +-32A.
But with proper treatment, it could be extended with little accuracy loss.
This factor can be set for individual battery system with available information.
    * Relative SOC introduced. Proper SMBUS battery should provide percentage of remaining battery
directly. Therefore it does not have to be computed like before.
    * State of Health introduced. Proper SMBUS battery should provide SOH value.
    * Max error: this shows estimation error of BMS.
 * Enhancement: With smart battery, precise estimation of time remaining is provided
with impedance track. It is unit of minute, so 60 seconds multiplied.
Update rate of this is not fast, but very useful.

Co-authored-by: Hyon Lim <[email protected]>

msg/battery_status.msg

@@ -17,6 +17,8 @@ uint16 cycle_count         # number of discharge cycles the battery has experien
 uint16 run_time_to_empty   # predicted remaining battery capacity based on the present rate of discharge in min
 uint16 average_time_to_empty   # predicted remaining battery capacity based on the average rate of discharge in min
 uint16 serial_number       # serial number of the battery pack
+uint16 state_of_health	# state of health. FullChargeCapacity/DesignCapacity.
+uint16 max_error # max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
 uint8 id                # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed.
 
 float32[10] voltage_cell_v   # Battery individual cell voltages

src/drivers/batt_smbus/batt_smbus.cpp

@@ -89,6 +89,9 @@ void BATT_SMBUS::RunImpl()
 	// Read data from sensor.
 	battery_status_s new_report = {};
 
+	// TODO(hyonlim): this driver should support multiple SMBUS going forward.
+	new_report.id = 1;
+
 	// Set time of reading.
 	new_report.timestamp = now;
 
@@ -108,37 +111,48 @@ void BATT_SMBUS::RunImpl()
 	// Read current.
 	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);
 
-	new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f);
+	new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult;
 	new_report.current_filtered_a = new_report.current_a;
 
 	// Read average current.
 	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);
 
-	float average_current = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f);
+	float average_current = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult;
 
 	new_report.average_current_a = average_current;
 
 	// If current is high, turn under voltage protection off. This is neccessary to prevent
 	// a battery from cutting off while flying with high current near the end of the packs capacity.
 	set_undervoltage_protection(average_current);
 
-	// Read run time to empty.
+	// Read run time to empty (minutes).
 	ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result);
 	new_report.run_time_to_empty = result;
 
-	// Read average time to empty.
+	// Read average time to empty (minutes).
 	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result);
 	new_report.average_time_to_empty = result;
 
 	// Read remaining capacity.
 	ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, result);
 
-	// Calculate remaining capacity percent with complementary filter.
-	new_report.remaining = 0.8f * _last_report.remaining + 0.2f * (1.0f - (float)((float)(_batt_capacity - result) /
-			       (float)_batt_capacity));
+	// Calculate total discharged amount in mah.
+	new_report.discharged_mah = _batt_capacity - (float)result * _c_mult;
+
+	// Read Relative SOC.
+	ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result);
+
+	// Normalize 0.0 to 1.0
+	new_report.remaining = (float)result / 100.0f;
+
+	// Read SOH
+	// TODO(hyonlim): this value can be used for battery_status.warning mavlink message.
+	ret |= _interface->read_word(BATT_SMBUS_STATE_OF_HEALTH, result);
+	new_report.state_of_health = result;
 
-	// Calculate total discharged amount.
-	new_report.discharged_mah = _batt_startup_capacity - result;
+	// Read Max Error
+	ret |= _interface->read_word(BATT_SMBUS_MAX_ERROR, result);
+	new_report.max_error = result;
 
 	// Check if max lifetime voltage delta is greater than allowed.
 	if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) {
@@ -304,9 +318,16 @@ int BATT_SMBUS::dataflash_write(uint16_t address, void *data, const unsigned len
 int BATT_SMBUS::get_startup_info()
 {
 	int result = 0;
+
 	// The name field is 21 characters, add one for null terminator.
 	const unsigned name_length = 22;
 
+	// Read battery threshold params on startup.
+	param_get(param_find("BAT_CRIT_THR"), &_crit_thr);
+	param_get(param_find("BAT_LOW_THR"), &_low_thr);
+	param_get(param_find("BAT_EMERGEN_THR"), &_emergency_thr);
+	param_get(param_find("BAT_C_MULT"), &_c_mult);
+
 	// Try and get battery SBS info.
 	if (_manufacturer_name == nullptr) {
 		char man_name[name_length] = {};
@@ -334,9 +355,9 @@ int BATT_SMBUS::get_startup_info()
 
 	if (!result) {
 		_serial_number = serial_num;
-		_batt_startup_capacity = remaining_cap;
+		_batt_startup_capacity = (uint16_t)((float)remaining_cap * _c_mult);
 		_cycle_count = cycle_count;
-		_batt_capacity = full_cap;
+		_batt_capacity = (uint16_t)((float)full_cap * _c_mult);
 	}
 
 	if (lifetime_data_flush() == PX4_OK) {
@@ -354,11 +375,6 @@ int BATT_SMBUS::get_startup_info()
 		PX4_WARN("Failed to flush lifetime data");
 	}
 
-	// Read battery threshold params on startup.
-	param_get(param_find("BAT_CRIT_THR"), &_crit_thr);
-	param_get(param_find("BAT_LOW_THR"), &_low_thr);
-	param_get(param_find("BAT_EMERGEN_THR"), &_emergency_thr);
-
 	return result;
 }
 

src/drivers/batt_smbus/batt_smbus.h

@@ -65,7 +65,9 @@
 #define BATT_SMBUS_ADDR                                 0x0B            ///< Default 7 bit address I2C address. 8 bit = 0x16
 
 #define BATT_SMBUS_CURRENT                              0x0A            ///< current register
-#define BATT_SMBUS_AVERAGE_CURRENT                      0x0B            ///< current register
+#define BATT_SMBUS_AVERAGE_CURRENT                      0x0B            ///< average current register
+#define BATT_SMBUS_MAX_ERROR				0x0C		///< max error
+#define BATT_SMBUS_RELATIVE_SOC				0x0D		///< Relative State Of Charge
 #define BATT_SMBUS_TEMP                                 0x08            ///< temperature register
 #define BATT_SMBUS_VOLTAGE                              0x09            ///< voltage register
 #define BATT_SMBUS_FULL_CHARGE_CAPACITY                 0x10            ///< capacity when fully charged
@@ -82,6 +84,7 @@
 #define BATT_SMBUS_MANUFACTURER_ACCESS                  0x00
 #define BATT_SMBUS_MANUFACTURER_DATA                    0x23
 #define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS            0x44
+#define BATT_SMBUS_STATE_OF_HEALTH			0x4F		///< State of Health. The SOH information of the battery in percentage of Design Capacity
 #define BATT_SMBUS_SECURITY_KEYS                        0x0035
 #define BATT_SMBUS_CELL_1_VOLTAGE                       0x3F
 #define BATT_SMBUS_CELL_2_VOLTAGE                       0x3E
@@ -255,6 +258,9 @@ class BATT_SMBUS : public I2CSPIDriver<BATT_SMBUS>
 	/** @param _low_thr Low battery threshold param. */
 	float _low_thr{0.f};
 
+	/** @parama _c_mult Capacity/current multiplier param  */
+	float _c_mult{0.f};
+
 	/** @param _manufacturer_name Name of the battery manufacturer. */
 	char *_manufacturer_name{nullptr};
 

src/drivers/batt_smbus/parameters.c

@@ -40,3 +40,12 @@
  * @group Sensors
  */
 PARAM_DEFINE_INT32(SENS_EN_BATT, 0);
+
+/**
+ * Capacity/current multiplier for high-current capable SMBUS battery
+ *
+ * @reboot_required true
+ * @decimal 1
+ * @group Sensors
+ */
+PARAM_DEFINE_FLOAT(BAT_C_MULT, 1.0f);

src/modules/mavlink/mavlink_messages.cpp

@@ -708,6 +708,7 @@ class MavlinkStreamBatteryStatus : public MavlinkStream
 				bat_msg.energy_consumed = -1;
 				bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1;
 				bat_msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.0f) : -1;
+				bat_msg.time_remaining = (battery_status.connected) ? battery_status.run_time_to_empty * 60 : 0;
 
 				switch (battery_status.warning) {
 				case (battery_status_s::BATTERY_WARNING_NONE):