ardupilot/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp

115 lines
3.2 KiB
C++
Raw Normal View History

#include "AP_BattMonitor_LTC2946.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_HAL/utility/sparse-endian.h>
#if HAL_BATTMON_LTC2946_ENABLED
extern const AP_HAL::HAL& hal;
#define REG_CTRLA 0x00
#define REG_CTRLB 0x01
#define REG_STATUS 0x80
#define REG_MFR_ID 0xe7
// first byte of 16 bit ID is stable
#define ID_LTC2946 0x60
#define REG_DELTA 0x14 // 16 bits
#define REG_VIN 0x1e // 16 bits
#define REGA_CONF 0x18 // sense, alternate
#define REGB_CONF 0x01 // auto-reset
void AP_BattMonitor_LTC2946::init(void)
{
dev = hal.i2c_mgr->get_device(HAL_BATTMON_LTC2946_BUS, HAL_BATTMON_LTC2946_ADDR, 100000, false, 20);
if (!dev) {
return;
}
uint8_t id = 0;
WITH_SEMAPHORE(dev->get_semaphore());
if (!dev->read_registers(REG_MFR_ID, &id, 1) || id != ID_LTC2946) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "LTC2946: Failed to find device 0x%04x", unsigned(id));
return;
}
if (!dev->write_register(REG_CTRLA, REGA_CONF) ||
!dev->write_register(REG_CTRLB, REGB_CONF)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "LTC2946: Failed to configure device");
return;
}
// use datasheet typical values
voltage_LSB = 102.4 / 4095.0;
current_LSB = (0.1024/0.0005) / 4095.0;
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "LTC2946: found monitor on bus %u", HAL_BATTMON_LTC2946_BUS);
if (dev) {
dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_LTC2946::timer, void));
}
}
/// read the battery_voltage and current, should be called at 10hz
void AP_BattMonitor_LTC2946::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 float dt = tnow - _state.last_time_micros;
// update total current drawn since startup
if (_state.last_time_micros != 0 && dt < 2000000.0) {
// .0002778 is 1/3600 (conversion to hours)
const float mah = _state.current_amps * dt * 0.0000002778;
_state.consumed_mah += mah;
_state.consumed_wh += 0.001 * mah * _state.voltage;
}
_state.last_time_micros = tnow;
}
/*
read word from register
returns true if read was successful, false if failed
*/
bool AP_BattMonitor_LTC2946::read_word(const uint8_t reg, uint16_t& data) const
{
// read the appropriate register from the device
if (!dev->read_registers(reg, (uint8_t *)&data, sizeof(data))) {
return false;
}
// convert byte order
data = uint16_t(be16toh(uint16_t(data)));
// return success
return true;
}
void AP_BattMonitor_LTC2946::timer(void)
{
uint16_t amps, volts;
if (!read_word(REG_DELTA, amps) ||
!read_word(REG_VIN, volts)) {
return;
}
WITH_SEMAPHORE(accumulate.sem);
// convert 12 bit ADC data
accumulate.volt_sum += (volts>>4) * voltage_LSB;
accumulate.current_sum += (amps>>4) * current_LSB;
accumulate.count++;
}
#endif // HAL_BATTMON_LTC2946_ENABLED