AP_BattMonitor: SMBus: Fetch pack capacity

This commit is contained in:
Michael du Breuil 2017-04-23 23:49:51 -07:00 committed by Francisco Ferreira
parent f919c409bb
commit 22ab8de4d2
6 changed files with 36 additions and 24 deletions

View File

@ -39,12 +39,6 @@ uint8_t AP_BattMonitor_Backend::capacity_remaining_pct() const
}
}
/// set capacity for this instance
void AP_BattMonitor_Backend::set_capacity(uint32_t capacity)
{
_mon._pack_capacity[_state.instance] = capacity;
}
/// get capacity for this instance
int32_t AP_BattMonitor_Backend::get_capacity() const
{

View File

@ -37,9 +37,6 @@ public:
/// capacity_remaining_pct - returns the % battery capacity remaining (0 ~ 100)
uint8_t capacity_remaining_pct() const;
/// set capacity for this instance
void set_capacity(uint32_t capacity);
/// get capacity for this instance
int32_t get_capacity() const;

View File

@ -3,7 +3,9 @@
#define AP_BATTMONITOR_SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation
#define BATTMONITOR_SMBUS_TEMP 0x08 // temperature register
#define BATTMONITOR_SMBUS_FULL_CHARGE_CAPACITY 0x10 // full charge capacity
#define BATTMONITOR_SMBUS_SERIAL 0x1C // serial number
AP_BattMonitor_SMBus::AP_BattMonitor_SMBus(AP_BattMonitor &mon,
AP_BattMonitor::BattMonitor_State &mon_state,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
@ -11,6 +13,7 @@ AP_BattMonitor_SMBus::AP_BattMonitor_SMBus(AP_BattMonitor &mon,
_dev(std::move(dev))
{
_mon._serial_numbers[_state.instance] = AP_BATT_SERIAL_NUMBER_DEFAULT;
_mon._pack_capacity[_state.instance] = 0;
}
/// read the battery_voltage and current, should be called at 10hz
@ -22,6 +25,25 @@ void AP_BattMonitor_SMBus::read(void)
if (_serial_number != _mon._serial_numbers[_state.instance]) {
_mon._serial_numbers[_state.instance].set_and_notify(_serial_number);
}
if (_full_charge_capacity != _mon._pack_capacity[_state.instance]) {
_mon._pack_capacity[_state.instance].set_and_notify(_full_charge_capacity);
}
}
// reads the pack full charge capacity
// returns true if the read was successful, or if we already knew the pack capacity
bool AP_BattMonitor_SMBus::read_full_charge_capacity(void)
{
uint16_t data;
if (_full_charge_capacity != 0) {
return true;
} else if (read_word(BATTMONITOR_SMBUS_FULL_CHARGE_CAPACITY, data)) {
_full_charge_capacity = data;
return true;
}
return false;
}
// reads the temperature word from the battery

View File

@ -28,6 +28,10 @@ protected:
void read(void) override;
// reads the pack full charge capacity
// returns true if the read was successful, or if we already knew the pack capacity
bool read_full_charge_capacity(void);
// reads the temperature word from the battery
// returns true if the read was successful
bool read_temp(void);
@ -48,6 +52,7 @@ protected:
bool _pec_supported; // true if PEC is supported
int32_t _serial_number = -1; // battery serial number
uint16_t _full_charge_capacity; // full charge capacity, used to stash the value before setting the parameter
};

View File

@ -82,6 +82,8 @@ void AP_BattMonitor_SMBus_Maxell::timer()
_state.last_time_micros = tnow;
}
read_full_charge_capacity();
read_temp();
read_serial_number();

View File

@ -7,7 +7,6 @@
#include <utility>
#define BATTMONITOR_SMBUS_SOLO_REMAINING_CAPACITY 0x0f // predicted remaining battery capacity in milliamps
#define BATTMONITOR_SMBUS_SOLO_FULL_CHARGE_CAPACITY 0x10 // full capacity register
#define BATTMONITOR_SMBUS_SOLO_MANUFACTURE_DATA 0x23 /// manufacturer data
#define BATTMONITOR_SMBUS_SOLO_CELL_VOLTAGE 0x28 // cell voltage register
#define BATTMONITOR_SMBUS_SOLO_CURRENT 0x2a // current register
@ -75,21 +74,14 @@ void AP_BattMonitor_SMBus_Solo::timer()
_state.last_time_micros = tnow;
}
// read battery design capacity
if (get_capacity() == 0) {
if (read_word(BATTMONITOR_SMBUS_SOLO_FULL_CHARGE_CAPACITY, data)) {
if (data > 0) {
set_capacity(data);
}
}
}
// read remaining capacity
if (read_full_charge_capacity()) {
// only read remaining capacity once we have the full capacity
if (get_capacity() > 0) {
if (read_word(BATTMONITOR_SMBUS_SOLO_REMAINING_CAPACITY, data)) {
_state.current_total_mah = MAX(0, get_capacity() - data);
}
}
}
// read the button press indicator
if (read_block(BATTMONITOR_SMBUS_SOLO_MANUFACTURE_DATA, buff, 6, false) == 6) {