Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: move read_block up to SMBus base class

This commit is contained in:
Joshua Henderson 2022-08-24 04:12:26 -04:00 committed by Andrew Tridgell
parent fb86318848
commit a93b887f5f
8 changed files with 48 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp
View File

@ -173,6 +173,45 @@ bool AP_BattMonitor_SMBus::read_word(uint8_t reg, uint16_t& data) const
return true;
}
// read_block - returns number of characters read if successful, zero if unsuccessful
uint8_t AP_BattMonitor_SMBus::read_block(uint8_t reg, uint8_t* data, uint8_t len) const
{
// get length
uint8_t bufflen;
// read byte (first byte indicates the number of bytes in the block)
if (!_dev->read_registers(reg, &bufflen, 1)) {
return 0;
}
// sanity check length returned by smbus
if (bufflen == 0 || bufflen > len) {
return 0;
}
// buffer to hold results (2 extra byte returned holding length and PEC)
const uint8_t read_size = bufflen + 1 + (_pec_supported ? 1 : 0);
uint8_t buff[read_size];
// read bytes
if (!_dev->read_registers(reg, buff, read_size)) {
return 0;
}
// check PEC
if (_pec_supported) {
const uint8_t pec = get_PEC(_address, reg, true, buff, bufflen+1);
if (pec != buff[bufflen+1]) {
return 0;
}
}
// copy data (excluding length & PEC)
memcpy(data, &buff[1], bufflen);
// return success
return bufflen;
}
/// get_PEC - calculate packet error correction code of buffer
uint8_t AP_BattMonitor_SMBus::get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const
{

6
libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h
View File

@ -10,7 +10,8 @@
#define AP_BATTMONITOR_SMBUS_BUS_INTERNAL 0
#define AP_BATTMONITOR_SMBUS_BUS_EXTERNAL 1
#define AP_BATTMONITOR_SMBUS_I2C_ADDR 0x0B
#define AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds
#define AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds
#define AP_BATTMONITOR_SMBUS_READ_BLOCK_MAXIMUM_TRANSFER 0x20 // A Block Read or Write is allowed to transfer a maximum of 32 data bytes.
class AP_BattMonitor_SMBus : public AP_BattMonitor_Backend
{
@ -84,6 +85,9 @@ protected:
// returns true if read was successful, false if failed
bool read_word(uint8_t reg, uint16_t& data) const;
// read_block - returns number of characters read if successful, zero if unsuccessful
uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t len) const;
// get_PEC - calculate PEC for a read or write from the battery
// buff is the data that was read or will be written
uint8_t get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;

46
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp
View File

@ -3,7 +3,6 @@
#include <AP_Math/AP_Math.h>
#include "AP_BattMonitor.h"
#include "AP_BattMonitor_SMBus_Generic.h"
#include <utility>
uint8_t smbus_cell_ids[] = { 0x3f, // cell 1
0x3e, // cell 2
@ -23,7 +22,6 @@ uint8_t smbus_cell_ids[] = { 0x3f, // cell 1
#endif
};
#define SMBUS_READ_BLOCK_MAXIMUM_TRANSFER 0x20 // A Block Read or Write is allowed to transfer a maximum of 32 data bytes.
#define SMBUS_CELL_COUNT_CHECK_TIMEOUT 15 // check cell count for up to 15 seconds
/*
@ -129,45 +127,6 @@ void AP_BattMonitor_SMBus_Generic::timer()
read_cycle_count();
}
// read_block - returns number of characters read if successful, zero if unsuccessful
uint8_t AP_BattMonitor_SMBus_Generic::read_block(uint8_t reg, uint8_t* data) const
{
// get length
uint8_t bufflen;
// read byte (first byte indicates the number of bytes in the block)
if (!_dev->read_registers(reg, &bufflen, 1)) {
return 0;
}
// sanity check length returned by smbus
if (bufflen == 0 || bufflen > SMBUS_READ_BLOCK_MAXIMUM_TRANSFER) {
return 0;
}
// buffer to hold results (2 extra byte returned holding length and PEC)
const uint8_t read_size = bufflen + 1 + (_pec_supported ? 1 : 0);
uint8_t buff[read_size];
// read bytes
if (!_dev->read_registers(reg, buff, read_size)) {
return 0;
}
// check PEC
if (_pec_supported) {
uint8_t pec = get_PEC(AP_BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, bufflen+1);
if (pec != buff[bufflen+1]) {
return 0;
}
}
// copy data (excluding PEC)
memcpy(data, &buff[1], bufflen);
// return success
return bufflen;
}
// check if PEC supported with the version value in SpecificationInfo() function
// returns true once PEC is confirmed as working or not working
bool AP_BattMonitor_SMBus_Generic::check_pec_support()
@ -194,8 +153,8 @@ bool AP_BattMonitor_SMBus_Generic::check_pec_support()
}
// check manufacturer name
uint8_t buff[SMBUS_READ_BLOCK_MAXIMUM_TRANSFER + 1] {};
if (read_block(BATTMONITOR_SMBUS_MANUFACTURE_NAME, buff)) {
uint8_t buff[AP_BATTMONITOR_SMBUS_READ_BLOCK_MAXIMUM_TRANSFER + 1] {};
if (read_block(BATTMONITOR_SMBUS_MANUFACTURE_NAME, buff, sizeof(buff))) {
// Hitachi maxell batteries do not support PEC
if (strcmp((char*)buff, "Hitachi maxell") == 0) {
_pec_supported = false;
@ -209,4 +168,3 @@ bool AP_BattMonitor_SMBus_Generic::check_pec_support()
_pec_confirmed = true;
return true;
}

3
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.h
View File

@ -25,9 +25,6 @@ private:
// returns true once PEC is confirmed as working or not working
bool check_pec_support();
// read_block - returns number of characters read if successful, zero if unsuccessful
uint8_t read_block(uint8_t reg, uint8_t* data) const;
uint8_t _pec_confirmed; // count of the number of times PEC has been confirmed as working
uint32_t _last_cell_update_us[BATTMONITOR_SMBUS_NUM_CELLS_MAX]; // system time of last successful read of cell voltage
uint32_t _cell_count_check_start_us; // system time we started attempting to count the number of cells

34
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.cpp
View File

@ -65,38 +65,6 @@ void AP_BattMonitor_SMBus_SUI::timer()
update_health();
}
// read_block - returns true if successful
bool AP_BattMonitor_SMBus_SUI::read_block(uint8_t reg, uint8_t* data, uint8_t len) const
{
// buffer to hold results (2 extra byte returned holding length and PEC)
uint8_t buff[len+2];
// read bytes
if (!_dev->read_registers(reg, buff, sizeof(buff))) {
return false;
}
// get length
uint8_t bufflen = buff[0];
// sanity check length returned by smbus
if (bufflen == 0 || bufflen > len) {
return false;
}
// check PEC
uint8_t pec = get_PEC(_address, reg, true, buff, bufflen+1);
if (pec != buff[bufflen+1]) {
return false;
}
// copy data (excluding PEC)
memcpy(data, &buff[1], bufflen);
// return success
return true;
}
// read_bare_block - returns true if successful
bool AP_BattMonitor_SMBus_SUI::read_block_bare(uint8_t reg, uint8_t* data, uint8_t len) const
{
@ -129,7 +97,7 @@ void AP_BattMonitor_SMBus_SUI::read_cell_voltages()
// we can't read voltage of all cells. get overall pack voltage to work out
// an average for remaining cells
uint16_t total_mv;
if (read_block(BATTMONITOR_SMBUS_VOLTAGE, (uint8_t *)&total_mv, sizeof(total_mv))) {
if (read_word(BATTMONITOR_SMBUS_VOLTAGE, total_mv)) {
// if total voltage is below pack_voltage_mv then we will
// read zero volts for the extra cells.
total_mv = MAX(total_mv, pack_voltage_mv);

3
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.h
View File

@ -21,8 +21,7 @@ private:
void read_cell_voltages();
void update_health();
// read_block - returns number of characters read if successful, zero if unsuccessful
bool read_block(uint8_t reg, uint8_t* data, uint8_t len) const;
// read_block_bare - returns number of characters read if successful, zero if unsuccessful
bool read_block_bare(uint8_t reg, uint8_t* data, uint8_t len) const;
const uint8_t cell_count;

31
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp
View File

@ -95,34 +95,3 @@ void AP_BattMonitor_SMBus_Solo::timer()
read_cycle_count();
}
// read_block - returns number of characters read if successful, zero if unsuccessful
uint8_t AP_BattMonitor_SMBus_Solo::read_block(uint8_t reg, uint8_t* data, uint8_t max_len) const
{
uint8_t buff[max_len+2]; // buffer to hold results (2 extra byte returned holding length and PEC)
// read bytes
if (!_dev->read_registers(reg, buff, sizeof(buff))) {
return 0;
}
// get length
uint8_t bufflen = buff[0];
// sanity check length returned by smbus
if (bufflen == 0 || bufflen > max_len) {
return 0;
}
// check PEC
uint8_t pec = get_PEC(_address, reg, true, buff, bufflen+1);
if (pec != buff[bufflen+1]) {
return 0;
}
// copy data (excluding PEC)
memcpy(data, &buff[1], bufflen);
// return success
return bufflen;
}

3
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h
View File

@ -15,8 +15,5 @@ private:
void timer(void) override;
// read_block - returns number of characters read if successful, zero if unsuccessful
uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len) const;
uint8_t _button_press_count;
};