From 4f231f9355d8abeb4db53423afe72adebe39a5c5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 4 Nov 2016 14:54:51 +1100
Subject: [PATCH] AP_BattMonitor: fixed thread usage for SMBus_I2C

---
 .../AP_BattMonitor_SMBus_I2C.cpp              | 32 ++++++-------------
 .../AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h |  2 ++
 2 files changed, 11 insertions(+), 23 deletions(-)

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp
index 6d0f7545d7..0b0ae6da88 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp
@@ -9,8 +9,6 @@ extern const AP_HAL::HAL& hal;
 
 #include <AP_HAL/AP_HAL.h>
 
-#if CONFIG_HAL_BOARD != HAL_BOARD_PX4
-
 #define BATTMONITOR_SMBUS_TEMP      0x08    // temperature register
 #define BATTMONITOR_SMBUS_VOLTAGE   0x09    // voltage register
 #define BATTMONITOR_SMBUS_FULL_CHARGE_CAPACITY  0x10    // full capacity register
@@ -31,10 +29,17 @@ AP_BattMonitor_SMBus_I2C::AP_BattMonitor_SMBus_I2C(AP_BattMonitor &mon, uint8_t
                                                    AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
     : AP_BattMonitor_SMBus(mon, instance, mon_state)
     , _dev(std::move(dev))
-{}
+{
+    _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus_I2C::timer, bool));
+}
 
 /// Read the battery voltage and current.  Should be called at 10hz
 void AP_BattMonitor_SMBus_I2C::read()
+{
+    // nothing to do - all done in timer()
+}
+
+bool AP_BattMonitor_SMBus_I2C::timer()
 {
     uint16_t data;
     uint8_t buff[4];
@@ -57,38 +62,29 @@ void AP_BattMonitor_SMBus_I2C::read()
     if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) {
         _state.healthy = false;
     }
+    return true;
 }
 
 // read word from register
 // returns true if read was successful, false if failed
 bool AP_BattMonitor_SMBus_I2C::read_word(uint8_t reg, uint16_t& data) const
 {
-    // take i2c bus semaphore
-    if (!_dev->get_semaphore()->take_nonblocking()) {
-        return false;
-    }
-
     uint8_t buff[3];    // buffer to hold results
 
     // read three bytes and place in last three bytes of buffer
     if (!_dev->read_registers(reg, buff, sizeof(buff))) {
-        _dev->get_semaphore()->give();
         return false;
     }
 
     // check PEC
     uint8_t pec = get_PEC(BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, 2);
     if (pec != buff[2]) {
-        _dev->get_semaphore()->give();
         return false;
     }
 
     // convert buffer to word
     data = (uint16_t)buff[1]<<8 | (uint16_t)buff[0];
 
-    // give back i2c semaphore
-    _dev->get_semaphore()->give();
-
     // return success
     return true;
 }
@@ -98,20 +94,11 @@ uint8_t AP_BattMonitor_SMBus_I2C::read_block(uint8_t reg, uint8_t* data, uint8_t
 {
     uint8_t buff[max_len+2];    // buffer to hold results (2 extra byte returned holding length and PEC)
 
-    // take i2c bus semaphore
-    if (!_dev->get_semaphore()->take_nonblocking()) {
-        return 0;
-    }
-
     // read bytes
     if (!_dev->read_registers(reg, buff, sizeof(buff))) {
-        _dev->get_semaphore()->give();
         return 0;
     }
 
-    // give back i2c semaphore
-    _dev->get_semaphore()->give();
-
     // get length
     uint8_t bufflen = buff[0];
 
@@ -179,4 +166,3 @@ uint8_t AP_BattMonitor_SMBus_I2C::get_PEC(const uint8_t i2c_addr, uint8_t cmd, b
     return crc;
 }
 
-#endif // CONFIG_HAL_BOARD != HAL_BOARD_PX4
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h
index f43dfe01db..a84e8aabf8 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h
@@ -23,6 +23,8 @@ public:
 
 private:
 
+    bool timer(void);
+
     // read word from register
     // returns true if read was successful, false if failed
     bool read_word(uint8_t reg, uint16_t& data) const;