Line data Source code
1 : #include "AP_BattMonitor_config.h"
2 :
3 : #if AP_BATTERY_SMBUS_ENABLED
4 :
5 : #include "AP_BattMonitor_SMBus.h"
6 :
7 : #define AP_BATTMONITOR_SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation
8 :
9 : extern const AP_HAL::HAL& hal;
10 :
11 : const AP_Param::GroupInfo AP_BattMonitor_SMBus::var_info[] = {
12 :
13 : // @Param: I2C_BUS
14 : // @DisplayName: Battery monitor I2C bus number
15 : // @Description: Battery monitor I2C bus number
16 : // @Range: 0 3
17 : // @User: Advanced
18 : // @RebootRequired: True
19 : AP_GROUPINFO("I2C_BUS", 10, AP_BattMonitor_SMBus, _bus, 0),
20 :
21 : // @Param: I2C_ADDR
22 : // @DisplayName: Battery monitor I2C address
23 : // @Description: Battery monitor I2C address
24 : // @Range: 0 127
25 : // @User: Advanced
26 : // @RebootRequired: True
27 : AP_GROUPINFO("I2C_ADDR", 11, AP_BattMonitor_SMBus, _address, AP_BATTMONITOR_SMBUS_I2C_ADDR),
28 :
29 : // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
30 :
31 : AP_GROUPEND
32 : };
33 :
34 2 : AP_BattMonitor_SMBus::AP_BattMonitor_SMBus(AP_BattMonitor &mon,
35 : AP_BattMonitor::BattMonitor_State &mon_state,
36 : AP_BattMonitor_Params ¶ms,
37 2 : uint8_t i2c_bus)
38 2 : : AP_BattMonitor_Backend(mon, mon_state, params)
39 : {
40 2 : AP_Param::setup_object_defaults(this, var_info);
41 2 : _state.var_info = var_info;
42 :
43 2 : _bus.set_default(i2c_bus);
44 2 : _params._serial_number.set(AP_BATT_SERIAL_NUMBER_DEFAULT);
45 2 : _params._pack_capacity.set(0);
46 2 : }
47 :
48 2 : void AP_BattMonitor_SMBus::init(void)
49 : {
50 2 : _dev = hal.i2c_mgr->get_device_ptr(_bus, _address, 100000, true, 20);
51 :
52 2 : if (_dev) {
53 2 : timer_handle = _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus::timer, void));
54 : }
55 2 : }
56 :
57 : // return true if cycle count can be provided and fills in cycles argument
58 0 : bool AP_BattMonitor_SMBus::get_cycle_count(uint16_t &cycles) const
59 : {
60 0 : if (!_has_cycle_count) {
61 0 : return false;
62 : }
63 0 : cycles = _cycle_count;
64 0 : return true;
65 : }
66 :
67 : /// read the battery_voltage and current, should be called at 10hz
68 300 : void AP_BattMonitor_SMBus::read(void)
69 : {
70 : // nothing to be done here for actually interacting with the battery
71 : // however we can use this to set any parameters that need to be set
72 :
73 300 : if (_serial_number != _params._serial_number) {
74 2 : _params._serial_number.set_and_notify(_serial_number);
75 : }
76 :
77 300 : if (_full_charge_capacity != _params._pack_capacity) {
78 2 : _params._pack_capacity.set_and_notify(_full_charge_capacity);
79 : }
80 300 : }
81 :
82 : // reads the pack full charge capacity
83 : // returns if we already knew the pack capacity
84 350 : void AP_BattMonitor_SMBus::read_full_charge_capacity(void)
85 : {
86 350 : if (_full_charge_capacity != 0) {
87 348 : return;
88 : }
89 :
90 2 : if (read_word(BATTMONITOR_SMBUS_FULL_CHARGE_CAPACITY, _full_charge_capacity)) {
91 2 : _full_charge_capacity *= get_capacity_scaler();
92 : }
93 : }
94 :
95 : // reads the remaining capacity
96 : // which will only be read if we know the full charge capacity (accounting for battery degradation)
97 350 : void AP_BattMonitor_SMBus::read_remaining_capacity(void)
98 : {
99 350 : int32_t capacity = _params._pack_capacity;
100 :
101 350 : if (capacity <= 0) {
102 34 : return;
103 : }
104 :
105 : uint16_t data;
106 316 : if (read_word(BATTMONITOR_SMBUS_REMAINING_CAPACITY, data)) {
107 316 : _state.consumed_mah = MAX(0, capacity - (data * get_capacity_scaler()));
108 : }
109 : }
110 :
111 : // reads the temperature word from the battery
112 350 : void AP_BattMonitor_SMBus::read_temp(void)
113 : {
114 : uint16_t data;
115 350 : if (!read_word(BATTMONITOR_SMBUS_TEMP, data)) {
116 0 : _has_temperature = (AP_HAL::millis() - _state.temperature_time) <= AP_BATT_MONITOR_TIMEOUT;
117 0 : return;
118 : }
119 350 : _has_temperature = true;
120 :
121 350 : _state.temperature_time = AP_HAL::millis();
122 350 : _state.temperature = KELVIN_TO_C(0.1f * data);
123 : }
124 :
125 : // reads the serial number if it's not already known
126 : // returns if the serial number was already known
127 350 : void AP_BattMonitor_SMBus::read_serial_number(void)
128 : {
129 : // don't recheck the serial number if we already have it
130 350 : if (_serial_number != -1) {
131 348 : return;
132 : }
133 :
134 : uint16_t data;
135 2 : if (read_word(BATTMONITOR_SMBUS_SERIAL, data)) {
136 2 : _serial_number = data;
137 : }
138 : }
139 :
140 : // reads the battery's cycle count
141 350 : void AP_BattMonitor_SMBus::read_cycle_count()
142 : {
143 : // only read cycle count once
144 350 : if (_has_cycle_count) {
145 348 : return;
146 : }
147 2 : _has_cycle_count = read_word(BATTMONITOR_SMBUS_CYCLE_COUNT, _cycle_count);
148 : }
149 :
150 : // read word from register
151 : // returns true if read was successful, false if failed
152 6274 : bool AP_BattMonitor_SMBus::read_word(uint8_t reg, uint16_t& data) const
153 : {
154 : // buffer to hold results (1 extra byte returned holding PEC)
155 6274 : const uint8_t read_size = 2 + (_pec_supported ? 1 : 0);
156 6274 : uint8_t buff[read_size]; // buffer to hold results
157 :
158 : // read the appropriate register from the device
159 6274 : if (!_dev->read_registers(reg, buff, sizeof(buff))) {
160 385 : return false;
161 : }
162 :
163 : // check PEC
164 5889 : if (_pec_supported) {
165 0 : const uint8_t pec = get_PEC(_address, reg, true, buff, 2);
166 0 : if (pec != buff[2]) {
167 0 : return false;
168 : }
169 : }
170 :
171 : // convert buffer to word
172 5889 : data = (uint16_t)buff[1]<<8 | (uint16_t)buff[0];
173 :
174 : // return success
175 5889 : return true;
176 6274 : }
177 :
178 : // read_block - returns number of characters read if successful, zero if unsuccessful
179 0 : uint8_t AP_BattMonitor_SMBus::read_block(uint8_t reg, uint8_t* data, uint8_t len) const
180 : {
181 : // get length
182 : uint8_t bufflen;
183 : // read byte (first byte indicates the number of bytes in the block)
184 0 : if (!_dev->read_registers(reg, &bufflen, 1)) {
185 0 : return 0;
186 : }
187 :
188 : // sanity check length returned by smbus
189 0 : if (bufflen == 0 || bufflen > len) {
190 0 : return 0;
191 : }
192 :
193 : // buffer to hold results (2 extra byte returned holding length and PEC)
194 0 : const uint8_t read_size = bufflen + 1 + (_pec_supported ? 1 : 0);
195 0 : uint8_t buff[read_size];
196 :
197 : // read bytes
198 0 : if (!_dev->read_registers(reg, buff, read_size)) {
199 0 : return 0;
200 : }
201 :
202 : // check PEC
203 0 : if (_pec_supported) {
204 0 : const uint8_t pec = get_PEC(_address, reg, true, buff, bufflen+1);
205 0 : if (pec != buff[bufflen+1]) {
206 0 : return 0;
207 : }
208 : }
209 :
210 : // copy data (excluding length & PEC)
211 0 : memcpy(data, &buff[1], bufflen);
212 :
213 : // return success
214 0 : return bufflen;
215 0 : }
216 :
217 : /// get_PEC - calculate packet error correction code of buffer
218 0 : 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
219 : {
220 : // exit immediately if no data
221 0 : if (len == 0) {
222 0 : return 0;
223 : }
224 :
225 : // prepare temp buffer for calculating crc
226 0 : uint8_t tmp_buff[len+3];
227 0 : tmp_buff[0] = i2c_addr << 1;
228 0 : tmp_buff[1] = cmd;
229 0 : tmp_buff[2] = tmp_buff[0] | (uint8_t)reading;
230 0 : memcpy(&tmp_buff[3],buff,len);
231 :
232 : // initialise crc to zero
233 0 : uint8_t crc = 0;
234 0 : uint8_t shift_reg = 0;
235 : bool do_invert;
236 :
237 : // for each byte in the stream
238 0 : for (uint8_t i=0; i<sizeof(tmp_buff); i++) {
239 : // load next data byte into the shift register
240 0 : shift_reg = tmp_buff[i];
241 : // for each bit in the current byte
242 0 : for (uint8_t j=0; j<8; j++) {
243 0 : do_invert = (crc ^ shift_reg) & 0x80;
244 0 : crc <<= 1;
245 0 : shift_reg <<= 1;
246 0 : if(do_invert) {
247 0 : crc ^= AP_BATTMONITOR_SMBUS_PEC_POLYNOME;
248 : }
249 : }
250 : }
251 :
252 : // return result
253 0 : return crc;
254 0 : }
255 :
256 : #endif // AP_BATTERY_SMBUS_ENABLED
|
