AP_BattMonitor: SMBus: Fetch pack capacity

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
 {

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;
 

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<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

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
 
 };
 

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();

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp

@@ -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,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;