aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_BattMonitor: added INA231 backend

Browse Source 
only enabled via hwdef.dat for now
This commit is contained in:
Andrew Tridgell 2021-10-07 22:19:01 +11:00
parent 8d9c9d9130
commit 2903e10fc0
5 changed files with 181 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_BattMonitor/AP_BattMonitor.cpp
View File

@ -14,6 +14,7 @@
#include "AP_BattMonitor_FuelLevel_PWM.h"
#include "AP_BattMonitor_Generator.h"
#include "AP_BattMonitor_MPPT_PacketDigital.h"
#include "AP_BattMonitor_INA231.h"
#include <AP_HAL/AP_HAL.h>
@ -295,6 +296,11 @@ AP_BattMonitor::init()
drivers[instance] = new AP_BattMonitor_MPPT_PacketDigital(*this, state[instance], _params[instance]);
break;
#endif // HAL_MPPT_PACKETDIGITAL_CAN_ENABLE
#if HAL_BATTMON_INA231_ENABLED
case Type::INA231:
drivers[instance] = new AP_BattMonitor_INA231(*this, state[instance], _params[instance]);
break;
#endif
case Type::NONE:
default:
break;

3
libraries/AP_BattMonitor/AP_BattMonitor.h
View File

@ -46,6 +46,7 @@ class AP_BattMonitor_SMBus_Rotoye;
class AP_BattMonitor_UAVCAN;
class AP_BattMonitor_Generator;
class AP_BattMonitor_MPPT_PacketDigital;
class AP_BattMonitor_INA231;
class AP_BattMonitor
{
@ -62,6 +63,7 @@ class AP_BattMonitor
friend class AP_BattMonitor_FuelLevel_PWM;
friend class AP_BattMonitor_Generator;
friend class AP_BattMonitor_MPPT_PacketDigital;
friend class AP_BattMonitor_INA231;
public:
@ -93,6 +95,7 @@ public:
GENERATOR_FUEL = 18,
Rotoye = 19,
MPPT_PacketDigital = 20,
INA231 = 21,
};
FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);

127
libraries/AP_BattMonitor/AP_BattMonitor_INA231.cpp Normal file
View File

@ -0,0 +1,127 @@
#include "AP_BattMonitor_INA231.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_HAL/utility/sparse-endian.h>
#if HAL_BATTMON_INA231_ENABLED
extern const AP_HAL::HAL& hal;
#define REG_CONFIG 0x00
#define REG_SHUNT_VOLTAGE 0x01
#define REG_BUS_VOLTAGE 0x02
#define REG_CURRENT 0x04
#define REG_CALIBRATION 0x05
#define REG_CONFIG_DEFAULT 0x4127
#define REG_CONFIG_RESET 0x8000
// this should become a parameter in future
#define MAX_AMPS 90.0
void AP_BattMonitor_INA231::init(void)
{
dev = hal.i2c_mgr->get_device(HAL_BATTMON_INA231_BUS, HAL_BATTMON_INA231_ADDR, 100000, false, 20);
if (!dev) {
return;
}
int16_t config = 0;
WITH_SEMAPHORE(dev->get_semaphore());
if (!write_word(REG_CONFIG, REG_CONFIG_RESET) ||
!write_word(REG_CONFIG, REG_CONFIG_DEFAULT) ||
!read_word(REG_CONFIG, config) ||
config != REG_CONFIG_DEFAULT) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "INA231: Failed to find device 0x%04x", unsigned(config));
return;
}
// configure for MAX_AMPS
const uint16_t conf = (0x2<<9) | (0x5<<6) | (0x5<<3) | 0x7; // 2ms conv time, 16x sampling
const float rShunt = 0.0005;
current_LSB = MAX_AMPS / 32768.0;
voltage_LSB = 0.00125; // 1.25mV/bit
const uint16_t cal = uint16_t(0.00512 / (current_LSB * rShunt));
if (!write_word(REG_CONFIG, REG_CONFIG_RESET) || // reset
!write_word(REG_CONFIG, conf) ||
!write_word(REG_CALIBRATION, cal)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "INA231: Failed to configure device");
return;
}
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "INA231: found monitor on bus %u", HAL_BATTMON_INA231_BUS);
if (dev) {
dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA231::timer, void));
}
}
/// read the battery_voltage and current, should be called at 10hz
void AP_BattMonitor_INA231::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_INA231::read_word(const uint8_t reg, int16_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 = 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_INA231::write_word(const uint8_t reg, const uint16_t data) const
{
const uint8_t b[3] { reg, uint8_t(data >> 8), uint8_t(data&0xff) };
return dev->transfer(b, sizeof(b), nullptr, 0);
}
void AP_BattMonitor_INA231::timer(void)
{
int16_t bus_voltage, current;
if (!read_word(REG_BUS_VOLTAGE, bus_voltage) ||
!read_word(REG_CURRENT, current)) {
return;
}
WITH_SEMAPHORE(accumulate.sem);
accumulate.volt_sum += bus_voltage * voltage_LSB;
accumulate.current_sum += current * current_LSB;
accumulate.count++;
}
#endif // HAL_BATTMON_INA231_ENABLED

44
libraries/AP_BattMonitor/AP_BattMonitor_INA231.h Normal file
View File

@ -0,0 +1,44 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/I2CDevice.h>
#include "AP_BattMonitor_Backend.h"
#include <utility>
#define HAL_BATTMON_INA231_ENABLED defined(HAL_BATTMON_INA231_BUS) && defined(HAL_BATTMON_INA231_ADDR)
#if HAL_BATTMON_INA231_ENABLED
class AP_BattMonitor_INA231 : public AP_BattMonitor_Backend
{
public:
// inherit constructor
using AP_BattMonitor_Backend::AP_BattMonitor_Backend;
bool has_cell_voltages() const override { return false; }
bool has_temperature() const override { return false; }
bool has_current() const override { return true; }
bool reset_remaining(float percentage) override { return false; }
bool get_cycle_count(uint16_t &cycles) const override { return false; }
virtual void init(void) override;
virtual void read() override;
private:
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
bool read_word(const uint8_t reg, int16_t& data) const;
bool write_word(const uint8_t reg, const uint16_t data) const;
void timer(void);
struct {
uint16_t count;
float volt_sum;
float current_sum;
HAL_Semaphore sem;
} accumulate;
float current_LSB;
float voltage_LSB;
};
#endif // HAL_BATTMON_INA231_ENABLED

2
libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
View File

@ -13,7 +13,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
// @Param: MONITOR
// @DisplayName: Battery monitoring
// @Description: Controls enabling monitoring of the battery's voltage and current
// @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:UAVCAN-BatteryInfo,9:ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye
// @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:UAVCAN-BatteryInfo,9:ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA231
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),