HAL_PX4: added thread-per-bus implementation for I2C

libraries/AP_HAL_PX4/I2CDevice.cpp

@@ -17,16 +17,54 @@
 #include <AP_HAL/AP_HAL.h>
 
 #include "Util.h"
+#include "Scheduler.h"
+
+extern bool _px4_thread_should_exit;
 
 namespace PX4 {
 
 uint8_t PX4::PX4_I2C::instance;
 
+/*
+  constructor for I2C wrapper class
+ */    
+PX4_I2C::PX4_I2C(uint8_t bus) :
+  I2C(devname, devpath, map_bus_number(bus), 0, 400000UL)
+{}
+
+/*
+  map ArduPilot bus numbers to PX4 bus numbers
+ */    
+uint8_t PX4_I2C::map_bus_number(uint8_t bus) const
+{
+    switch (bus) {
+    case 0:
+        // map to internal bus
+#ifdef PX4_I2C_BUS_ONBOARD
+        return PX4_I2C_BUS_ONBOARD;
+#else
+        return 0;
+#endif
+
+    case 1:
+        // map to expansion bus
+#ifdef PX4_I2C_BUS_EXPANSION
+        return PX4_I2C_BUS_EXPANSION;
+#else
+        return 1;
+#endif
+        
+    }
+    // default to bus 1
+    return 1;
+}
+    
 /*
   implement wrapper for PX4 I2C driver
  */
 bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)
 {
+    set_address(address);
     if (!init_done) {
         init_done = true;
         // we do late init() so we can setup the device paths
@@ -40,13 +78,15 @@ bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_le
     if (!init_ok) {
         return false;
     }
-    set_address(address);
     bool ret = (transfer(send, send_len, recv, recv_len) == OK);
     return ret;
 }
 
+PX4::I2CDevice::bus_info PX4::I2CDevice::businfo[num_buses];
+    
 I2CDevice::I2CDevice(uint8_t bus, uint8_t address) :
-    _bus(bus),
+    _busnum(bus),
+    _px4dev(_busnum),
     _address(address)
 {
 }
@@ -58,7 +98,7 @@ I2CDevice::~I2CDevice()
 bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
                          uint8_t *recv, uint32_t recv_len)
 {
-    return _bus.do_transfer(_address, send, send_len, recv, recv_len);
+    return _px4dev.do_transfer(_address, send, send_len, recv, recv_len);
 }
 
 bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
@@ -67,6 +107,122 @@ bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
     return false;
 }
 
+/*
+  per-bus i2c callback thread
+*/
+void *I2CDevice::i2c_thread(void *arg)
+{
+    struct bus_info *binfo = (struct bus_info *)arg;
+    while (!_px4_thread_should_exit) {
+        uint64_t now = AP_HAL::micros64();
+        uint64_t next_needed = 0;
+        struct callback_info *callback;
+
+        // find a callback to run
+        for (callback = binfo->callbacks; callback; callback = callback->next) {
+            if (now >= callback->next_usec) {
+                while (now >= callback->next_usec) {
+                    callback->next_usec += callback->period_usec;
+                }
+                // call it with semaphore held
+                if (binfo->semaphore.take(0)) {
+                    callback->cb();
+                    binfo->semaphore.give();
+                }
+            }
+            if (next_needed == 0 ||
+                callback->next_usec < next_needed) {
+                next_needed = callback->next_usec;
+            }
+        }
+
+        // delay for at most 50ms, to handle newly added callbacks
+        now = AP_HAL::micros64();
+        uint32_t delay = 50000;
+        if (next_needed > now && next_needed - now < delay) {
+            delay = next_needed - now;
+        }
+        hal.scheduler->delay_microseconds(delay);
+    }
+    return nullptr;
+}
+    
+
+/*
+  find or create thread for this bus
+*/
+I2CDevice::bus_info *I2CDevice::find_thread(void)
+{
+    if (_busnum >= num_buses) {
+        return nullptr;
+    }
+    struct bus_info &binfo = businfo[_busnum];
+    if (binfo.thread_started) {
+        return &binfo;
+    }
+
+    binfo.bus = _busnum;
+    binfo.thread_started = true;
+    
+    pthread_attr_t thread_attr;
+    struct sched_param param;
+    
+    pthread_attr_init(&thread_attr);
+    pthread_attr_setstacksize(&thread_attr, 1024);
+    
+    param.sched_priority = APM_I2C_PRIORITY;
+    (void)pthread_attr_setschedparam(&thread_attr, &param);
+    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+    
+    pthread_create(&binfo.thread_ctx, &thread_attr, &I2CDevice::i2c_thread, &binfo);
+
+    return &binfo;
+}
+    
+/*
+  register a periodic callback
+*/
+AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
+{
+    struct bus_info *binfo = find_thread();
+    if (binfo == nullptr) {
+        return nullptr;
+    }
+    struct callback_info *callback = new callback_info;
+    if (callback == nullptr) {
+        return nullptr;
+    }
+    callback->cb = cb;
+    callback->period_usec = period_usec;
+    callback->next_usec = AP_HAL::micros64() + period_usec;
+
+    // add to linked list of callbacks on thread
+    callback->next = binfo->callbacks;
+    binfo->callbacks = callback;
+
+    return callback;
+}
+    
+
+/*
+  adjust a periodic callback
+*/
+bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
+{
+    struct bus_info *binfo = find_thread();
+    if (binfo == nullptr) {
+        return nullptr;
+    }
+
+    for (struct callback_info *callback = binfo->callbacks; callback; callback = callback->next) {
+        if (h == (AP_HAL::Device::PeriodicHandle)callback) {
+            callback->period_usec = period_usec;
+            return true;
+        }
+    }
+    return false;
+}
+    
 AP_HAL::OwnPtr<AP_HAL::I2CDevice>
 I2CDeviceManager::get_device(uint8_t bus, uint8_t address)
 {

libraries/AP_HAL_PX4/I2CDevice.h

@@ -21,8 +21,7 @@
 #include <AP_HAL/HAL.h>
 #include <AP_HAL/I2CDevice.h>
 #include <AP_HAL/utility/OwnPtr.h>
-#include <AP_HAL_Empty/AP_HAL_Empty.h>
-#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
+#include "Semaphores.h"
 #include "I2CWrapper.h"
 
 namespace PX4 {
@@ -41,7 +40,7 @@ public:
     void set_address(uint8_t address) override { _address = address; }
 
     /* See AP_HAL::I2CDevice::set_retries() */
-    void set_retries(uint8_t retries) override { _retries = retries; }
+    void set_retries(uint8_t retries) override { _px4dev.set_retries(retries); }
 
     /* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */
     bool set_speed(enum Device::Speed speed) override { return true; }
@@ -55,27 +54,43 @@ public:
 
     /* See AP_HAL::Device::register_periodic_callback() */
     AP_HAL::Device::PeriodicHandle register_periodic_callback(
-        uint32_t period_usec, AP_HAL::Device::PeriodicCb) override
-    {
-        /* Not implemented yet */
-        return nullptr;
-    }
+        uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
 
     /* See AP_HAL::Device::adjust_periodic_callback() */
-    bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override
-    {
-        /* Not implemented yet */
-        return false;
+    bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
+
+    AP_HAL::Semaphore* get_semaphore() override {
+        // if asking for invalid bus number use bus 0 semaphore
+        return &businfo[_busnum<num_buses?_busnum:0].semaphore;
     }
 
-    AP_HAL::Semaphore* get_semaphore() override { return &semaphore; }
+    struct callback_info {
+        struct callback_info *next;
+        AP_HAL::Device::PeriodicCb cb;
+        uint32_t period_usec;
+        uint64_t next_usec;
+    };
+
+    struct bus_info {
+        struct callback_info *callbacks;
+        Semaphore semaphore;
+        pthread_t thread_ctx;
+        bool thread_started;
+        uint8_t bus;
+    };
     
 private:
-    // we use an empty semaphore as the underlying I2C class already has a semaphore
-    Empty::Semaphore semaphore;
-    PX4_I2C _bus;
+    static const uint8_t num_buses = 2;
+    static struct bus_info businfo[num_buses];
+    
+    // find or create thread for this bus
+    struct bus_info *find_thread(void);
+
+    static void *i2c_thread(void *arg);
+    
+    uint8_t _busnum;
+    PX4_I2C _px4dev;
     uint8_t _address;
-    uint8_t _retries = 0;
 };
 
 class I2CDeviceManager : public AP_HAL::I2CDeviceManager {

libraries/AP_HAL_PX4/I2CWrapper.h

@@ -13,10 +13,15 @@ extern const AP_HAL::HAL& hal;
  */
 class PX4::PX4_I2C : public device::I2C {
 public:
-    PX4_I2C(uint8_t bus) : I2C(devname, devpath, bus, 0, 400000UL) { }
+    PX4_I2C(uint8_t bus);
     bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len);
 
+    void set_retries(uint8_t retries) {
+        _retries = retries;
+    }
+    
 private:
+    uint8_t map_bus_number(uint8_t bus) const;
     static uint8_t instance;
     bool init_done;
     bool init_ok;

libraries/AP_HAL_PX4/Scheduler.h

@@ -13,6 +13,7 @@
 #define APM_MAIN_PRIORITY_BOOST 241
 #define APM_MAIN_PRIORITY       180
 #define APM_TIMER_PRIORITY      181
+#define APM_I2C_PRIORITY        178
 #define APM_UART_PRIORITY        60
 #define APM_STORAGE_PRIORITY     59
 #define APM_IO_PRIORITY          58