mirror of https://github.com/ArduPilot/ardupilot
161 lines
4.3 KiB
C++
161 lines
4.3 KiB
C++
#include "AP_BattMonitor_INA239.h"
|
|
#include <GCS_MAVLink/GCS.h>
|
|
#include <AP_HAL/utility/sparse-endian.h>
|
|
|
|
#if HAL_BATTMON_INA239_ENABLED
|
|
|
|
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 ¶ms)
|
|
: AP_BattMonitor_Backend(mon, mon_state, params)
|
|
{
|
|
}
|
|
|
|
void AP_BattMonitor_INA239::init(void)
|
|
{
|
|
dev = hal.spi->get_device(HAL_BATTMON_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 // HAL_BATTMON_INA239_ENABLED
|