diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 1690eb654b..898326287f 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -18,6 +18,8 @@ uint16 run_time_to_empty # predicted remaining battery capacity based on the p 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 +float32[4] 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 diff --git a/src/drivers/batt_smbus/CMakeLists.txt b/src/drivers/batt_smbus/CMakeLists.txt index 634eaec7a3..1c8ff57c14 100644 --- a/src/drivers/batt_smbus/CMakeLists.txt +++ b/src/drivers/batt_smbus/CMakeLists.txt @@ -36,8 +36,8 @@ px4_add_module( STACK_MAIN 1100 COMPILE_FLAGS SRCS - batt_smbus_main.cpp batt_smbus.cpp - batt_smbus_i2c.cpp + DEPENDS + drivers__smbus ) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 4652d01caa..7be4615283 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -37,21 +37,24 @@ * Driver for a battery monitor connected via SMBus (I2C). * Designed for BQ40Z50-R1/R2 * - * @author Randy Mackay * @author Alex Klimaj - * @author Mark Sauder * @author Jacob Dahl */ #include +#include #include "batt_smbus.h" -BATT_SMBUS::BATT_SMBUS(device::Device *interface, const char *path) : - CDev(path), +extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); + +struct work_s BATT_SMBUS::_work = {}; + +BATT_SMBUS::BATT_SMBUS(SMBus *interface, const char *path) : _interface(interface), + _cycle(perf_alloc(PC_ELAPSED, "batt_smbus_cycle")), _batt_topic(nullptr), - _batt_orb_id(nullptr), + _cell_count(4), _batt_capacity(0), _batt_startup_capacity(0), _cycle_count(0), @@ -59,101 +62,107 @@ BATT_SMBUS::BATT_SMBUS(device::Device *interface, const char *path) : _crit_thr(0.0f), _emergency_thr(0.0f), _low_thr(0.0f), - _manufacturer_name(nullptr) + _manufacturer_name(nullptr), + _lifetime_max_delta_cell_voltage(0.0f), + _cell_undervoltage_protection_status(1) { + battery_status_s new_report = {}; + _batt_topic = orb_advertise(ORB_ID(battery_status), &new_report); + + int battsource = 1; + param_set(param_find("BAT_SOURCE"), &battsource); + + _interface->init(); } BATT_SMBUS::~BATT_SMBUS() { - // Ensure we are truly inactive. - stop(); + orb_unadvertise(_batt_topic); + perf_free(_cycle); if (_manufacturer_name != nullptr) { delete[] _manufacturer_name; } - PX4_WARN("Smart battery driver stopped"); + if (_interface != nullptr) { + delete _interface; + } int battsource = 0; param_set(param_find("BAT_SOURCE"), &battsource); + + PX4_WARN("Exiting."); } -int BATT_SMBUS::block_read(const uint8_t cmd_code, void *data, const unsigned length) +int BATT_SMBUS::task_spawn(int argc, char *argv[]) { - unsigned byte_count = 0; - // Length of data (32max). byte_count(1), cmd_code(2), pec(1) - uint8_t rx_data[DATA_BUFFER_SIZE + 4]; + enum BATT_SMBUS_BUS busid = BATT_SMBUS_BUS_ALL; + int ch; - // If this is a ManufacturerBlockAccess() command then the first 2 data bytes will be the cmd_code. - if (cmd_code == BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS) { - _interface->read(cmd_code, rx_data, length + 4); + while ((ch = getopt(argc, argv, "XTRIA:")) != EOF) { + switch (ch) { + case 'X': + busid = BATT_SMBUS_BUS_I2C_EXTERNAL; + break; - byte_count = rx_data[0]; + case 'T': + busid = BATT_SMBUS_BUS_I2C_EXTERNAL1; + break; - // addr1, addr2, byte_count, - memcpy(data, &rx_data[3], byte_count); + case 'R': + busid = BATT_SMBUS_BUS_I2C_EXTERNAL2; + break; - } else { - // byte_count(1) + data[32max] + pec(1) - _interface->read(cmd_code, rx_data, length + 2); + case 'I': + busid = BATT_SMBUS_BUS_I2C_INTERNAL; + break; - byte_count = rx_data[0]; + case 'A': + busid = BATT_SMBUS_BUS_ALL; + break; - memcpy(data, &rx_data[1], byte_count); - } - - // addr(wr), cmd_code, addr(r), byte_count, rx_data[] - uint8_t device_address = _interface->get_device_address(); - uint8_t full_data_packet[DATA_BUFFER_SIZE + 4] = {0}; - - full_data_packet[0] = (device_address << 1) | 0x00; - full_data_packet[1] = cmd_code; - full_data_packet[2] = (device_address << 1) | 0x01; - full_data_packet[3] = byte_count; - - memcpy(&full_data_packet[4], &rx_data[1], byte_count); - - uint8_t pec = get_pec(full_data_packet, byte_count + 4); - - // First byte is byte count, followed by data. - if (pec != ((uint8_t *)rx_data)[byte_count + 1]) { - PX4_INFO("bad PEC from block_read"); - return PX4_ERROR; - - } else { - return PX4_OK; - } -} - -int BATT_SMBUS::block_write(const uint8_t cmd_code, void *data, const unsigned byte_count) -{ - // cmd code, byte count, data[byte_count], pec - uint8_t buf[byte_count + 2]; - - buf[0] = cmd_code; - buf[1] = (uint8_t)byte_count; - memcpy(&buf[2], data, byte_count); - - uint8_t pec = get_pec(buf, sizeof(buf)); - buf[byte_count + 2] = pec; - - unsigned i = 0; - - // If block_write fails, try up to 10 times. - while (i < 10) { - if (PX4_OK != _interface->write(0, buf, sizeof(buf))) { - i++; - PX4_WARN("block_write failed: %d", i); - usleep(100000); - - } else { - return PX4_OK; + default: + print_usage(); + return PX4_ERROR; } } + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + + if (_object == nullptr && (busid == BATT_SMBUS_BUS_ALL || bus_options[i].busid == busid)) { + + SMBus *interface = new SMBus(bus_options[i].busnum, BATT_SMBUS_ADDR); + BATT_SMBUS *dev = new BATT_SMBUS(interface, bus_options[i].devpath); + + + // Successful read of device type, we've found our battery + _object = dev; + _task_id = task_id_is_work_queue; + + int result = dev->get_startup_info(); + + if (result != PX4_OK) { + return PX4_ERROR; + } + + // Throw it into the work queue. + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, dev, 0); + + return PX4_OK; + + } + } + + PX4_WARN("Not found."); return PX4_ERROR; } +void BATT_SMBUS::cycle_trampoline(void *arg) +{ + BATT_SMBUS *dev = (BATT_SMBUS *)arg; + dev->cycle(); +} + void BATT_SMBUS::cycle() { // Get the current time. @@ -165,173 +174,204 @@ void BATT_SMBUS::cycle() // Set time of reading. new_report.timestamp = now; - // Don't publish if any reads fail. - bool success = true; + new_report.connected = true; // Temporary variable for storing SMBUS reads. - uint16_t tmp; + uint16_t result; - if (read_word(BATT_SMBUS_VOLTAGE, &tmp) == PX4_OK) { + int ret = _interface->read_word(BATT_SMBUS_VOLTAGE, &result); - new_report.connected = true; + ret |= get_cell_voltages(); - // Convert millivolts to volts. - new_report.voltage_v = ((float)tmp) / 1000.0f; - new_report.voltage_filtered_v = new_report.voltage_v; + // Convert millivolts to volts. + new_report.voltage_v = ((float)result) / 1000.0f; + new_report.voltage_filtered_v = new_report.voltage_v; - // Read current. - if (read_word(BATT_SMBUS_CURRENT, &tmp) == PX4_OK) { - new_report.current_a = (-1.0f * ((float)(*(int16_t *)&tmp)) / 1000.0f); - new_report.current_filtered_a = new_report.current_a; + // Read current. + ret |= _interface->read_word(BATT_SMBUS_CURRENT, &result); - } else { - success = false; - } + new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f); + new_report.current_filtered_a = new_report.current_a; - // Read average current. - if (read_word(BATT_SMBUS_AVERAGE_CURRENT, &tmp) == PX4_OK) { - new_report.average_current_a = (-1.0f * ((float)(*(int16_t *)&tmp)) / 1000.0f); + // Read average current. + ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, &result); - } else { - success = false; - } + float average_current = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f); - // Read run time to empty. - if (read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, &tmp) == PX4_OK) { - new_report.run_time_to_empty = tmp; + new_report.average_current_a = average_current; - } else { - success = false; - } + // If current is high, turn under voltage protection off. + set_undervoltage_protection(average_current); - // Read average time to empty. - if (read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, &tmp) == PX4_OK) { - new_report.average_time_to_empty = tmp; + // Read run time to empty. + ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, &result); + new_report.run_time_to_empty = result; - } else { - success = false; - } + // Read average time to empty. + ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, &result); + new_report.average_time_to_empty = result; - // Read remaining capacity. - if (read_word(BATT_SMBUS_REMAINING_CAPACITY, &tmp) == PX4_OK) { - - if (tmp > _batt_capacity) { - PX4_WARN("Remaining capacity greater than total: Capacity:%hu \tRemaining Capacity:%hu", - (uint16_t)_batt_capacity, (uint16_t)tmp); - _batt_capacity = (uint16_t)tmp; - } - - // Calculate remaining capacity percent with complementary filter - new_report.remaining = ((float)_last_report.remaining * 0.8f) + (0.2f * (1.0f - - (((float)_batt_capacity - (float)tmp) / (float)_batt_capacity))); - - // Calculate total discharged amount. - new_report.discharged_mah = (float)_batt_startup_capacity - (float)tmp; - - // Check if remaining % is out of range. - if ((new_report.remaining > 1.00f) || (new_report.remaining <= 0.00f)) { - new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; - PX4_INFO("Percent out of range: %4.2f", (double)new_report.remaining); - } - - // Check if discharged amount is greater than the starting capacity. - else if (new_report.discharged_mah > (float)_batt_startup_capacity) { - new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; - PX4_INFO("Discharged greater than startup capacity: %4.2f", (double)new_report.discharged_mah); - } - - // Propagate warning state. - 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 { - uint64_t timer = hrt_absolute_time() - now; - new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; - - /* Only warn every 5 seconds */ - if (timer > 5000000) { - PX4_WARN("Battery Warning Emergency: %4.2f", (double)new_report.remaining); - } - } - } - - } else { - success = false; - } - - // Read battery temperature and covert to Celsius. - if (read_word(BATT_SMBUS_TEMP, &tmp) == PX4_OK) { - new_report.temperature = (float)(((float)tmp / 10.0f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS); - - } else { - success = false; - } - - new_report.capacity = _batt_capacity; - new_report.cycle_count = _cycle_count; - new_report.serial_number = _serial_number; - - // Publish to orb. - if (_batt_topic != nullptr && success) { - - orb_publish(_batt_orb_id, _batt_topic, &new_report); - - // Copy report for test(). - _last_report = new_report; - - - } else { - _batt_topic = orb_advertise(_batt_orb_id, &new_report); - - if (_batt_topic == nullptr) { - PX4_ERR("ADVERT FAIL"); - return; - } - } + // Read remaining capacity. + ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, &result); + if (result > _batt_capacity) { + _batt_capacity = result; } - // Schedule a fresh cycle call when the measurement is done. - work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, - USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_US)); + // 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. + new_report.discharged_mah = _batt_startup_capacity - 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; + } + + // Propagate warning state. + 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.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) { + orb_publish(ORB_ID(battery_status), _batt_topic, &new_report); + + _last_report = new_report; + } + + if (should_exit()) { + exit_and_cleanup(); + + } else { + // Schedule a fresh cycle call when the measurement is done. + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, + USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_US)); + } } -void BATT_SMBUS::cycle_trampoline(void *arg) +int BATT_SMBUS::get_cell_voltages() { - BATT_SMBUS *dev = (BATT_SMBUS *)arg; - dev->cycle(); + // Temporary variable for storing SMBUS reads. + uint16_t result = 0; + + int ret = _interface->read_word(BATT_SMBUS_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_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; + + //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)); i++) { + _min_cell_voltage = math::min(_min_cell_voltage, _cell_voltages[i]); + max_cell_voltage = math::max(_min_cell_voltage, _cell_voltages[i]); + } + + // Calculate the max difference between the min and max cells with complementary filter. + _max_cell_voltage_delta = (0.5f * (max_cell_voltage - _min_cell_voltage)) + + (0.5f * _last_report.max_cell_voltage_delta); + + return ret; } +void BATT_SMBUS::set_undervoltage_protection(float average_current) +{ + // Disable undervoltage protection if armed. Enable if disarmed and cell voltage is above limit. + if (average_current > BATT_CURRENT_UNDERVOLTAGE_THRESHOLD) { + if (_cell_undervoltage_protection_status != 0) { + // Disable undervoltage protection + uint8_t protections_a_tmp = BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED; + uint16_t address = BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS; + + if (write_flash(address, &protections_a_tmp, 1) == PX4_OK) { + _cell_undervoltage_protection_status = 0; + PX4_WARN("Disabled CUV"); + } + } + + } else { + if (_cell_undervoltage_protection_status == 0) { + if (_min_cell_voltage > BATT_VOLTAGE_UNDERVOLTAGE_THRESHOLD) { + // Enable undervoltage protection + uint8_t protections_a_tmp = BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT; + uint16_t address = BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS; + + if (write_flash(address, &protections_a_tmp, 1) == PX4_OK) { + _cell_undervoltage_protection_status = 1; + PX4_WARN("Enabled CUV"); + + } + } + } + } + +} + +//@NOTE: Currently unused, could be helpful for debugging a parameter set though. int BATT_SMBUS::dataflash_read(uint16_t &address, void *data) { uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS; // address is 2 bytes - block_write(code, &address, 2); - // @NOTE: The data buffer MUST be 32 bytes. - block_read(code, data, DATA_BUFFER_SIZE); + int result = _interface->block_write(code, &address, 2, false); - // for debug only: print out the receieved buffer - for (unsigned i = 0; i < DATA_BUFFER_SIZE ; i++) { - PX4_INFO("%d", ((uint8_t *)data)[i]); + if (result != PX4_OK) { + return result; } - return PX4_OK; -} + // @NOTE: The data buffer MUST be 32 bytes. + result = _interface->block_read(code, data, DATA_BUFFER_SIZE + 2, true); + // When reading a BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS the first 2 bytes will be the command code + // We will remove these since we do not care about the command code. + memcpy(data, &((uint8_t *)data)[2], DATA_BUFFER_SIZE); + + return result; +} int BATT_SMBUS::dataflash_write(uint16_t &address, void *data, const unsigned length) { uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS; - // @NOTE: The data buffer can be 1 - 32 bytes. The address field is 2 bytes. uint8_t tx_buf[DATA_BUFFER_SIZE + 2] = {0}; tx_buf[0] = ((uint8_t *)&address)[0]; @@ -339,41 +379,15 @@ int BATT_SMBUS::dataflash_write(uint16_t &address, void *data, const unsigned le memcpy(&tx_buf[2], data, length); // code (1), byte_count (1), addr(2), data(32) + pec - block_write(code, tx_buf, length + 2); + int result = _interface->block_write(code, tx_buf, length + 2, false); - return PX4_OK; -} - -uint8_t BATT_SMBUS::get_pec(uint8_t *buff, const uint8_t len) -{ - // Initialise CRC to zero. - uint8_t crc = 0; - uint8_t shift_register = 0; - bool invert_crc; - - // Calculate crc for each byte in the stream. - for (uint8_t i = 0; i < len; i++) { - // Load next data byte into the shift register - shift_register = buff[i]; - - // Calculate crc for each bit in the current byte. - for (uint8_t j = 0; j < 8; j++) { - invert_crc = (crc ^ shift_register) & 0x80; - crc <<= 1; - shift_register <<= 1; - - if (invert_crc) { - crc ^= BATT_SMBUS_PEC_POLYNOMIAL; - } - } - } - - return crc; + return result; } int BATT_SMBUS::get_startup_info() { - int result = PX4_ERROR; + int result = 0; + // The name field is 21 characters, add one for null terminator. const unsigned name_length = 22; // Try and get battery SBS info. @@ -381,40 +395,75 @@ int BATT_SMBUS::get_startup_info() char man_name[name_length] = {0}; result = manufacturer_name((uint8_t *)man_name, sizeof(man_name)); - if (PX4_OK != result) { + if (result != PX4_OK) { PX4_WARN("Failed to get manufacturer name"); return PX4_ERROR; } _manufacturer_name = new char[sizeof(man_name)]; - strcpy(_manufacturer_name, man_name); } // Temporary variable for storing SMBUS reads. uint16_t tmp = 0; - // Read battery serial number on startup. - if (read_word(BATT_SMBUS_SERIAL_NUMBER, &tmp) == PX4_OK) { - _serial_number = tmp; - result = PX4_OK; + result = _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, &tmp); + uint16_t serial_num = tmp; + + result |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, &tmp); + uint16_t remaining_cap = tmp; + + result |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, &tmp); + uint16_t cycle_count = tmp; + + result |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, &tmp); + uint16_t full_cap = tmp; + + if (!result) { + _serial_number = serial_num; + _batt_startup_capacity = remaining_cap; + _cycle_count = cycle_count; + _batt_capacity = full_cap; } - // Read battery capacity on startup. - if (read_word(BATT_SMBUS_REMAINING_CAPACITY, &tmp) == PX4_OK) { - _batt_startup_capacity = tmp; - result = PX4_OK; - } - // Read battery cycle count on startup. - if (read_word(BATT_SMBUS_CYCLE_COUNT, &tmp) == PX4_OK) { - _cycle_count = tmp; - result = PX4_OK; - } - // Read battery design capacity on startup. - if (read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, &tmp) == PX4_OK) { - _batt_capacity = tmp; - result = PX4_OK; + + // if (_interface->read_word(BATT_SMBUS_SERIAL_NUMBER, &tmp) == PX4_OK) { + // _serial_number = tmp; + // result = PX4_OK; + // } + + // // Read battery capacity on startup. + // if (_interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, &tmp) == PX4_OK) { + // _batt_startup_capacity = tmp; + // result = PX4_OK; + // } + + // // Read battery cycle count on startup. + // if (_interface->read_word(BATT_SMBUS_CYCLE_COUNT, &tmp) == PX4_OK) { + // _cycle_count = tmp; + // result = PX4_OK; + // } + + // // Read battery design capacity on startup. + // if (_interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, &tmp) == PX4_OK) { + // _batt_capacity = tmp; + // result = PX4_OK; + // } + + if (lifetime_data_flush() == PX4_OK) { + if (lifetime_read_block_one() == PX4_OK) { + if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) { + PX4_WARN("Battery Damaged Will Not Fly. Lifetime max voltage difference: %4.2f", + (double)_lifetime_max_delta_cell_voltage); + } + + } else { + PX4_WARN("Failed to read lifetime block 1"); + } + + } else { + PX4_WARN("Failed to flush lifetime data"); } // Read battery threshold params on startup. @@ -429,65 +478,25 @@ uint16_t BATT_SMBUS::get_serial_number() { uint16_t serial_num = 0; - if (read_word(BATT_SMBUS_SERIAL_NUMBER, &serial_num) == PX4_OK) { + if (_interface->read_word(BATT_SMBUS_SERIAL_NUMBER, &serial_num) == PX4_OK) { return serial_num; } return PX4_ERROR; } -int BATT_SMBUS::info() -{ - print_message(_last_report); - return PX4_OK; -} - -int BATT_SMBUS::init() -{ - if (PX4_OK != CDev::init()) { - PX4_ERR("CDev init failed"); - return PX4_ERROR; - } - - // Find the battery on the bus and read startup info. - if (search_addresses() != PX4_OK) { - PX4_ERR("Failed to init I2C"); - return PX4_ERROR; - } - - // Retry up to 10 times to read startup info. - for (unsigned i = 0; i < 10; i++) { - if (PX4_OK == get_startup_info()) { - break; - } - - if (i == 9) { - PX4_ERR("Failed to get battery startup info"); - return PX4_ERROR; - } - } - - // Initialize the orb ID. - _batt_orb_id = ORB_ID(battery_status); - - // Start the work queue. - start(); - - return PX4_OK; -} - -int BATT_SMBUS::manufacture_date(void *man_date) +int BATT_SMBUS::manufacture_date() { + uint16_t date = PX4_ERROR; uint8_t code = BATT_SMBUS_MANUFACTURE_DATE; - int result = read_word(code, man_date); + int result = _interface->read_word(code, &date); - if (PX4_OK != result) { - PX4_WARN("Could not read manufacturer name"); - return PX4_ERROR; + if (result != PX4_OK) { + return result; } - return PX4_OK; + return date; } int BATT_SMBUS::manufacturer_name(uint8_t *man_name, const uint8_t length) @@ -496,18 +505,18 @@ int BATT_SMBUS::manufacturer_name(uint8_t *man_name, const uint8_t length) uint8_t rx_buf[21] = {0}; // Returns 21 bytes, add 1 byte for null terminator. - int result = block_read(code, rx_buf, length - 1); + int result = _interface->block_read(code, rx_buf, length - 1, true); memcpy(man_name, rx_buf, sizeof(rx_buf)); man_name[21] = '\0'; - if (PX4_OK != result) { - PX4_WARN("Could not read manufacturer name"); - return PX4_ERROR; - } + return result; +} - return PX4_OK; +void BATT_SMBUS::print_report() +{ + print_message(_last_report); } int BATT_SMBUS::manufacturer_read(const uint16_t cmd_code, void *data, const unsigned length) @@ -518,18 +527,17 @@ 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]; - block_write(code, address, sizeof(address)); + int result = _interface->block_write(code, address, 2, false); - // returns the 2 bytes of addr info + data[] - block_read(code, data, length); + if (result != PX4_OK) { + return result; + } - uint8_t rx_buf[DATA_BUFFER_SIZE] = {0}; - memcpy(rx_buf, data, DATA_BUFFER_SIZE); + // returns the 2 bytes of addr + data[] + result = _interface->block_read(code, data, length + 2, true); + memcpy(data, &((uint8_t *)data)[2], length); - PX4_INFO("Received data: %d %d %d %d %d %d %d %d %d %d %d %d", rx_buf[0], rx_buf[1], rx_buf[2], rx_buf[3], rx_buf[4], - rx_buf[5], rx_buf[6], rx_buf[7], rx_buf[8], rx_buf[9], rx_buf[10], rx_buf[11]); - - return PX4_OK; + return result; } int BATT_SMBUS::manufacturer_write(const uint16_t cmd_code, void *data, const unsigned length) @@ -544,89 +552,185 @@ int BATT_SMBUS::manufacturer_write(const uint16_t cmd_code, void *data, const un memcpy(tx_buf, address, 2); memcpy(&tx_buf[2], data, length); - block_write(code, tx_buf, length + 2); + int result = _interface->block_write(code, tx_buf, length + 2, false); - return PX4_OK; -} - -int BATT_SMBUS::read_word(const uint8_t cmd_code, void *data) -{ - // 2 data bytes + pec byte - int result = _interface->read(cmd_code, data, 3); - - if (PX4_OK == result) { - // Check PEC. - uint8_t addr = (_interface->get_device_address() << 1); - uint8_t full_data_packet[5]; - full_data_packet[0] = addr | 0x00; - full_data_packet[1] = cmd_code; - full_data_packet[2] = addr | 0x01; - memcpy(&full_data_packet[3], data, 2); - - uint8_t pec = get_pec(full_data_packet, sizeof(full_data_packet)); - - if (pec == ((uint8_t *)data)[2]) { - return PX4_OK; - - } else { - return PX4_ERROR; - } - } - - return PX4_ERROR; -} - -int BATT_SMBUS::search_addresses() -{ - uint16_t tmp; - - _interface->set_device_address(BATT_SMBUS_ADDR); - - if (PX4_OK != read_word(BATT_SMBUS_VOLTAGE, &tmp)) { - // Search through all valid SMBus addresses. - for (uint8_t i = BATT_SMBUS_ADDR_MIN; i < BATT_SMBUS_ADDR_MAX; i++) { - _interface->set_device_address(i); - - if (read_word(BATT_SMBUS_VOLTAGE, &tmp) == PX4_OK) { - if (tmp > 0) { - break; - } - } - - if (i == BATT_SMBUS_ADDR_MAX - 1) { - PX4_WARN("No smart batteries found."); - return PX4_ERROR; - } - } - } - - PX4_INFO("Smart battery found at 0x%x", _interface->get_device_address()); - PX4_INFO("Smart battery connected"); - - return PX4_OK; -} - -void BATT_SMBUS::start() -{ - // Schedule a cycle to start things. - work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1); -} - -void BATT_SMBUS::stop() -{ - // Cancel the work queue. - work_cancel(HPWORK, &_work); + return result; } int BATT_SMBUS::unseal() { - // See pg85 of bq40z50 technical reference. + // See pg95 of bq40z50 technical reference. uint16_t keys[2] = {0x0414, 0x3672}; - if (PX4_OK != manufacturer_write(keys[0], &keys[1], 2)) { - PX4_INFO("Failed to unseal device."); + return manufacturer_write(keys[0], &keys[1], 2); +} + +int BATT_SMBUS::seal() +{ + // See pg95 of bq40z50 technical reference. + uint16_t seal = BATT_SMBUS_SEAL; + + return manufacturer_write(seal, 0, 0); +} + +int BATT_SMBUS::lifetime_data_flush() +{ + // See pg95 of bq40z50 technical reference. + uint16_t flush = BATT_SMBUS_LIFETIME_FLUSH; + + return manufacturer_write(flush, 0, 0); +} + +int BATT_SMBUS::lifetime_read_block_one() +{ + + uint8_t lifetime_block_one[32] = {}; + + // + if (PX4_OK != manufacturer_read(BATT_SMBUS_LIFETIME_BLOCK_ONE, lifetime_block_one, 32)) { + 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; + + PX4_INFO("Max Cell Delta: %4.2f", (double)_lifetime_max_delta_cell_voltage); + return PX4_OK; } + +int BATT_SMBUS::write_flash(uint16_t address, uint8_t *tx_buf, const unsigned length) +{ + + if (length > 32) { + PX4_WARN("Data length out of range: Max 32 bytes"); + return PX4_ERROR; + } + + if (PX4_OK != dataflash_write(address, tx_buf, length)) { + PX4_INFO("Dataflash write failed: %d", address); + usleep(100000); + return PX4_ERROR; + + } else { + usleep(100000); + return PX4_OK; + } +} + +int BATT_SMBUS::custom_command(int argc, char *argv[]) +{ + //const char *input = argv[optind]; + uint8_t man_name[22]; + int result = 0; + const char *input = argv[0]; + BATT_SMBUS *obj = get_instance(); + + if (!strcmp(input, "man_info")) { + + result = obj->manufacturer_name(man_name, sizeof(man_name)); + PX4_INFO("The manufacturer name: %s", man_name); + + result = obj->manufacture_date(); + PX4_INFO("The manufacturer date: %d", result); + + uint16_t serial_num = 0; + serial_num = obj->get_serial_number(); + PX4_INFO("The serial number: %d", serial_num); + + return 0; + } + + if (!strcmp(input, "unseal")) { + obj->unseal(); + return 0; + } + + if (!strcmp(input, "seal")) { + obj->seal(); + return 0; + } + + if (!strcmp(input, "report")) { + obj->print_report(); + return 0; + } + + if (!strcmp(input, "serial_num")) { + uint16_t serial_num = obj->get_serial_number(); + PX4_INFO("Serial number: %d", serial_num); + return 0; + } + + if (!strcmp(input, "write_flash")) { + if (argv[1] && argv[2]) { + uint16_t address = atoi(argv[1]); + unsigned length = atoi(argv[2]); + uint8_t tx_buf[32] = {0}; + + if (length > 32) { + PX4_WARN("Data length out of range: Max 32 bytes"); + return 1; + } + + // Data needs to be fed in 1 byte (0x01) at a time. + for (unsigned i = 0; i < length; i++) { + tx_buf[i] = atoi(argv[5 + i]); + } + + if (PX4_OK != obj->dataflash_write(address, tx_buf, length)) { + PX4_INFO("Dataflash write failed: %d", address); + usleep(100000); + return 1; + + } else { + PX4_INFO("wrote to flash"); + usleep(100000); + return 0; + } + } + } + + print_usage(); + + return PX4_ERROR; +} + +int BATT_SMBUS::print_usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Smart battery driver for the BQ40Z50 fuel gauge IC. + +### Examples +To write to flash to set parameters. address, number_of_bytes, byte0, ... , byteN +$ batt_smbus -X write_flash 19069 2 27 0 + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("batt_smbus", "driver"); + + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('X', "BATT_SMBUS_BUS_I2C_EXTERNAL", nullptr, nullptr, true); + PRINT_MODULE_USAGE_PARAM_STRING('I', "BATT_SMBUS_BUS_I2C_INTERNAL", nullptr, nullptr, true); + PRINT_MODULE_USAGE_PARAM_STRING('A', "BATT_SMBUS_BUS_ALL", nullptr, nullptr, true); + + PRINT_MODULE_USAGE_COMMAND_DESCR("man_info", "Prints manufacturer info."); + PRINT_MODULE_USAGE_COMMAND_DESCR("report", "Prints the last report."); + PRINT_MODULE_USAGE_COMMAND_DESCR("unseal", "Unseals the devices flash memory to enable write_flash commands."); + PRINT_MODULE_USAGE_COMMAND_DESCR("seal", "Seals the devices flash memory to disbale write_flash commands."); + + PRINT_MODULE_USAGE_COMMAND_DESCR("write_flash", "Writes to flash. The device must first be unsealed with the unseal command."); + PRINT_MODULE_USAGE_ARG("address", "The address to start writing.", true); + PRINT_MODULE_USAGE_ARG("number of bytes", "Number of bytes to send.", true); + PRINT_MODULE_USAGE_ARG("data[0]...data[n]", "One byte of data at a time separated by spaces.", true); + + return PX4_OK; +} + +int batt_smbus_main(int argc, char *argv[]) +{ + return BATT_SMBUS::main(argc, argv); +} diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h index da072a0ec7..5bae179f16 100644 --- a/src/drivers/batt_smbus/batt_smbus.h +++ b/src/drivers/batt_smbus/batt_smbus.h @@ -47,22 +47,30 @@ #include #include +#include + +#include #include #include #include -#include -#include -#include +#include + #include #include #include -#define DATA_BUFFER_SIZE 32 +#define DATA_BUFFER_SIZE 32 + +#define BATT_CELL_VOLTAGE_THRESHOLD_RTL 0.5f ///< Threshold in volts to RTL if cells are imbalanced +#define BATT_CELL_VOLTAGE_THRESHOLD_FAILED 1.5f ///< Threshold in volts to Land if cells are imbalanced + +#define BATT_CURRENT_UNDERVOLTAGE_THRESHOLD 5.0f ///< Threshold in amps to disable undervoltage protection +#define BATT_VOLTAGE_UNDERVOLTAGE_THRESHOLD 3.4f ///< Threshold in volts to re-enable undervoltage protection #define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION #define BATT_SMBUS_CURRENT 0x0A ///< current register @@ -84,20 +92,49 @@ #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_TIMEOUT_US 1000000 ///< timeout looking for battery 10seconds after startup +#define BATT_SMBUS_TIMEOUT_US 1000000 ///< timeout looking for battery 10seconds after startup #define BATT_SMBUS_MANUFACTURER_ACCESS 0x00 -#define BATT_SMBUS_MANUFACTURER_DATA 0x23 +#define BATT_SMBUS_MANUFACTURER_DATA 0x23 #define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS 0x44 -#define BATT_SMBUS_SECURITY_KEYS 0x0035 +#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 -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. +#define BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT 0xcf +#define BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED 0xce + +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +enum BATT_SMBUS_BUS { + BATT_SMBUS_BUS_ALL = 0, + BATT_SMBUS_BUS_I2C_INTERNAL, + BATT_SMBUS_BUS_I2C_EXTERNAL, + BATT_SMBUS_BUS_I2C_EXTERNAL1, + BATT_SMBUS_BUS_I2C_EXTERNAL2 +}; + +struct batt_smbus_bus_option { + enum BATT_SMBUS_BUS busid; + const char *devpath; + uint8_t busnum; +} bus_options[] = { + { BATT_SMBUS_BUS_I2C_EXTERNAL, "/dev/batt_smbus_ext", PX4_I2C_BUS_EXPANSION}, +#ifdef PX4_I2C_BUS_EXPANSION1 + { BATT_SMBUS_BUS_I2C_EXTERNAL1, "/dev/batt_smbus_ext1", PX4_I2C_BUS_EXPANSION1}, #endif - -/** - * @brief Nuttshell accessible method to return the driver usage arguments. - */ -void batt_smbus_usage(); +#ifdef PX4_I2C_BUS_EXPANSION2 + { BATT_SMBUS_BUS_I2C_EXTERNAL2, "/dev/batt_smbus_ext2", PX4_I2C_BUS_EXPANSION2}, +#endif +#ifdef PX4_I2C_BUS_ONBOARD + { BATT_SMBUS_BUS_I2C_INTERNAL, "/dev/batt_smbus_int", PX4_I2C_BUS_ONBOARD}, +#endif +}; /** * @brief Nuttshell accessible method to return the battery manufacture date. @@ -117,42 +154,26 @@ int manufacturer_name(); */ int serial_number(); -extern device::Device *BATT_SMBUS_I2C_interface(int bus); -typedef device::Device *(*BATT_SMBUS_constructor)(int); -class BATT_SMBUS : public cdev::CDev + +class BATT_SMBUS : public ModuleBase { public: - /** - * @brief Default Constructor. - * @param interface The device communication interface (i2c) - * @param path The device i2c address - */ - BATT_SMBUS(device::Device *interface, const char *path); + BATT_SMBUS(SMBus *interface, const char *path); - /** - * @brief Default Destructor. - */ - virtual ~BATT_SMBUS(); + ~BATT_SMBUS(); - /** - * @brief Sends a block read command. - * @param cmd_code The command code. - * @param data The returned data. - * @param length The number of bytes being read - * @return Returns PX4_OK on success, PX4_ERROR on failure. - */ - int block_read(const uint8_t cmd_code, void *data, const unsigned length); + friend SMBus; - /** - * @brief Sends a block write command. - * @param cmd_code The command code. - * @param data The data to be written. - * @param length The number of bytes being written. - * @return Returns PX4_OK on success, PX4_ERROR on failure. - */ - int block_write(const uint8_t cmd_code, void *data, const unsigned length); + /** @see ModuleBase */ + static int print_usage(); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); /** * @brief Reads data from flash. @@ -171,14 +192,6 @@ public: */ int dataflash_write(uint16_t &address, void *data, const unsigned length); - /** - * @brief Calculates the PEC from the data. - * @param buff The buffer that stores the data. - * @param length The number of bytes being written. - * @return Returns PX4_OK on success, PX4_ERROR on failure. - */ - uint8_t get_pec(uint8_t *buffer, const uint8_t length); - /** * @brief Returns the SBS serial number of the battery device. * @return Returns the SBS serial number of the battery device. @@ -193,21 +206,15 @@ public: /** * @brief Prints the latest report. - * @return Returns PX4_OK on success, PX4_ERROR on failure. */ - int info(); + void print_report(); - /** - * @brief Initializes the smart battery device. Calls probe() to check for device on bus. - * @return Returns PX4_OK on success, PX4_ERROR on failure. - */ - virtual int init(); /** * @brief Gets the SBS manufacture date of the battery. * @return Returns PX4_OK on success, PX4_ERROR on failure. */ - int manufacture_date(void *man_date); + int manufacture_date(); /** * @brief Gets the SBS manufacturer name of the battery device. @@ -235,13 +242,6 @@ public: * @return Returns PX4_OK on success, PX4_ERROR on failure. */ int manufacturer_write(const uint16_t cmd_code, void *data, const unsigned length); - /** - * @brief Sends a read-word command with cmd_code as the command. - * @param cmd_code The command code. - * @param data The returned data. - * @return Returns PX4_OK on success, PX4_ERROR on failure. - */ - int read_word(const uint8_t cmd_code, void *data); /** * @brief Search all possible slave addresses for a smart battery. @@ -249,24 +249,44 @@ public: */ int search_addresses(); - /** - * @brief Starts periodic reads from the battery. - */ - void start(); - - /** - * @brief Stops periodic reads from the battery. - */ - void stop(); - /** * @brief Unseals the battery to allow writing to restricted flash. * @return Returns PX4_OK on success, PX4_ERROR on failure. */ int unseal(); -protected: - device::Device *_interface; + /** + * @brief Seals the battery to disallow writing to restricted flash. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int seal(); + + /** + * @brief This command flushes the RAM Lifetime Data to data flash to help streamline evaluation testing. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int lifetime_data_flush(); + + /** + * @brief Reads the lifetime data from block 1. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int lifetime_read_block_one(); + + /** + * @brief Reads the lifetime data from block 1. + * @param address Address of the register to write + * @param tx_buf The sent data. + * @param length The number of bytes being written. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int write_flash(uint16_t address, uint8_t *tx_buf, const unsigned length); + + int get_cell_voltages(); + + void set_undervoltage_protection(float average_current); + + SMBus *_interface; private: @@ -275,13 +295,19 @@ private: */ static void cycle_trampoline(void *arg); + perf_counter_t _cycle; + float _cell_voltages[4] = {}; + + float _max_cell_voltage_delta{0}; + float _min_cell_voltage{0}; + /** * @brief The loop that continually generates new reports. */ void cycle(); /** @struct _work Work queue for scheduling reads. */ - work_s _work = work_s{}; + static work_s _work; /** @param _last_report Last published report, used for test(). */ battery_status_s _last_report = battery_status_s{}; @@ -289,8 +315,8 @@ private: /** @param _batt_topic uORB battery topic. */ orb_advert_t _batt_topic; - /** @param _batt_orb_id uORB battery topic ID. */ - orb_id_t _batt_orb_id; + /** @param _cell_count Number of series cell. */ + uint8_t _cell_count; /** @param _batt_capacity Battery design capacity in mAh (0 means unknown). */ uint16_t _batt_capacity; @@ -316,7 +342,13 @@ private: /** @param _manufacturer_name Name of the battery manufacturer. */ char *_manufacturer_name; - /* Do not allow copy construction or move assignment of this class. */ + /** @param _lifetime_max_delta_cell_voltage Max lifetime delta of the battery cells */ + float _lifetime_max_delta_cell_voltage; + + /** @param _cell_undervoltage_protection_status 0 if protection disabled, 1 if enabled */ + uint8_t _cell_undervoltage_protection_status; + + /** Do not allow copy construction or move assignment of this class. */ BATT_SMBUS(const BATT_SMBUS &); BATT_SMBUS operator=(const BATT_SMBUS &); };