BattMon_SMBus_PX4: read capacity

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp

@@ -34,12 +34,24 @@ extern const AP_HAL::HAL& hal;
 
 // Constructor
 AP_BattMonitor_SMBus_PX4::AP_BattMonitor_SMBus_PX4(AP_BattMonitor &mon, uint8_t instance, AP_BattMonitor::BattMonitor_State &mon_state) :
-        AP_BattMonitor_SMBus(mon, instance, mon_state)
+        AP_BattMonitor_SMBus(mon, instance, mon_state),
+        _batt_fd(-1),
+        _capacity_updated(false)
 {
     // orb subscription for battery status
     _batt_sub = orb_subscribe(ORB_ID(battery_status));
 }
 
+void AP_BattMonitor_SMBus_PX4::init()
+{
+    // open the device
+    _batt_fd = open(BATT_SMBUS0_DEVICE_PATH, O_RDWR);
+    if (_batt_fd == -1) {
+        hal.console->printf("Unable to open " BATT_SMBUS0_DEVICE_PATH);
+        _state.healthy = false;
+    }
+}
+
 // read - read latest voltage and current
 void AP_BattMonitor_SMBus_PX4::read()
 {
@@ -57,6 +69,16 @@ void AP_BattMonitor_SMBus_PX4::read()
             _state.last_time_micros = hal.scheduler->micros();
             _state.current_total_mah = batt_status.discharged_mah;
             _state.healthy = true;
+
+            // read capacity
+            uint32_t tnow = hal.scheduler->micros();
+            if ((_batt_fd >= 0) && !_capacity_updated) {
+                uint16_t tmp;
+                if (ioctl(_batt_fd, BATT_SMBUS_GET_CAPACITY, (unsigned long)&tmp) == OK) {
+                    _capacity_updated = true;
+                    set_capacity(tmp);
+                }
+            }
         }
     } else if (_state.healthy) {
         // timeout after 5 seconds

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.h

@@ -29,11 +29,16 @@ public:
     // Constructor
     AP_BattMonitor_SMBus_PX4(AP_BattMonitor &mon, uint8_t instance, AP_BattMonitor::BattMonitor_State &mon_state);
 
+    /// init
+    void init();
+
     /// read - read the battery voltage and current
     void read();
 
 private:
     int         _batt_sub;          // orb subscription description
+    int         _batt_fd;           // file descriptor
+    bool        _capacity_updated;  // capacity info read
 };
 
 #endif // AP_BATTMONITOR_SMBUS_PX4_H