AP_HAL_Linux: implement threaded I2C

libraries/AP_HAL/Device.h

@@ -19,6 +19,7 @@
 
 #include <inttypes.h>
 
+#include "AP_HAL_Namespace.h"
 #include "utility/functor.h"
 
 /*

libraries/AP_HAL_Linux/I2CDevice.cpp

@@ -46,6 +46,10 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
 
+#include "PollerThread.h"
+#include "Scheduler.h"
+#include "Semaphores.h"
+#include "Thread.h"
 #include "Util.h"
 
 /* Workaround broken header from i2c-tools */
@@ -73,11 +77,20 @@ static inline char *startswith(const char *s, const char *prefix)
 }
 
 /* Private struct to maintain for each bus */
-class I2CBus {
+class I2CBus : public TimerPollable::WrapperCb {
 public:
     ~I2CBus();
+
+    /*
+     * TimerPollable::WrapperCb methods to take
+     * and release semaphore while calling the callback
+     */
+    void start_cb() override;
+    void end_cb() override;
+
     int open(uint8_t n);
 
+    PollerThread thread;
     Semaphore sem;
     int fd = -1;
     uint8_t bus;
@@ -91,6 +104,16 @@ I2CBus::~I2CBus()
     }
 }
 
+void I2CBus::start_cb()
+{
+    sem.take(HAL_SEMAPHORE_BLOCK_FOREVER);
+}
+
+void I2CBus::end_cb()
+{
+    sem.give();
+}
+
 int I2CBus::open(uint8_t n)
 {
     char path[sizeof("/dev/i2c-XXX")];
@@ -220,15 +243,29 @@ int I2CDevice::get_fd()
 }
 
 AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(
-    uint32_t period_usec, AP_HAL::Device::PeriodicCb)
+    uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
 {
-    return nullptr;
+    TimerPollable *p = _bus.thread.add_timer(cb, &_bus, period_usec);
+    if (!p) {
+        AP_HAL::panic("Could not create periodic callback");
+    }
+
+    if (!_bus.thread.is_started()) {
+        char name[16];
+        snprintf(name, sizeof(name), "ap-i2c-%u", _bus.bus);
+
+        _bus.thread.set_stack_size(AP_LINUX_SENSORS_STACK_SIZE);
+        _bus.thread.start(name, AP_LINUX_SENSORS_SCHED_POLICY,
+                          AP_LINUX_SENSORS_SCHED_PRIO);
+    }
+
+    return static_cast<AP_HAL::Device::PeriodicHandle>(p);
 }
 
 bool I2CDevice::adjust_periodic_callback(
     AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
 {
-    return false;
+    return _bus.thread.adjust_timer(static_cast<TimerPollable*>(h), period_usec);
 }
 
 I2CDeviceManager::I2CDeviceManager()

libraries/AP_HAL_Linux/Scheduler.h

@@ -10,6 +10,10 @@
 #define LINUX_SCHEDULER_MAX_TIMESLICED_PROCS 10
 #define LINUX_SCHEDULER_MAX_IO_PROCS 10
 
+#define AP_LINUX_SENSORS_STACK_SIZE  256 * 1024
+#define AP_LINUX_SENSORS_SCHED_POLICY  SCHED_FIFO
+#define AP_LINUX_SENSORS_SCHED_PRIO 12
+
 namespace Linux {
 
 class Scheduler : public AP_HAL::Scheduler {