diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 1579352342..2cd57c997b 100644 --- a/msg/battery_status.msg +++ b/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 diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 582c22c193..5449d80fe6 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/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,13 +111,13 @@ 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; @@ -122,23 +125,34 @@ void BATT_SMBUS::RunImpl() // 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; - // Calculate total discharged amount. - new_report.discharged_mah = _batt_startup_capacity - result; + // 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; + + // 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; } diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h index 622132fe8d..77d9e3daaa 100644 --- a/src/drivers/batt_smbus/batt_smbus.h +++ b/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 @@ private: /** @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}; diff --git a/src/drivers/batt_smbus/parameters.c b/src/drivers/batt_smbus/parameters.c index bd6cabc90c..3634c2471c 100644 --- a/src/drivers/batt_smbus/parameters.c +++ b/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); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7dffaa69ab..711332f4f3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -708,6 +708,7 @@ protected: 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):