mirror of https://github.com/ArduPilot/ardupilot
182 lines
5.2 KiB
C++
182 lines
5.2 KiB
C++
#include "AP_BattMonitor_SMBus.h"
|
|
|
|
#define AP_BATTMONITOR_SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation
|
|
|
|
AP_BattMonitor_SMBus::AP_BattMonitor_SMBus(AP_BattMonitor &mon,
|
|
AP_BattMonitor::BattMonitor_State &mon_state,
|
|
AP_BattMonitor_Params ¶ms,
|
|
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
|
|
: AP_BattMonitor_Backend(mon, mon_state, params),
|
|
_dev(std::move(dev))
|
|
{
|
|
_params._serial_number = AP_BATT_SERIAL_NUMBER_DEFAULT;
|
|
_params._pack_capacity = 0;
|
|
}
|
|
|
|
void AP_BattMonitor_SMBus::init(void)
|
|
{
|
|
if (_dev) {
|
|
timer_handle = _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus::timer, void));
|
|
}
|
|
}
|
|
|
|
// return true if cycle count can be provided and fills in cycles argument
|
|
bool AP_BattMonitor_SMBus::get_cycle_count(uint16_t &cycles) const
|
|
{
|
|
if (!_has_cycle_count) {
|
|
return false;
|
|
}
|
|
cycles = _cycle_count;
|
|
return true;
|
|
}
|
|
|
|
/// read the battery_voltage and current, should be called at 10hz
|
|
void AP_BattMonitor_SMBus::read(void)
|
|
{
|
|
// nothing to be done here for actually interacting with the battery
|
|
// however we can use this to set any parameters that need to be set
|
|
|
|
if (_serial_number != _params._serial_number) {
|
|
_params._serial_number.set_and_notify(_serial_number);
|
|
}
|
|
|
|
if (_full_charge_capacity != _params._pack_capacity) {
|
|
_params._pack_capacity.set_and_notify(_full_charge_capacity);
|
|
}
|
|
}
|
|
|
|
// reads the pack full charge capacity
|
|
// returns if we already knew the pack capacity
|
|
void AP_BattMonitor_SMBus::read_full_charge_capacity(void)
|
|
{
|
|
if (_full_charge_capacity != 0) {
|
|
return;
|
|
}
|
|
|
|
if (read_word(BATTMONITOR_SMBUS_FULL_CHARGE_CAPACITY, _full_charge_capacity)) {
|
|
_full_charge_capacity *= get_capacity_scaler();
|
|
}
|
|
}
|
|
|
|
// reads the remaining capacity
|
|
// which will only be read if we know the full charge capacity (accounting for battery degradation)
|
|
void AP_BattMonitor_SMBus::read_remaining_capacity(void)
|
|
{
|
|
int32_t capacity = _params._pack_capacity;
|
|
|
|
if (capacity <= 0) {
|
|
return;
|
|
}
|
|
|
|
uint16_t data;
|
|
if (read_word(BATTMONITOR_SMBUS_REMAINING_CAPACITY, data)) {
|
|
_state.consumed_mah = MAX(0, capacity - (data * get_capacity_scaler()));
|
|
}
|
|
}
|
|
|
|
// reads the temperature word from the battery
|
|
void AP_BattMonitor_SMBus::read_temp(void)
|
|
{
|
|
uint16_t data;
|
|
if (!read_word(BATTMONITOR_SMBUS_TEMP, data)) {
|
|
_has_temperature = (AP_HAL::millis() - _state.temperature_time) <= AP_BATT_MONITOR_TIMEOUT;
|
|
return;
|
|
}
|
|
_has_temperature = true;
|
|
|
|
_state.temperature_time = AP_HAL::millis();
|
|
_state.temperature = (data * 0.1f) - C_TO_KELVIN;
|
|
}
|
|
|
|
// reads the serial number if it's not already known
|
|
// returns if the serial number was already known
|
|
void AP_BattMonitor_SMBus::read_serial_number(void)
|
|
{
|
|
// don't recheck the serial number if we already have it
|
|
if (_serial_number != -1) {
|
|
return;
|
|
}
|
|
|
|
uint16_t data;
|
|
if (read_word(BATTMONITOR_SMBUS_SERIAL, data)) {
|
|
_serial_number = data;
|
|
}
|
|
}
|
|
|
|
// reads the battery's cycle count
|
|
void AP_BattMonitor_SMBus::read_cycle_count()
|
|
{
|
|
// only read cycle count once
|
|
if (_has_cycle_count) {
|
|
return;
|
|
}
|
|
_has_cycle_count = read_word(BATTMONITOR_SMBUS_CYCLE_COUNT, _cycle_count);
|
|
}
|
|
|
|
// read word from register
|
|
// returns true if read was successful, false if failed
|
|
bool AP_BattMonitor_SMBus::read_word(uint8_t reg, uint16_t& data) const
|
|
{
|
|
// buffer to hold results (1 extra byte returned holding PEC)
|
|
const uint8_t read_size = 2 + (_pec_supported ? 1 : 0);
|
|
uint8_t buff[read_size]; // buffer to hold results
|
|
|
|
// read the appropriate register from the device
|
|
if (!_dev->read_registers(reg, buff, sizeof(buff))) {
|
|
return false;
|
|
}
|
|
|
|
// check PEC
|
|
if (_pec_supported) {
|
|
const uint8_t pec = get_PEC(AP_BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, 2);
|
|
if (pec != buff[2]) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// convert buffer to word
|
|
data = (uint16_t)buff[1]<<8 | (uint16_t)buff[0];
|
|
|
|
// return success
|
|
return true;
|
|
}
|
|
|
|
/// 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
|
|
{
|
|
// exit immediately if no data
|
|
if (len == 0) {
|
|
return 0;
|
|
}
|
|
|
|
// prepare temp buffer for calculating crc
|
|
uint8_t tmp_buff[len+3];
|
|
tmp_buff[0] = i2c_addr << 1;
|
|
tmp_buff[1] = cmd;
|
|
tmp_buff[2] = tmp_buff[0] | (uint8_t)reading;
|
|
memcpy(&tmp_buff[3],buff,len);
|
|
|
|
// initialise crc to zero
|
|
uint8_t crc = 0;
|
|
uint8_t shift_reg = 0;
|
|
bool do_invert;
|
|
|
|
// for each byte in the stream
|
|
for (uint8_t i=0; i<sizeof(tmp_buff); i++) {
|
|
// load next data byte into the shift register
|
|
shift_reg = tmp_buff[i];
|
|
// for each bit in the current byte
|
|
for (uint8_t j=0; j<8; j++) {
|
|
do_invert = (crc ^ shift_reg) & 0x80;
|
|
crc <<= 1;
|
|
shift_reg <<= 1;
|
|
if(do_invert) {
|
|
crc ^= AP_BATTMONITOR_SMBUS_PEC_POLYNOME;
|
|
}
|
|
}
|
|
}
|
|
|
|
// return result
|
|
return crc;
|
|
}
|