APM:Libraries
AP_BattMonitor_SMBus.cpp
Go to the documentation of this file.
1 #include "AP_BattMonitor_SMBus.h"
2 
3 #define AP_BATTMONITOR_SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation
4 
7  AP_BattMonitor_Params &params,
9  : AP_BattMonitor_Backend(mon, mon_state, params),
10  _dev(std::move(dev))
11 {
14 }
15 
17  if (_dev) {
19  }
20 }
21 
24 {
25  // nothing to be done here for actually interacting with the battery
26  // however we can use this to set any parameters that need to be set
27 
29  _params._serial_number.set_and_notify(_serial_number);
30  }
31 
34  }
35 }
36 
37 // reads the pack full charge capacity
38 // returns true if the read was successful, or if we already knew the pack capacity
40 {
41  uint16_t data;
42 
43  if (_full_charge_capacity != 0) {
44  return true;
46  _full_charge_capacity = data;
47  return true;
48  }
49  return false;
50 }
51 
52 // reads the remaining capacity
53 // returns true if the read was succesful, which is only considered to be the
54 // we know the full charge capacity
56 {
57  int32_t capacity = _params._pack_capacity;
58 
59  if (capacity > 0) {
60  uint16_t data;
62  _state.consumed_mah = MAX(0, capacity - data);
63  return true;
64  }
65  }
66 
67  return false;
68 }
69 
70 // reads the temperature word from the battery
71 // returns true if the read was successful
73 {
74  uint16_t data;
75  if (read_word(BATTMONITOR_SMBUS_TEMP, data)) {
77  _state.temperature = ((float)(data - 2731)) * 0.1f;
78  return true;
79  }
80 
81  return false;
82 }
83 
84 // reads the serial number if it's not already known
85 // returns true if the read was successful or the number was already known
87  uint16_t data;
88 
89  // don't recheck the serial number if we already have it
90  if (_serial_number != -1) {
91  return true;
92  } else if (read_word(BATTMONITOR_SMBUS_SERIAL, data)) {
93  _serial_number = data;
94  return true;
95  }
96 
97  return false;
98 }
99 
100 // read word from register
101 // returns true if read was successful, false if failed
102 bool AP_BattMonitor_SMBus::read_word(uint8_t reg, uint16_t& data) const
103 {
104  // buffer to hold results (1 extra byte returned holding PEC)
105  const uint8_t read_size = 2 + (_pec_supported ? 1 : 0);
106  uint8_t buff[read_size]; // buffer to hold results
107 
108  // read the appropriate register from the device
109  if (!_dev->read_registers(reg, buff, sizeof(buff))) {
110  return false;
111  }
112 
113  // check PEC
114  if (_pec_supported) {
115  const uint8_t pec = get_PEC(AP_BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, 2);
116  if (pec != buff[2]) {
117  return false;
118  }
119  }
120 
121  // convert buffer to word
122  data = (uint16_t)buff[1]<<8 | (uint16_t)buff[0];
123 
124  // return success
125  return true;
126 }
127 
129 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
130 {
131  // exit immediately if no data
132  if (len == 0) {
133  return 0;
134  }
135 
136  // prepare temp buffer for calcing crc
137  uint8_t tmp_buff[len+3];
138  tmp_buff[0] = i2c_addr << 1;
139  tmp_buff[1] = cmd;
140  tmp_buff[2] = tmp_buff[0] | (uint8_t)reading;
141  memcpy(&tmp_buff[3],buff,len);
142 
143  // initialise crc to zero
144  uint8_t crc = 0;
145  uint8_t shift_reg = 0;
146  bool do_invert;
147 
148  // for each byte in the stream
149  for (uint8_t i=0; i<sizeof(tmp_buff); i++) {
150  // load next data byte into the shift register
151  shift_reg = tmp_buff[i];
152  // for each bit in the current byte
153  for (uint8_t j=0; j<8; j++) {
154  do_invert = (crc ^ shift_reg) & 0x80;
155  crc <<= 1;
156  shift_reg <<= 1;
157  if(do_invert) {
159  }
160  }
161  }
162 
163  // return result
164  return crc;
165 }
166 
virtual Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, Device::PeriodicCb) override=0
#define AP_BATTMONITOR_SMBUS_PEC_POLYNOME
virtual void timer(void)=0
AP_BattMonitor_SMBus(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params, AP_HAL::OwnPtr< AP_HAL::I2CDevice > dev)
Constructor.
#define AP_BATTMONITOR_SMBUS_I2C_ADDR
bool read_word(uint8_t reg, uint16_t &data) const
AP_BattMonitor_Params & _params
void init(void) override
static auto MAX(const A &one, const B &two) -> decltype(one > two ? one :two)
Definition: AP_Math.h:202
AP_Int32 _pack_capacity
offset voltage that is subtracted from current pin before conversion to amps
uint8_t get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const
get_PEC - calculate packet error correction code of buffer
static AP_HAL::OwnPtr< AP_HAL::Device > dev
Definition: ICM20789.cpp:16
#define f(i)
uint32_t millis()
Definition: system.cpp:157
AP_HAL::OwnPtr< AP_HAL::I2CDevice > _dev
void read(void) override
read the battery_voltage and current, should be called at 10hz
AP_BattMonitor::BattMonitor_State & _state
#define AP_BATT_SERIAL_NUMBER_DEFAULT
bool read_registers(uint8_t first_reg, uint8_t *recv, uint32_t recv_len)
Definition: Device.h:113
#define FUNCTOR_BIND_MEMBER(func, rettype,...)
Definition: functor.h:31
AP_Int32 _serial_number
max battery power allowed. Reduce max throttle to reduce current to satisfy t his limit ...