ardupilot/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp

164 lines
4.4 KiB
C++
Raw Normal View History

#include "AP_BattMonitor_config.h"
#if AP_BATTERY_INA239_ENABLED
#include <GCS_MAVLink/GCS.h>
#include <AP_HAL/utility/sparse-endian.h>
#include "AP_BattMonitor_INA239.h"
extern const AP_HAL::HAL& hal;
/*
note that registers are clocked on SPI MSB first, with register
number in top 6 bits, LSB is read flag
*/
#define REG_CONFIG 0x00
#define REG_ADC_CONFIG 0x01
#define REG_SHUNT_CAL 0x02
#define REG_SHUNT_VOLTAGE 0x04
#define REG_BUS_VOLTAGE 0x05
#define REG_CURRENT 0x07
#define REG_ADC_CONFIG_RESET 0xFB68U
#ifndef HAL_BATTMON_INA239_SHUNT_RESISTANCE
#define HAL_BATTMON_INA239_SHUNT_RESISTANCE 0.0002
#endif
#ifndef HAL_BATTMON_INA239_MAX_CURRENT
#define HAL_BATTMON_INA239_MAX_CURRENT 90
#endif
AP_BattMonitor_INA239::AP_BattMonitor_INA239(AP_BattMonitor &mon,
AP_BattMonitor::BattMonitor_State &mon_state,
AP_BattMonitor_Params &params)
: AP_BattMonitor_Backend(mon, mon_state, params)
{
}
void AP_BattMonitor_INA239::init(void)
{
dev = hal.spi->get_device(AP_BATTERY_INA239_SPI_DEVICE);
if (!dev) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "device fail");
return;
}
dev->set_read_flag(0x01);
// register now and configure in the timer callbacks
dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA239::timer, void));
}
void AP_BattMonitor_INA239::configure(void)
{
WITH_SEMAPHORE(dev->get_semaphore());
int16_t adc_config = 0;
if (!read_word(REG_ADC_CONFIG, adc_config) ||
uint16_t(adc_config) != REG_ADC_CONFIG_RESET) {
return;
}
voltage_LSB = 3.125e-3;
current_LSB = float(HAL_BATTMON_INA239_MAX_CURRENT) / 0x8000;
const int16_t shunt_cal = 819.2 * 1e6 * current_LSB * HAL_BATTMON_INA239_SHUNT_RESISTANCE;
int16_t shunt_cal2 = 0;
if (!write_word(REG_SHUNT_CAL, shunt_cal) ||
!read_word(REG_SHUNT_CAL, shunt_cal2) ||
shunt_cal != shunt_cal2) {
return;
}
configured = true;
}
/// read the battery_voltage and current, should be called at 10hz
void AP_BattMonitor_INA239::read(void)
{
WITH_SEMAPHORE(accumulate.sem);
_state.healthy = accumulate.count > 0;
if (!_state.healthy) {
return;
}
_state.voltage = accumulate.volt_sum / accumulate.count;
_state.current_amps = accumulate.current_sum / accumulate.count;
accumulate.volt_sum = 0;
accumulate.current_sum = 0;
accumulate.count = 0;
const uint32_t tnow = AP_HAL::micros();
const uint32_t dt_us = tnow - _state.last_time_micros;
// update total current drawn since startup
update_consumed(_state, dt_us);
_state.last_time_micros = tnow;
}
/*
read word from register
returns true if read was successful, false if failed
*/
bool AP_BattMonitor_INA239::read_word(const uint8_t reg, int16_t& data) const
{
// read the appropriate register from the device
if (!dev->read_registers(reg<<2, (uint8_t *)&data, sizeof(data))) {
return false;
}
// convert byte order
data = int16_t(be16toh(uint16_t(data)));
return true;
}
/*
write word to a register, byte swapped
returns true if write was successful, false if failed
*/
bool AP_BattMonitor_INA239::write_word(const uint8_t reg, const uint16_t data) const
{
const uint8_t b[3] { uint8_t(reg<<2), uint8_t(data >> 8), uint8_t(data&0xff) };
return dev->transfer(b, sizeof(b), nullptr, 0);
}
void AP_BattMonitor_INA239::timer(void)
{
// allow for power-on after boot
if (!configured) {
uint32_t now = AP_HAL::millis();
if (now - last_configure_ms > 200) {
// try contacting the device at 5Hz
last_configure_ms = now;
configure();
}
if (!configured) {
// waiting for the device to respond
return;
}
}
int16_t bus_voltage, current;
if (!read_word(REG_BUS_VOLTAGE, bus_voltage) ||
!read_word(REG_CURRENT, current)) {
failed_reads++;
if (failed_reads > 10) {
// device has disconnected, we need to reconfigure it
configured = false;
}
return;
}
failed_reads = 0;
WITH_SEMAPHORE(accumulate.sem);
accumulate.volt_sum += bus_voltage * voltage_LSB;
accumulate.current_sum += current * current_LSB;
accumulate.count++;
}
#endif // AP_BATTERY_INA239_ENABLED