amiro-os / components / power / ina219.cpp @ b8b3a9c9
History | View | Annotate | Download (10.767 KB)
1 |
#include <amiro/power/ina219.hpp> |
---|---|
2 |
|
3 |
#include <ch.hpp> |
4 |
#include <chprintf.h> |
5 |
#include <global.hpp> |
6 |
|
7 |
using namespace chibios_rt; |
8 |
using namespace amiro; |
9 |
using namespace INA219; |
10 |
|
11 |
extern Global global;
|
12 |
|
13 |
Driver::Driver(I2CDriver &i2c_driver, const uint8_t i2c_address) :
|
14 |
BaseSensor<InitData,CalibData>(), i2c_driver(&i2c_driver), tx_params({i2c_address, NULL, 0, NULL, 0}), current_lsb_uA(0) |
15 |
{ |
16 |
this->config.content.brng = Configuration::BRNG_DEFAULT;
|
17 |
this->config.content.pg = Configuration::PGA_DEFAULT;
|
18 |
this->config.content.badc = Configuration::ADC_DEFAULT;
|
19 |
this->config.content.sadc = Configuration::ADC_DEFAULT;
|
20 |
this->config.content.mode = Configuration::MODE_DEFAULT;
|
21 |
|
22 |
return;
|
23 |
} |
24 |
|
25 |
Driver::~Driver() |
26 |
{} |
27 |
|
28 |
msg_t |
29 |
Driver::init(InitData* initialization_data) |
30 |
{ |
31 |
if(!initialization_data)
|
32 |
{ |
33 |
return ERROR;
|
34 |
} |
35 |
|
36 |
// write configuration
|
37 |
if (this->writeRegister(REG_CONFIGURATION, initialization_data->configuration.value & MASK_CONFIGURATION)) { |
38 |
return ERROR;
|
39 |
} |
40 |
this->config.value = initialization_data->configuration.value & MASK_CONFIGURATION;
|
41 |
|
42 |
if (this->writeRegister(REG_CALIBRATION, initialization_data->calibration & MASK_CALIBRATION)) { |
43 |
return ERROR;
|
44 |
} |
45 |
|
46 |
this->current_lsb_uA = initialization_data->current_lsb_uA;
|
47 |
|
48 |
return this->update(); |
49 |
} |
50 |
|
51 |
msg_t |
52 |
Driver::update() |
53 |
{ |
54 |
this->status.power = 0; |
55 |
msg_t res = this->readRegister(REG_BUS_VOLTAGE, this->status.bus_voltage.value); |
56 |
res |= this->readRegister(REG_POWER, this->status.power); |
57 |
|
58 |
// if the power register was not updated yet, try again
|
59 |
while (!this->status.bus_voltage.content.conversion_ready || this->status.power == 0 || res != 0) |
60 |
{ |
61 |
BaseThread::sleep(MS2ST(10));
|
62 |
res |= this->readRegister(REG_BUS_VOLTAGE, this->status.bus_voltage.value); |
63 |
res |= this->readRegister(REG_POWER, this->status.power); |
64 |
} |
65 |
|
66 |
return res ? ERROR : SUCCESS;
|
67 |
} |
68 |
|
69 |
msg_t |
70 |
Driver::wakeup() |
71 |
{ |
72 |
if (this->writeRegister(REG_CONFIGURATION, this->config.value)) { |
73 |
return ERROR;
|
74 |
} else {
|
75 |
return this->update(); |
76 |
} |
77 |
} |
78 |
|
79 |
msg_t |
80 |
Driver::hibernate() |
81 |
{ |
82 |
Configuration::Register tmp_config = this->config;
|
83 |
tmp_config.content.mode = Configuration::MODE_PowerDown; |
84 |
if (this->writeRegister(REG_CONFIGURATION, tmp_config.value)) { |
85 |
return ERROR;
|
86 |
} else {
|
87 |
return SUCCESS;
|
88 |
} |
89 |
|
90 |
} |
91 |
|
92 |
#ifndef AMIRO_NCALIBRATION
|
93 |
msg_t |
94 |
Driver::calibration(CalibData* calibration_data) |
95 |
{ |
96 |
if (!calibration_data) {
|
97 |
return ERROR;
|
98 |
} |
99 |
|
100 |
uint16_t current_lsb_uA = calibration_data->input.current_lsb_uA; |
101 |
if (current_lsb_uA < calibration_data->input.max_expected_current_A / 0.032767f) { |
102 |
current_lsb_uA = calibration_data->input.max_expected_current_A / 0.032767f; |
103 |
} else if (current_lsb_uA > calibration_data->input.max_expected_current_A / 0.004096f) { |
104 |
current_lsb_uA = calibration_data->input.max_expected_current_A / 0.004096f; |
105 |
} |
106 |
|
107 |
const uint16_t calibration_value = uint16_t(40960 / (current_lsb_uA * calibration_data->input.shunt_resistance_O)); |
108 |
|
109 |
float V_shunt_max;
|
110 |
switch (calibration_data->input.configuration.content.pg)
|
111 |
{ |
112 |
case Configuration::PGA_40mV:
|
113 |
V_shunt_max = 0.04f; |
114 |
break;
|
115 |
case Configuration::PGA_80mV:
|
116 |
V_shunt_max = 0.08f; |
117 |
break;
|
118 |
case Configuration::PGA_160mV:
|
119 |
V_shunt_max = 0.16f; |
120 |
break;
|
121 |
case Configuration::PGA_320mV:
|
122 |
V_shunt_max = 0.32f; |
123 |
break;
|
124 |
} |
125 |
|
126 |
const float max_current_before_overflow = ( (current_lsb_uA * 0.032767f >= V_shunt_max) / (calibration_data->input.shunt_resistance_O) )? |
127 |
V_shunt_max / calibration_data->input.shunt_resistance_O : |
128 |
current_lsb_uA * 0.032767f; |
129 |
|
130 |
const float max_shuntvoltage_before_overflow = ( (max_current_before_overflow * calibration_data->input.shunt_resistance_O) >= V_shunt_max )? |
131 |
V_shunt_max : |
132 |
max_current_before_overflow * calibration_data->input.shunt_resistance_O; |
133 |
|
134 |
calibration_data->output.max_current_before_overflow_A = max_current_before_overflow; |
135 |
calibration_data->output.max_shuntvoltage_before_overflow_V = max_shuntvoltage_before_overflow; |
136 |
calibration_data->output.current_lsb_uA = current_lsb_uA; |
137 |
calibration_data->output.calibration_value = calibration_value; |
138 |
|
139 |
return SUCCESS;
|
140 |
} |
141 |
#endif
|
142 |
|
143 |
#ifndef AMIRO_NSELFTEST
|
144 |
msg_t |
145 |
Driver::selftest() |
146 |
{ |
147 |
struct RegisterContent {
|
148 |
Configuration::Register configuration; |
149 |
uint16_t shunt_voltage = 0;
|
150 |
BusVoltage bus_voltage; |
151 |
uint16_t power = 0;
|
152 |
uint16_t current = 0;
|
153 |
uint16_t calibration = 0;
|
154 |
}; |
155 |
|
156 |
// backup the current status
|
157 |
RegisterContent backup; |
158 |
msg_t res = this->readRegister(REG_CONFIGURATION, backup.configuration.value);
|
159 |
res |= this->readRegister(REG_SHUNT_VOLTAGE, backup.shunt_voltage);
|
160 |
res |= this->readRegister(REG_BUS_VOLTAGE, backup.bus_voltage.value);
|
161 |
res |= this->readRegister(REG_POWER, backup.power);
|
162 |
res |= this->readRegister(REG_CURRENT, backup.current);
|
163 |
res |= this->readRegister(REG_CALIBRATION, backup.calibration);
|
164 |
if (res ||
|
165 |
!(backup.configuration.value || |
166 |
backup.shunt_voltage || |
167 |
backup.bus_voltage.value || |
168 |
backup.power || |
169 |
backup.current || |
170 |
backup.calibration)) |
171 |
{ |
172 |
return ST_FAIL_BACKUP;
|
173 |
} |
174 |
|
175 |
// reset the INA219
|
176 |
if (this->reset()) |
177 |
{ |
178 |
return ST_FAIL_RESET;
|
179 |
} |
180 |
|
181 |
// read the configuration
|
182 |
Configuration::Register config = this->readConfiguration();
|
183 |
|
184 |
// check for the default configuration
|
185 |
Configuration::Register config_default; |
186 |
config_default.content.brng = Configuration::BRNG_DEFAULT; |
187 |
config_default.content.pg = Configuration::PGA_DEFAULT; |
188 |
config_default.content.badc = Configuration::ADC_DEFAULT; |
189 |
config_default.content.sadc = Configuration::ADC_DEFAULT; |
190 |
config_default.content.mode = Configuration::MODE_DEFAULT; |
191 |
if (config.value != config_default.value)
|
192 |
{ |
193 |
return ST_FAIL_IS_DEFAULT;
|
194 |
} |
195 |
|
196 |
// revert to the previous configuration
|
197 |
if (this->writeRegister(REG_CONFIGURATION, backup.configuration.value)) |
198 |
{ |
199 |
return ST_FAIL_WRITE_CONFIG;
|
200 |
} |
201 |
|
202 |
// revert to the previous calibration
|
203 |
if (this->writeRegister(REG_CALIBRATION, backup.calibration)) |
204 |
{ |
205 |
return ST_FAIL_WRITE_CALIB;
|
206 |
} |
207 |
|
208 |
// read the current configuration
|
209 |
if (this->readConfiguration().value != backup.configuration.value) |
210 |
{ |
211 |
return ST_FAIL_CHECK_CONFIG;
|
212 |
} |
213 |
|
214 |
// read the current calibration
|
215 |
if (this->readCalibration() != backup.calibration) |
216 |
{ |
217 |
return ST_FAIL_CHECK_CALIB;
|
218 |
} |
219 |
|
220 |
// read and print the current status
|
221 |
INA219::BusVoltage bus_voltage = this->readBusVoltage();
|
222 |
uint16_t power; |
223 |
this->readRegister(REG_POWER, power);
|
224 |
// wait until the bus voltage and the power register are valid
|
225 |
while (!bus_voltage.conversion_ready || power == 0) |
226 |
{ |
227 |
BaseThread::sleep(MS2ST(10));
|
228 |
bus_voltage = this->readBusVoltage();
|
229 |
this->readRegister(REG_POWER, power);
|
230 |
} |
231 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "shunt voltage : %fV\n", this->readShuntVoltage_uV() / 1000000.f); |
232 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "bus voltage : %fV\n", bus_voltage.voltage_uV / 1000000.f); |
233 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "power : %fW\n", this->readPower_uW() / 1000000.f); |
234 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "current : %fA\n", this->readCurrent_uA() / 1000000.f); |
235 |
|
236 |
return ST_OK;
|
237 |
} |
238 |
#endif
|
239 |
|
240 |
Configuration::Register |
241 |
Driver::readConfiguration() |
242 |
{ |
243 |
Configuration::Register reg; |
244 |
this->readRegister(REG_CONFIGURATION, reg.value);
|
245 |
return reg;
|
246 |
} |
247 |
|
248 |
int32_t |
249 |
Driver::readShuntVoltage_uV() |
250 |
{ |
251 |
uint16_t val = 0;
|
252 |
this->readRegister(REG_SHUNT_VOLTAGE, val);
|
253 |
/*
|
254 |
* Depending on the configuration either one, two, three or four most significant bits are used for coding the sign (no two's complement!)
|
255 |
* -> Masking bits in order to separate sign bits and value bits.
|
256 |
* Furthermore, the least significant bit represents 10uV.
|
257 |
* -> Multiplication by 10 or -10 respectively.
|
258 |
*/
|
259 |
switch (this->config.content.pg) |
260 |
{ |
261 |
case Configuration::PGA_320mV:
|
262 |
return ( uint32_t(val & 0x7FFFu) * ((val & 0x8000u)? -10 : 10) ); |
263 |
case Configuration::PGA_160mV:
|
264 |
return ( uint32_t(val & 0x3FFFu) * ((val & 0xC000u)? -10 : 10) ); |
265 |
case Configuration::PGA_80mV:
|
266 |
return ( uint32_t(val & 0x1FFFu) * ((val & 0xE000u)? -10 : 10) ); |
267 |
case Configuration::PGA_40mV:
|
268 |
return ( uint32_t(val & 0x0FFFu) * ((val & 0xF000u)? -10 : 10) ); |
269 |
} |
270 |
} |
271 |
|
272 |
INA219::BusVoltage |
273 |
Driver::readBusVoltage() |
274 |
{ |
275 |
Driver::BusVoltage reg; |
276 |
this->readRegister(REG_BUS_VOLTAGE, reg.value);
|
277 |
return this->busVoltageReg2uV(reg); |
278 |
} |
279 |
|
280 |
uint32_t |
281 |
Driver::readPower_uW() |
282 |
{ |
283 |
uint16_t val = 0;
|
284 |
this->readRegister(REG_POWER, val);
|
285 |
return this->powerReg2uW(val); |
286 |
} |
287 |
|
288 |
int32_t |
289 |
Driver::readCurrent_uA() |
290 |
{ |
291 |
uint16_t val = 0;
|
292 |
this->readRegister(REG_CURRENT, val);
|
293 |
|
294 |
/*
|
295 |
* Reinterpret register value as a signed integer (two's complement).
|
296 |
* Multiply with the value of the least significant bit.
|
297 |
*/
|
298 |
return int32_t(*reinterpret_cast<int16_t*>(&val)) * this->current_lsb_uA; |
299 |
} |
300 |
|
301 |
uint16_t |
302 |
Driver::readCalibration() |
303 |
{ |
304 |
uint16_t val = 0;
|
305 |
this->readRegister(REG_CALIBRATION, val);
|
306 |
return (val & MASK_CALIBRATION);
|
307 |
} |
308 |
|
309 |
uint8_t |
310 |
Driver::reset() |
311 |
{ |
312 |
const msg_t res = this->writeRegister(REG_CONFIGURATION, MASK_RESET); |
313 |
return (res? ERROR : SUCCESS);
|
314 |
} |
315 |
|
316 |
msg_t |
317 |
Driver::main(void)
|
318 |
{ |
319 |
while (!this->shouldTerminate()) |
320 |
{ |
321 |
this->update();
|
322 |
//this->eventSource.broadcastFlags(0);
|
323 |
this->sleep(MS2ST(1000)); |
324 |
//this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(1000));
|
325 |
} |
326 |
|
327 |
return RDY_OK;
|
328 |
} |
329 |
|
330 |
msg_t |
331 |
Driver::readRegister(const RegisterAddress reg, uint16_t& dst)
|
332 |
{ |
333 |
uint8_t buffer[2];
|
334 |
this->tx_params.txbuf = reinterpret_cast<const uint8_t*>(®); |
335 |
this->tx_params.txbytes = 1; |
336 |
this->tx_params.rxbuf = buffer;
|
337 |
this->tx_params.rxbytes = 2; |
338 |
|
339 |
this->i2c_driver->acquireBus();
|
340 |
const msg_t res = this->i2c_driver->masterTransmit(&this->tx_params); |
341 |
this->i2c_driver->releaseBus();
|
342 |
|
343 |
if (!res) {
|
344 |
dst = (buffer[0] << 8) | buffer[1]; |
345 |
if (reg == REG_CALIBRATION)
|
346 |
dst &= MASK_CALIBRATION; |
347 |
#ifndef NDEBUG
|
348 |
} else {
|
349 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: i2c transmit failed (%d | 0x%08X)\n", __FILE__ , __LINE__ , res, this->i2c_driver->getErrors()); |
350 |
#endif
|
351 |
} |
352 |
|
353 |
return res;
|
354 |
} |
355 |
|
356 |
msg_t |
357 |
Driver::writeRegister(const RegisterAddress reg, const uint16_t& val) |
358 |
{ |
359 |
const uint8_t buffer[3] = {reg, |
360 |
static_cast<uint8_t>((val & 0xFF00u) >> 8), |
361 |
static_cast<uint8_t>(val & 0x00FFu)}; |
362 |
this->tx_params.txbuf = buffer;
|
363 |
this->tx_params.txbytes = 3; |
364 |
this->tx_params.rxbytes = 0; |
365 |
|
366 |
this->i2c_driver->acquireBus();
|
367 |
const msg_t res = this->i2c_driver->masterTransmit(&this->tx_params); |
368 |
this->i2c_driver->releaseBus();
|
369 |
|
370 |
#ifndef NDEBUG
|
371 |
if (res) {
|
372 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: i2c transmit failed (%d | 0x%08X)\n", __FILE__ , __LINE__ , res, this->i2c_driver->getErrors()); |
373 |
} |
374 |
#endif
|
375 |
|
376 |
return res;
|
377 |
} |
378 |
|