mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-16 21:01:36 +08:00
SMbus battery driver - a lot of updates and optimizations
- 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:
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user