diff --git a/msg/battery_status.msg b/msg/battery_status.msg index f9784be011..554ce1b7be 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -1,40 +1,42 @@ -uint64 timestamp # time since system start (microseconds) -float32 voltage_v # Battery voltage in volts, 0 if unknown +uint64 timestamp # time since system start (microseconds) +float32 voltage_v # Battery voltage in volts, 0 if unknown float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown -float32 current_a # Battery current in amperes, -1 if unknown +float32 current_a # Battery current in amperes, -1 if unknown float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown -float32 average_current_a # Battery current average in amperes, -1 if unknown +float32 average_current_a # Battery current average in amperes, -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown -float32 remaining # From 1 to 0, -1 if unknown -float32 scale # Power scaling factor, >= 1, or -1 if unknown -float32 temperature # temperature of the battery. NaN if unknown -int32 cell_count # Number of cells -bool connected # Whether or not a battery is connected, based on a voltage threshold +float32 remaining # From 1 to 0, -1 if unknown +float32 scale # Power scaling factor, >= 1, or -1 if unknown +float32 temperature # temperature of the battery. NaN if unknown +int32 cell_count # Number of cells +bool connected # Whether or not a battery is connected, based on a voltage threshold uint8 BATTERY_SOURCE_POWER_MODULE = 0 uint8 BATTERY_SOURCE_EXTERNAL = 1 uint8 BATTERY_SOURCE_ESCS = 2 -uint8 source # Battery source -uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 -uint16 capacity # actual capacity of the battery -uint16 cycle_count # number of discharge cycles the battery has experienced -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. +uint8 source # Battery source +uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 +uint16 capacity # actual capacity of the battery +uint16 cycle_count # number of discharge cycles the battery has experienced +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 manufacture_date # manufacture date, part of 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. +uint16 interface_error # SMBUS interface error counter -float32[10] voltage_cell_v # Battery individual cell voltages -float32 max_cell_voltage_delta # Max difference between individual cell voltages +float32[10] voltage_cell_v # Battery individual cell voltages +float32 max_cell_voltage_delta # Max difference between individual cell voltages -bool is_powering_off # Power off event imminent indication, false if unknown +bool is_powering_off # Power off event imminent indication, false if unknown -uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active -uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage -uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately -uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required -uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely +uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active +uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage +uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately +uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required +uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely -uint8 warning # current battery warning +uint8 warning # current battery warning diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 5449d80fe6..8651c0b02c 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,16 +35,16 @@ * @file batt_smbus.h * * Header for a battery monitor connected via SMBus (I2C). - * Designed for BQ40Z50-R1/R2 + * Designed for BQ40Z50-R1/R2 and BQ40Z80 * * @author Jacob Dahl * @author Alex Klimaj + * @author Bazooka Joe + * */ #include "batt_smbus.h" -#include - extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); BATT_SMBUS::BATT_SMBUS(I2CSPIBusOption bus_option, const int bus, SMBus *interface) : @@ -55,7 +55,20 @@ BATT_SMBUS::BATT_SMBUS(I2CSPIBusOption bus_option, const int bus, SMBus *interfa _batt_topic = orb_advertise(ORB_ID(battery_status), &new_report); int battsource = 1; + int batt_device_type = (int)SMBUS_DEVICE_TYPE::UNDEFINED; + param_set(param_find("BAT_SOURCE"), &battsource); + param_get(param_find("BAT_SMBUS_MODEL"), &batt_device_type); + + + //TODO: probe the device and autodetect its type + if ((SMBUS_DEVICE_TYPE)batt_device_type == SMBUS_DEVICE_TYPE::BQ40Z80) { + _device_type = SMBUS_DEVICE_TYPE::BQ40Z80; + + } else { + //default + _device_type = SMBUS_DEVICE_TYPE::BQ40Z50; + } _interface->init(); // unseal() here to allow an external config script to write to protected flash. @@ -69,10 +82,6 @@ BATT_SMBUS::~BATT_SMBUS() orb_unadvertise(_batt_topic); perf_free(_cycle); - if (_manufacturer_name != nullptr) { - delete[] _manufacturer_name; - } - if (_interface != nullptr) { delete _interface; } @@ -83,8 +92,10 @@ BATT_SMBUS::~BATT_SMBUS() void BATT_SMBUS::RunImpl() { - // Get the current time. - uint64_t now = hrt_absolute_time(); + int ret = PX4_OK; + + // Temporary variable for storing SMBUS reads. + uint16_t result; // Read data from sensor. battery_status_s new_report = {}; @@ -93,17 +104,18 @@ void BATT_SMBUS::RunImpl() new_report.id = 1; // Set time of reading. - new_report.timestamp = now; + new_report.timestamp = hrt_absolute_time(); new_report.connected = true; - // Temporary variable for storing SMBUS reads. - uint16_t result; - - int ret = _interface->read_word(BATT_SMBUS_VOLTAGE, result); + ret |= _interface->read_word(BATT_SMBUS_VOLTAGE, result); ret |= get_cell_voltages(); + for (int i = 0; i < _cell_count; i++) { + new_report.voltage_cell_v[i] = _cell_voltages[i]; + } + // Convert millivolts to volts. new_report.voltage_v = ((float)result) / 1000.0f; new_report.voltage_filtered_v = new_report.voltage_v; @@ -137,7 +149,7 @@ void BATT_SMBUS::RunImpl() ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, result); // Calculate total discharged amount in mah. - new_report.discharged_mah = _batt_capacity - (float)result * _c_mult; + new_report.discharged_mah = _batt_startup_capacity - (float)result * _c_mult; // Read Relative SOC. ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result); @@ -145,48 +157,41 @@ void BATT_SMBUS::RunImpl() // 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) { - new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; - - } else if (new_report.remaining > _low_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_NONE; - - } else if (new_report.remaining > _crit_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_LOW; - - } else if (new_report.remaining > _emergency_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; - - } else { - new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; - } - // Read battery temperature and covert to Celsius. ret |= _interface->read_word(BATT_SMBUS_TEMP, result); new_report.temperature = ((float)result / 10.0f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; - new_report.capacity = _batt_capacity; - new_report.cycle_count = _cycle_count; - new_report.serial_number = _serial_number; - new_report.max_cell_voltage_delta = _max_cell_voltage_delta; - new_report.cell_count = _cell_count; - new_report.voltage_cell_v[0] = _cell_voltages[0]; - new_report.voltage_cell_v[1] = _cell_voltages[1]; - new_report.voltage_cell_v[2] = _cell_voltages[2]; - new_report.voltage_cell_v[3] = _cell_voltages[3]; - // Only publish if no errors. - if (!ret) { + if (ret == PX4_OK) { + new_report.capacity = _batt_capacity; + new_report.cycle_count = _cycle_count; + new_report.serial_number = _serial_number; + new_report.max_cell_voltage_delta = _max_cell_voltage_delta; + new_report.cell_count = _cell_count; + new_report.state_of_health = _state_of_health; + + // Check if max lifetime voltage delta is greater than allowed. + if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) { + new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + + } else if (new_report.remaining > _low_thr) { + new_report.warning = battery_status_s::BATTERY_WARNING_NONE; + + } else if (new_report.remaining > _crit_thr) { + new_report.warning = battery_status_s::BATTERY_WARNING_LOW; + + } else if (new_report.remaining > _emergency_thr) { + new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + + } else { + new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + } + + new_report.interface_error = perf_event_count(_interface->_interface_errors); orb_publish(ORB_ID(battery_status), _batt_topic, &new_report); _last_report = new_report; @@ -207,28 +212,63 @@ int BATT_SMBUS::get_cell_voltages() { // Temporary variable for storing SMBUS reads. uint16_t result = 0; + int ret = PX4_OK; - int ret = _interface->read_word(BATT_SMBUS_CELL_1_VOLTAGE, result); - // Convert millivolts to volts. - _cell_voltages[0] = ((float)result) / 1000.0f; + if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z50) { + ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_1_VOLTAGE, result); + // Convert millivolts to volts. + _cell_voltages[0] = ((float)result) / 1000.0f; - ret = _interface->read_word(BATT_SMBUS_CELL_2_VOLTAGE, result); - // Convert millivolts to volts. - _cell_voltages[1] = ((float)result) / 1000.0f; + ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_2_VOLTAGE, result); + // Convert millivolts to volts. + _cell_voltages[1] = ((float)result) / 1000.0f; - ret = _interface->read_word(BATT_SMBUS_CELL_3_VOLTAGE, result); - // Convert millivolts to volts. - _cell_voltages[2] = ((float)result) / 1000.0f; + ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_3_VOLTAGE, result); + // Convert millivolts to volts. + _cell_voltages[2] = ((float)result) / 1000.0f; - ret = _interface->read_word(BATT_SMBUS_CELL_4_VOLTAGE, result); - // Convert millivolts to volts. - _cell_voltages[3] = ((float)result) / 1000.0f; + ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_4_VOLTAGE, result); + // Convert millivolts to volts. + _cell_voltages[3] = ((float)result) / 1000.0f; + _cell_voltages[4] = 0; + _cell_voltages[5] = 0; + _cell_voltages[6] = 0; + + } else if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z80) { + uint8_t DAstatus1[32 + 2] = {}; // 32 bytes of data and 2 bytes of address + + //TODO: need to consider if set voltages to 0? -1? + if (PX4_OK != manufacturer_read(BATT_SMBUS_DASTATUS1, DAstatus1, sizeof(DAstatus1))) { + return PX4_ERROR; + } + + // Convert millivolts to volts. + _cell_voltages[0] = ((float)((DAstatus1[1] << 8) | DAstatus1[0]) / 1000.0f); + _cell_voltages[1] = ((float)((DAstatus1[3] << 8) | DAstatus1[2]) / 1000.0f); + _cell_voltages[2] = ((float)((DAstatus1[5] << 8) | DAstatus1[4]) / 1000.0f); + _cell_voltages[3] = ((float)((DAstatus1[7] << 8) | DAstatus1[6]) / 1000.0f); + + _pack_power = ((float)((DAstatus1[29] << 8) | DAstatus1[28]) / 100.0f); //TODO: decide if both needed + _pack_average_power = ((float)((DAstatus1[31] << 8) | DAstatus1[30]) / 100.0f); + + uint8_t DAstatus3[18 + 2] = {}; // 18 bytes of data and 2 bytes of address + + //TODO: need to consider if set voltages to 0? -1? + if (PX4_OK != manufacturer_read(BATT_SMBUS_DASTATUS3, DAstatus3, sizeof(DAstatus3))) { + return PX4_ERROR; + } + + _cell_voltages[4] = ((float)((DAstatus3[1] << 8) | DAstatus3[0]) / 1000.0f); + _cell_voltages[5] = ((float)((DAstatus3[7] << 8) | DAstatus3[6]) / 1000.0f); + _cell_voltages[6] = ((float)((DAstatus3[13] << 8) | DAstatus3[12]) / 1000.0f); + + } //Calculate max cell delta _min_cell_voltage = _cell_voltages[0]; float max_cell_voltage = _cell_voltages[0]; - for (uint8_t i = 1; i < (sizeof(_cell_voltages) / sizeof(_cell_voltages[0])); i++) { + for (uint8_t i = 1; (i < _cell_count && i < (sizeof(_cell_voltages) / sizeof(_cell_voltages[0]))); i++) { _min_cell_voltage = math::min(_min_cell_voltage, _cell_voltages[i]); max_cell_voltage = math::max(max_cell_voltage, _cell_voltages[i]); } @@ -279,29 +319,29 @@ void BATT_SMBUS::set_undervoltage_protection(float average_current) } //@NOTE: Currently unused, could be helpful for debugging a parameter set though. -int BATT_SMBUS::dataflash_read(uint16_t &address, void *data, const unsigned length) +int BATT_SMBUS::dataflash_read(const uint16_t address, void *data, const unsigned length) { uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS; - int result = _interface->block_write(code, &address, 2, true); + int ret = _interface->block_write(code, &address, 2, true); - if (result != PX4_OK) { - return result; + if (ret != PX4_OK) { + return ret; } - result = _interface->block_read(code, data, length, true); + ret = _interface->block_read(code, data, length, true); - return result; + return ret; } -int BATT_SMBUS::dataflash_write(uint16_t address, void *data, const unsigned length) +int BATT_SMBUS::dataflash_write(const uint16_t address, void *data, const unsigned length) { uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS; uint8_t tx_buf[MAC_DATA_BUFFER_SIZE + 2] = {}; - tx_buf[0] = ((uint8_t *)&address)[0]; - tx_buf[1] = ((uint8_t *)&address)[1]; + tx_buf[0] = address & 0xff; + tx_buf[1] = (address >> 8) & 0xff; if (length > MAC_DATA_BUFFER_SIZE) { return PX4_ERROR; @@ -310,17 +350,14 @@ int BATT_SMBUS::dataflash_write(uint16_t address, void *data, const unsigned len memcpy(&tx_buf[2], data, length); // code (1), byte_count (1), addr(2), data(32) + pec - int result = _interface->block_write(code, tx_buf, length + 2, false); + int ret = _interface->block_write(code, tx_buf, length + 2, false); - return result; + return ret; } 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; + int ret = PX4_OK; // Read battery threshold params on startup. param_get(param_find("BAT_CRIT_THR"), &_crit_thr); @@ -328,41 +365,44 @@ int BATT_SMBUS::get_startup_info() 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] = {}; - result = manufacturer_name((uint8_t *)man_name, sizeof(man_name)); + int32_t cell_count_param = 0; + param_get(param_find("BAT_N_CELLS"), &cell_count_param); + _cell_count = math::min((uint8_t)cell_count_param, MAX_NUM_OF_CELLS); - if (result != PX4_OK) { - PX4_DEBUG("Failed to get manufacturer name"); - return PX4_ERROR; - } - - _manufacturer_name = new char[sizeof(man_name)]; - } + ret |= _interface->block_read(BATT_SMBUS_MANUFACTURER_NAME, _manufacturer_name, BATT_SMBUS_MANUFACTURER_NAME_SIZE, + true); + _manufacturer_name[sizeof(_manufacturer_name) - 1] = '\0'; uint16_t serial_num; - result = _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num); + ret |= _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num); uint16_t remaining_cap; - result |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, remaining_cap); + ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, remaining_cap); uint16_t cycle_count; - result |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, cycle_count); + ret |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, cycle_count); uint16_t full_cap; - result |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, full_cap); + ret |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, full_cap); - if (!result) { + uint16_t manufacture_date; + ret |= _interface->read_word(BATT_SMBUS_MANUFACTURE_DATE, manufacture_date); + + uint16_t state_of_health; + ret |= _interface->read_word(BATT_SMBUS_STATE_OF_HEALTH, state_of_health); + + if (!ret) { _serial_number = serial_num; _batt_startup_capacity = (uint16_t)((float)remaining_cap * _c_mult); _cycle_count = cycle_count; _batt_capacity = (uint16_t)((float)full_cap * _c_mult); + _manufacture_date = manufacture_date; + _state_of_health = state_of_health; } if (lifetime_data_flush() == PX4_OK) { // Flush needs time to complete, otherwise device is busy. 100ms not enough, 200ms works. - px4_usleep(200000); + px4_usleep(200_ms); if (lifetime_read_block_one() == PX4_OK) { if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) { @@ -375,45 +415,7 @@ int BATT_SMBUS::get_startup_info() PX4_WARN("Failed to flush lifetime data"); } - return result; -} - -uint16_t BATT_SMBUS::get_serial_number() -{ - uint16_t serial_num = 0; - - if (_interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num) == PX4_OK) { - return serial_num; - } - - return PX4_ERROR; -} - -int BATT_SMBUS::manufacture_date() -{ - uint16_t date; - int result = _interface->read_word(BATT_SMBUS_MANUFACTURE_DATE, date); - - if (result != PX4_OK) { - return result; - } - - return date; -} - -int BATT_SMBUS::manufacturer_name(uint8_t *man_name, const uint8_t length) -{ - uint8_t code = BATT_SMBUS_MANUFACTURER_NAME; - uint8_t rx_buf[21] = {}; - - // Returns 21 bytes, add 1 byte for null terminator. - int result = _interface->block_read(code, rx_buf, length - 1, true); - - memcpy(man_name, rx_buf, sizeof(rx_buf)); - - man_name[21] = '\0'; - - return result; + return ret; } int BATT_SMBUS::manufacturer_read(const uint16_t cmd_code, void *data, const unsigned length) @@ -424,36 +426,33 @@ int BATT_SMBUS::manufacturer_read(const uint16_t cmd_code, void *data, const uns address[0] = ((uint8_t *)&cmd_code)[0]; address[1] = ((uint8_t *)&cmd_code)[1]; - int result = _interface->block_write(code, address, 2, false); + int ret = _interface->block_write(code, address, 2, false); - if (result != PX4_OK) { - return result; + if (ret != PX4_OK) { + return ret; } - result = _interface->block_read(code, data, length, true); + ret = _interface->block_read(code, data, length, true); memmove(data, &((uint8_t *)data)[2], length - 2); // remove the address bytes - return result; + return ret; } int BATT_SMBUS::manufacturer_write(const uint16_t cmd_code, void *data, const unsigned length) { uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS; - uint8_t address[2] = {}; - address[0] = ((uint8_t *)&cmd_code)[0]; - address[1] = ((uint8_t *)&cmd_code)[1]; - uint8_t tx_buf[MAC_DATA_BUFFER_SIZE + 2] = {}; - memcpy(tx_buf, address, 2); + tx_buf[0] = cmd_code & 0xff; + tx_buf[1] = (cmd_code >> 8) & 0xff; - if (data != nullptr) { + if (data != nullptr && length <= MAC_DATA_BUFFER_SIZE) { memcpy(&tx_buf[2], data, length); } - int result = _interface->block_write(code, tx_buf, length + 2, false); + int ret = _interface->block_write(code, tx_buf, length + 2, false); - return result; + return ret; } int BATT_SMBUS::unseal() @@ -471,30 +470,32 @@ int BATT_SMBUS::unseal() int BATT_SMBUS::seal() { // See bq40z50 technical reference. - uint16_t reg = BATT_SMBUS_SEAL; - - return manufacturer_write(reg, nullptr, 0); + return manufacturer_write(BATT_SMBUS_SEAL, nullptr, 0); } int BATT_SMBUS::lifetime_data_flush() { - uint16_t flush = BATT_SMBUS_LIFETIME_FLUSH; - - return manufacturer_write(flush, nullptr, 0); + return manufacturer_write(BATT_SMBUS_LIFETIME_FLUSH, nullptr, 0); } int BATT_SMBUS::lifetime_read_block_one() { - const int buffer_size = 32 + 2; // 32 bytes of data and 2 bytes of address - uint8_t lifetime_block_one[buffer_size] = {}; + uint8_t lifetime_block_one[32 + 2] = {}; // 32 bytes of data and 2 bytes of address - if (PX4_OK != manufacturer_read(BATT_SMBUS_LIFETIME_BLOCK_ONE, lifetime_block_one, buffer_size)) { + if (PX4_OK != manufacturer_read(BATT_SMBUS_LIFETIME_BLOCK_ONE, lifetime_block_one, sizeof(lifetime_block_one))) { PX4_INFO("Failed to read lifetime block 1."); return PX4_ERROR; } //Get max cell voltage delta and convert from mV to V. - _lifetime_max_delta_cell_voltage = (float)(lifetime_block_one[17] << 8 | lifetime_block_one[16]) / 1000.0f; + if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z50) { + + _lifetime_max_delta_cell_voltage = (float)(lifetime_block_one[17] << 8 | lifetime_block_one[16]) / 1000.0f; + + } else if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z80) { + + _lifetime_max_delta_cell_voltage = (float)(lifetime_block_one[29] << 8 | lifetime_block_one[28]) / 1000.0f; + } PX4_INFO("Max Cell Delta: %4.2f", (double)_lifetime_max_delta_cell_voltage); @@ -548,9 +549,9 @@ I2CSPIDriverBase *BATT_SMBUS::instantiate(const BusCLIArguments &cli, const BusI return nullptr; } - int result = instance->get_startup_info(); + int ret = instance->get_startup_info(); - if (result != PX4_OK) { + if (ret != PX4_OK) { delete instance; return nullptr; } @@ -565,16 +566,9 @@ BATT_SMBUS::custom_method(const BusCLIArguments &cli) { switch(cli.custom1) { case 1: { - uint8_t man_name[22]; - int result = manufacturer_name(man_name, sizeof(man_name)); - PX4_INFO("The manufacturer name: %s", man_name); - - result = manufacture_date(); - PX4_INFO("The manufacturer date: %d", result); - - uint16_t serial_num = 0; - serial_num = get_serial_number(); - PX4_INFO("The serial number: %d", serial_num); + PX4_INFO("The manufacturer name: %s", _manufacturer_name); + PX4_INFO("The manufacturer date: %d", _manufacture_date); + PX4_INFO("The serial number: %d", _serial_number); } break; case 2: @@ -598,7 +592,7 @@ BATT_SMBUS::custom_method(const BusCLIArguments &cli) if (PX4_OK != dataflash_write(address, tx_buf+1, length)) { PX4_ERR("Dataflash write failed: %d", address); } - px4_usleep(100000); + px4_usleep(100_ms); } break; } diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h index 77d9e3daaa..1babc9c47f 100644 --- a/src/drivers/batt_smbus/batt_smbus.h +++ b/src/drivers/batt_smbus/batt_smbus.h @@ -35,10 +35,11 @@ * @file batt_smbus.h * * Header for a battery monitor connected via SMBus (I2C). - * Designed for BQ40Z50-R1/R2 + * Designed for BQ40Z50-R1/R2 or BQ40Z80 * * @author Jacob Dahl * @author Alex Klimaj + * @author Bazooka Joe */ #pragma once @@ -48,12 +49,17 @@ #include #include #include +#include #include #include #include #include +using namespace time_literals; + +#define BATT_SMBUS_MEASUREMENT_INTERVAL_US 100_ms ///< time in microseconds, measure at 10Hz + #define MAC_DATA_BUFFER_SIZE 32 #define BATT_CELL_VOLTAGE_THRESHOLD_RTL 0.5f ///< Threshold in volts to RTL if cells are imbalanced @@ -64,40 +70,61 @@ #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 ///< 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_CURRENT 0x0A ///< 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_ABSOLUTE_SOC 0x0E ///< Absolute State of charge +#define BATT_SMBUS_REMAINING_CAPACITY 0x0F ///< predicted remaining battery capacity as a percentage #define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged #define BATT_SMBUS_RUN_TIME_TO_EMPTY 0x11 ///< predicted remaining battery capacity based on the present rate of discharge in min #define BATT_SMBUS_AVERAGE_TIME_TO_EMPTY 0x12 ///< predicted remaining battery capacity based on the present rate of discharge in min -#define BATT_SMBUS_REMAINING_CAPACITY 0x0F ///< predicted remaining battery capacity as a percentage #define BATT_SMBUS_CYCLE_COUNT 0x17 ///< number of cycles the battery has experienced #define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register #define BATT_SMBUS_MANUFACTURER_NAME 0x20 ///< manufacturer name +#define BATT_SMBUS_MANUFACTURER_NAME_SIZE 21 ///< manufacturer name data size #define BATT_SMBUS_MANUFACTURE_DATE 0x1B ///< manufacture date register #define BATT_SMBUS_SERIAL_NUMBER 0x1C ///< serial number register -#define BATT_SMBUS_MEASUREMENT_INTERVAL_US 100000 ///< time in microseconds, measure at 10Hz + +#define BATT_SMBUS_BQ40Z50_CELL_4_VOLTAGE 0x3C +#define BATT_SMBUS_BQ40Z50_CELL_3_VOLTAGE 0x3D +#define BATT_SMBUS_BQ40Z50_CELL_2_VOLTAGE 0x3E +#define BATT_SMBUS_BQ40Z50_CELL_1_VOLTAGE 0x3F + +#define BATT_SMBUS_BQ40Z80_CELL_7_VOLTAGE 0x3C +#define BATT_SMBUS_BQ40Z80_CELL_6_VOLTAGE 0x3D +#define BATT_SMBUS_BQ40Z80_CELL_5_VOLTAGE 0x3E +#define BATT_SMBUS_BQ40Z80_CELL_4_VOLTAGE 0x3F + +#define BATT_SMBUS_STATE_OF_HEALTH 0x4F ///< State of Health. The SOH information of the battery in percentage of Design Capacity + #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 -#define BATT_SMBUS_CELL_3_VOLTAGE 0x3D -#define BATT_SMBUS_CELL_4_VOLTAGE 0x3C + #define BATT_SMBUS_LIFETIME_FLUSH 0x002E #define BATT_SMBUS_LIFETIME_BLOCK_ONE 0x0060 #define BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS 0x4938 #define BATT_SMBUS_SEAL 0x0030 +#define BATT_SMBUS_DASTATUS1 0x0071 +#define BATT_SMBUS_DASTATUS2 0x0072 +#define BATT_SMBUS_DASTATUS3 0x007B #define BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT 0xcf #define BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED 0xce + +enum class SMBUS_DEVICE_TYPE { + UNDEFINED = 0, + BQ40Z50 = 1, + BQ40Z80 = 2, +}; + class BATT_SMBUS : public I2CSPIDriver { public: @@ -121,7 +148,7 @@ public: * @param data The returned data. * @return Returns PX4_OK on success, PX4_ERROR on failure. */ - int dataflash_read(uint16_t &address, void *data, const unsigned length); + int dataflash_read(const uint16_t address, void *data, const unsigned length); /** * @brief Writes data to flash. @@ -130,13 +157,7 @@ public: * @param length The number of bytes being written. * @return Returns PX4_OK on success, PX4_ERROR on failure. */ - int dataflash_write(uint16_t address, void *data, const unsigned length); - - /** - * @brief Returns the SBS serial number of the battery device. - * @return Returns the SBS serial number of the battery device. - */ - uint16_t get_serial_number(); + int dataflash_write(const uint16_t address, void *data, const unsigned length); /** * @brief Read info from battery on startup. @@ -144,21 +165,6 @@ public: */ int get_startup_info(); - /** - * @brief Gets the SBS manufacture date of the battery. - * @return Returns PX4_OK on success, PX4_ERROR on failure. - */ - int manufacture_date(); - - /** - * @brief Gets the SBS manufacturer name of the battery device. - * @param manufacturer_name Pointer to a buffer into which the manufacturer name is to be written. - * @param max_length The maximum number of bytes to attempt to read from the manufacturer name register, - * including the null character that is appended to the end. - * @return Returns PX4_OK on success, PX4_ERROR on failure. - */ - int manufacturer_name(uint8_t *manufacturer_name, const uint8_t length); - /** * @brief Performs a ManufacturerBlockAccess() read command. * @param cmd_code The command code. @@ -220,14 +226,20 @@ private: SMBus *_interface; + SMBUS_DEVICE_TYPE _device_type{SMBUS_DEVICE_TYPE::UNDEFINED}; + perf_counter_t _cycle{perf_alloc(PC_ELAPSED, "batt_smbus_cycle")}; - float _cell_voltages[4] {}; + static const uint8_t MAX_NUM_OF_CELLS = 7; + float _cell_voltages[MAX_NUM_OF_CELLS] {}; float _max_cell_voltage_delta{0}; float _min_cell_voltage{0}; + float _pack_power{0}; + float _pack_average_power{0}; + /** @param _last_report Last published report, used for test(). */ battery_status_s _last_report{}; @@ -235,7 +247,7 @@ private: orb_advert_t _batt_topic{nullptr}; /** @param _cell_count Number of series cell. */ - uint8_t _cell_count{4}; + uint8_t _cell_count{0}; /** @param _batt_capacity Battery design capacity in mAh (0 means unknown). */ uint16_t _batt_capacity{0}; @@ -262,7 +274,13 @@ private: float _c_mult{0.f}; /** @param _manufacturer_name Name of the battery manufacturer. */ - char *_manufacturer_name{nullptr}; + char _manufacturer_name[BATT_SMBUS_MANUFACTURER_NAME_SIZE + 1] {}; // Plus one for terminator + + /** @param _manufacture_date Date of the battery manufacturing. */ + uint16_t _manufacture_date{0}; + + /** @param _state_of_health state of health as read on connection */ + float _state_of_health{0.f}; /** @param _lifetime_max_delta_cell_voltage Max lifetime delta of the battery cells */ float _lifetime_max_delta_cell_voltage{0.f}; diff --git a/src/drivers/batt_smbus/parameters.c b/src/drivers/batt_smbus/parameters.c index 3634c2471c..dd3c9d22bf 100644 --- a/src/drivers/batt_smbus/parameters.c +++ b/src/drivers/batt_smbus/parameters.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * SMBUS Smart battery driver (BQ40Z50) + * SMBUS Smart battery driver BQ40Z50 and BQ40Z80 * * @reboot_required true * @@ -49,3 +49,16 @@ PARAM_DEFINE_INT32(SENS_EN_BATT, 0); * @group Sensors */ PARAM_DEFINE_FLOAT(BAT_C_MULT, 1.0f); + +/** + * Battery device model + * + * @reboot_required true + * @min 0 + * @max 2 + * @group Sensors + * @value 0 AutoDetect + * @value 1 BQ40Z50 based + * @value 2 BQ40Z80 based + */ +PARAM_DEFINE_INT32(BAT_SMBUS_MODEL, 0); diff --git a/src/lib/drivers/smbus/SMBus.cpp b/src/lib/drivers/smbus/SMBus.cpp index 4da7f5b7e5..b227260115 100644 --- a/src/lib/drivers/smbus/SMBus.cpp +++ b/src/lib/drivers/smbus/SMBus.cpp @@ -36,6 +36,7 @@ * SMBus v2.0 protocol implementation. * * @author Jacob Dahl + * @author Bazooka Joe * * TODO * - Enable SMBus mode at the NuttX level. This may be tricky sharing the bus with i2c. @@ -50,6 +51,11 @@ SMBus::SMBus(int bus_num, uint16_t address) : { } +SMBus::~SMBus() +{ + perf_free(_interface_errors); +} + int SMBus::read_word(const uint8_t cmd_code, uint16_t &data) { uint8_t buf[6]; @@ -68,7 +74,11 @@ int SMBus::read_word(const uint8_t cmd_code, uint16_t &data) if (pec != buf[sizeof(buf) - 1]) { result = -EINVAL; + perf_count(_interface_errors); } + + } else { + perf_count(_interface_errors); } return result; @@ -87,10 +97,14 @@ int SMBus::write_word(const uint8_t cmd_code, uint16_t data) int result = transfer(&buf[1], 4, nullptr, 0); + if (result != PX4_OK) { + perf_count(_interface_errors); + } + return result; } -int SMBus::block_read(const uint8_t cmd_code, void *data, const uint8_t length, bool use_pec) +int SMBus::block_read(const uint8_t cmd_code, void *data, const uint8_t length, const bool use_pec) { unsigned byte_count = 0; // addr(wr), cmd_code, addr(r), byte_count, data (32 bytes max), pec @@ -99,6 +113,7 @@ int SMBus::block_read(const uint8_t cmd_code, void *data, const uint8_t length, int result = transfer(&cmd_code, 1, (uint8_t *)&rx_data[3], length + 2); if (result != PX4_OK) { + perf_count(_interface_errors); return result; } @@ -115,13 +130,14 @@ int SMBus::block_read(const uint8_t cmd_code, void *data, const uint8_t length, if (pec != rx_data[byte_count + 4]) { result = -EINVAL; + perf_count(_interface_errors); } } return result; } -int SMBus::block_write(const uint8_t cmd_code, void *data, uint8_t byte_count, bool use_pec) +int SMBus::block_write(const uint8_t cmd_code, const void *data, uint8_t byte_count, const bool use_pec) { // cmd code[1], byte count[1], data[byte_count] (32max), pec[1] (optional) uint8_t buf[32 + 2]; @@ -141,11 +157,11 @@ int SMBus::block_write(const uint8_t cmd_code, void *data, uint8_t byte_count, b // If block_write fails, try up to 10 times. while (i < 10 && ((result = transfer((uint8_t *)buf, byte_count + 2, nullptr, 0)) != PX4_OK)) { + perf_count(_interface_errors); i++; } if (i == 10 || result) { - PX4_WARN("Block_write failed %d times", i); result = -EINVAL; } @@ -154,6 +170,8 @@ int SMBus::block_write(const uint8_t cmd_code, void *data, uint8_t byte_count, b uint8_t SMBus::get_pec(uint8_t *buff, const uint8_t len) { + // TODO: use "return crc8ccitt(buff, len);" + // Initialise CRC to zero. uint8_t crc = 0; uint8_t shift_register = 0; diff --git a/src/lib/drivers/smbus/SMBus.hpp b/src/lib/drivers/smbus/SMBus.hpp index 2a3f70adc3..dbd44dfeb0 100644 --- a/src/lib/drivers/smbus/SMBus.hpp +++ b/src/lib/drivers/smbus/SMBus.hpp @@ -36,10 +36,11 @@ * SMBus v2.0 protocol implementation. * * @author Jacob Dahl + * @author Bazooka Joe */ #include - +#include #include #define SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC @@ -48,7 +49,7 @@ class SMBus : public device::I2C { public: SMBus(int bus_num, uint16_t address); - ~SMBus() override = default; + ~SMBus() override; /** * @brief Sends a block write command. @@ -57,7 +58,7 @@ public: * @param length The number of bytes being written. * @return Returns PX4_OK on success, -errno on failure. */ - int block_write(const uint8_t cmd_code, void *data, uint8_t byte_count, bool use_pec); + int block_write(const uint8_t cmd_code, const void *data, uint8_t byte_count, const bool use_pec); /** * @brief Sends a block read command. @@ -66,7 +67,7 @@ public: * @param length The number of bytes being read. * @return Returns PX4_OK on success, -errno on failure. */ - int block_read(const uint8_t cmd_code, void *data, const uint8_t length, bool use_pec); + int block_read(const uint8_t cmd_code, void *data, const uint8_t length, const bool use_pec); /** * @brief Sends a read word command. @@ -92,4 +93,6 @@ public: */ uint8_t get_pec(uint8_t *buffer, uint8_t length); + perf_counter_t _interface_errors{perf_alloc(PC_COUNT, MODULE_NAME": errors")}; + }; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1596186d0a..46e2e82b61 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -772,6 +772,100 @@ protected: } }; + +class MavlinkStreamSmartBatteryInfo : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamSmartBatteryInfo::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "SMART_BATTERY_INFO"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_SMART_BATTERY_INFO; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamSmartBatteryInfo(mavlink); + } + + unsigned get_size() override + { + unsigned total_size = 0; + + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (_battery_status_sub[i].advertised()) { + total_size += MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + } + + return total_size; + } + +private: + uORB::Subscription _battery_status_sub[ORB_MULTI_MAX_INSTANCES] { + {ORB_ID(battery_status), 0}, {ORB_ID(battery_status), 1}, {ORB_ID(battery_status), 2}, {ORB_ID(battery_status), 3} + }; + + /* do not allow top copying this class */ + MavlinkStreamSmartBatteryInfo(MavlinkStreamSysStatus &) = delete; + MavlinkStreamSmartBatteryInfo &operator = (const MavlinkStreamSysStatus &) = delete; + +protected: + explicit MavlinkStreamSmartBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) + { + } + + bool send(const hrt_abstime t) override + { + bool updated = false; + + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + battery_status_s battery_status; + + if (_battery_status_sub[i].update(&battery_status)) { + if (battery_status.serial_number == 0) { + //This is not smart battery + continue; + } + + mavlink_smart_battery_info_t msg = {}; + + msg.id = battery_status.id - 1; + msg.capacity_full_specification = battery_status.capacity; + msg.capacity_full = (int32_t)((float)(battery_status.state_of_health * battery_status.capacity) / 100.f); + msg.cycle_count = battery_status.cycle_count; + msg.serial_number = battery_status.serial_number + (battery_status.manufacture_date << 16); + //msg.device_name = ?? + msg.weight = -1; + msg.discharge_minimum_voltage = -1; + msg.charging_minimum_voltage = -1; + msg.resting_minimum_voltage = -1; + + + mavlink_msg_smart_battery_info_send_struct(_mavlink->get_channel(), &msg); + + updated = true; + } + + } + + return updated; + } +}; + class MavlinkStreamHighresIMU : public MavlinkStream { public: @@ -5249,6 +5343,7 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), create_stream_list_item(), create_stream_list_item(), + create_stream_list_item(), create_stream_list_item(), create_stream_list_item(), create_stream_list_item(),