From 22ab8de4d23a84a6232f56330771f943df497bea Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Sun, 23 Apr 2017 23:49:51 -0700 Subject: [PATCH] AP_BattMonitor: SMBus: Fetch pack capacity --- .../AP_BattMonitor/AP_BattMonitor_Backend.cpp | 6 ----- .../AP_BattMonitor/AP_BattMonitor_Backend.h | 3 --- .../AP_BattMonitor/AP_BattMonitor_SMBus.cpp | 26 +++++++++++++++++-- .../AP_BattMonitor/AP_BattMonitor_SMBus.h | 5 ++++ .../AP_BattMonitor_SMBus_Maxell.cpp | 2 ++ .../AP_BattMonitor_SMBus_Solo.cpp | 18 ++++--------- 6 files changed, 36 insertions(+), 24 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 280fe57d59..d0c1acf57b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -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 { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index d4d2db3f19..97e906c53c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -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; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp index 025e228149..97d02cf022 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp @@ -2,8 +2,10 @@ #define AP_BATTMONITOR_SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation -#define BATTMONITOR_SMBUS_TEMP 0x08 // temperature register -#define BATTMONITOR_SMBUS_SERIAL 0x1C // serial number +#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 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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h index 06cd098080..e636023003 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h @@ -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 }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp index b2398c2f87..0ef1b8f16c 100755 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp @@ -82,6 +82,8 @@ void AP_BattMonitor_SMBus_Maxell::timer() _state.last_time_micros = tnow; } + read_full_charge_capacity(); + read_temp(); read_serial_number(); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp index ab5056a062..091e8bc649 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp @@ -7,7 +7,6 @@ #include #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,22 +74,15 @@ 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); + 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 remaining 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) { bool pressed = (buff[1] >> 3) & 0x01;