yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

SMbus battery driver - a lot of updates and optimizations

Browse Source 
- added support for BQ40Z80 based battery
- added performance counter for interface errors
- added SMART_BATTERY_INFO mavlink message
- general code cleanups and optimization
- fixed: void flooding the log in case of interface error
- fixed: using _batt_startup_capacity instead of _batt_capacity for discharged_mah
- update: read manufacture_date
- update: get _cell_count from parameter and not const 4
- update: avoid re-reading data that has already been read and stored on class already
- currently the battery type defined by BAT_SMBUS_MODEL parameter and not by auto detection
This commit is contained in:
bazooka joe
2020-08-14 13:41:59 -07:00
committed by Daniel Agar
parent 372866a22b
commit 190b96a46c
7 changed files with 386 additions and 243 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
msg/battery_status.msg
View File

@@ -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

332
src/drivers/batt_smbus/batt_smbus.cpp
View File

@@ -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 <dahl.jakejacob@gmail.com>
* @author Alex Klimaj <alexklimaj@gmail.com>
* @author Bazooka Joe <BazookaJoe1900@gmail.com>
*
*/
#include "batt_smbus.h"
#include <lib/parameters/param.h>
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;
}

94
src/drivers/batt_smbus/batt_smbus.h
View File

@@ -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 <dahl.jakejacob@gmail.com>
* @author Alex Klimaj <alexklimaj@gmail.com>
* @author Bazooka Joe <BazookaJoe1900@gmail.com>
*/
#pragma once
@@ -48,12 +49,17 @@
#include <mathlib/mathlib.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/param.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/topics/battery_status.h>
#include <board_config.h>
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<BATT_SMBUS>
{
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};

15
src/drivers/batt_smbus/parameters.c
View File

@@ -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);

24
src/lib/drivers/smbus/SMBus.cpp
View File

@@ -36,6 +36,7 @@
* SMBus v2.0 protocol implementation.
*
* @author Jacob Dahl <dahl.jakejacob@gmail.com>
* @author Bazooka Joe <BazookaJoe1900@gmail.com>
*
* 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;

11
src/lib/drivers/smbus/SMBus.hpp
View File

@@ -36,10 +36,11 @@
* SMBus v2.0 protocol implementation.
*
* @author Jacob Dahl <dahl.jakejacob@gmail.com>
* @author Bazooka Joe <BazookaJoe1900@gmail.com>
*/
#include <drivers/device/i2c.h>
#include <perf/perf_counter.h>
#include <string.h>
#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")};
};

95
src/modules/mavlink/mavlink_messages.cpp
View File

@@ -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<MavlinkStreamCommandLong>(),
create_stream_list_item<MavlinkStreamSysStatus>(),
create_stream_list_item<MavlinkStreamBatteryStatus>(),
create_stream_list_item<MavlinkStreamSmartBatteryInfo>(),
create_stream_list_item<MavlinkStreamHighresIMU>(),
create_stream_list_item<MavlinkStreamScaledIMU>(),
create_stream_list_item<MavlinkStreamScaledIMU2>(),