HAL_PX4: moved to common DeviceBus class for thread management in I2C and SPI

libraries/AP_HAL_PX4/Device.cpp

@@ -0,0 +1,116 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include "Device.h"
+
+#include <arch/board/board.h>
+#include "board_config.h"
+#include <stdio.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/OwnPtr.h>
+#include "Scheduler.h"
+#include "Semaphores.h"
+
+extern bool _px4_thread_should_exit;
+
+namespace PX4 {
+
+static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
+
+/*
+  per-bus callback thread
+*/
+void *DeviceBus::bus_thread(void *arg)
+{
+    struct DeviceBus *binfo = (struct DeviceBus *)arg;
+    while (!_px4_thread_should_exit) {
+        uint64_t now = AP_HAL::micros64();
+        DeviceBus::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();
+                }
+            }
+        }
+
+        // work out when next loop is needed
+        uint64_t next_needed = 0;
+        now = AP_HAL::micros64();
+
+        for (callback = binfo->callbacks; callback; callback = callback->next) {
+            if (next_needed == 0 ||
+                callback->next_usec < next_needed) {
+                next_needed = callback->next_usec;
+                if (next_needed < now) {
+                    next_needed = now;
+                }
+            }
+        }
+
+        // delay for at most 50ms, to handle newly added callbacks
+        uint32_t delay = 50000;
+        if (next_needed >= now && next_needed - now < delay) {
+            delay = next_needed - now;
+        }
+        // don't delay for less than 400usec, so one thread doesn't
+        // completely dominate the CPU
+        if (delay < 400) {
+            delay = 400;
+        }
+        hal.scheduler->delay_microseconds(delay);
+    }
+    return nullptr;
+}
+    
+AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
+{
+    if (!thread_started) {
+        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 = thread_priority;
+        (void)pthread_attr_setschedparam(&thread_attr, &param);
+        pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+    
+        pthread_create(&thread_ctx, &thread_attr, &DeviceBus::bus_thread, this);
+    }
+    DeviceBus::callback_info *callback = new DeviceBus::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 = callbacks;
+    callbacks = callback;
+
+    return callback;
+}
+
+}

libraries/AP_HAL_PX4/Device.h

@@ -0,0 +1,47 @@
+/*
+ * This file is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation, either version 3 of the License,
+ * or (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <inttypes.h>
+#include <AP_HAL/HAL.h>
+#include "Semaphores.h"
+#include "Scheduler.h"
+
+namespace PX4 {
+
+class DeviceBus {
+public:
+    DeviceBus(uint8_t _thread_priority = APM_I2C_PRIORITY) :
+        thread_priority(_thread_priority) {}
+    
+    struct DeviceBus *next;
+    Semaphore semaphore;
+
+    AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb);
+    static void *bus_thread(void *arg);
+
+private:
+    struct callback_info {
+        struct callback_info *next;
+        AP_HAL::Device::PeriodicCb cb;
+        uint32_t period_usec;
+        uint64_t next_usec;        
+    } *callbacks;
+    uint8_t thread_priority;
+    pthread_t thread_ctx;
+    bool thread_started;
+};
+
+}

libraries/AP_HAL_PX4/I2CDevice.cpp

@@ -19,8 +19,6 @@
 #include "Util.h"
 #include "Scheduler.h"
 
-extern bool _px4_thread_should_exit;
-
 namespace PX4 {
 
 uint8_t PX4::PX4_I2C::instance;
@@ -82,8 +80,6 @@ bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_le
     return ret;
 }
 
-PX4::I2CDevice::bus_info PX4::I2CDevice::businfo[num_buses];
-    
 I2CDevice::I2CDevice(uint8_t bus, uint8_t address) :
     _busnum(bus),
     _px4dev(_busnum),
@@ -107,100 +103,17 @@ 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 (_busnum >= num_buses) {
-    if (binfo == nullptr) {
         return nullptr;
     }
-    struct callback_info *callback = new callback_info;
+    struct DeviceBus &binfo = businfo[_busnum];
-    if (callback == nullptr) {
+    return binfo.register_periodic_callback(period_usec, cb);
-        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;
 }
     
 
@@ -209,17 +122,6 @@ AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t pe
 */
 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;
 }
     

libraries/AP_HAL_PX4/I2CDevice.h

@@ -23,6 +23,7 @@
 #include <AP_HAL/utility/OwnPtr.h>
 #include "Semaphores.h"
 #include "I2CWrapper.h"
+#include "Device.h"
 
 namespace PX4 {
 
@@ -64,29 +65,9 @@ public:
         return &businfo[_busnum<num_buses?_busnum:0].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:
     static const uint8_t num_buses = 2;
-    static struct bus_info businfo[num_buses];
+    DeviceBus 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;

libraries/AP_HAL_PX4/SPIDevice.cpp

@@ -23,8 +23,6 @@
 #include "Scheduler.h"
 #include "Semaphores.h"
 
-extern bool _px4_thread_should_exit;
-
 namespace PX4 {
 
 static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
@@ -50,7 +48,6 @@ SPIDesc SPIDeviceManager::device_table[] = {
 SPIDevice::SPIDevice(SPIBus &_bus, SPIDesc &_device_desc)
     : bus(_bus)
     , device_desc(_device_desc)
-    , px4dev(device_desc.bus, device_desc.device, device_desc.mode, device_desc.lowspeed)
 {
 }
 
@@ -62,15 +59,30 @@ bool SPIDevice::set_speed(AP_HAL::Device::Speed speed)
 {
     switch (speed) {
     case AP_HAL::Device::SPEED_HIGH:
-        px4dev.set_speed(device_desc.highspeed);
+        frequency = device_desc.highspeed;
         break;
     case AP_HAL::Device::SPEED_LOW:
-        px4dev.set_speed(device_desc.lowspeed);
+        frequency = device_desc.lowspeed;
         break;
     }
     return true;
 }
 
+/*
+  low level transfer function
+ */
+void SPIDevice::do_transfer(uint8_t *send, uint8_t *recv, uint32_t len)
+{
+    irqstate_t state = irqsave();
+    SPI_SETFREQUENCY(bus.dev, frequency);
+    SPI_SETMODE(bus.dev, device_desc.mode);
+    SPI_SETBITS(bus.dev, 8);
+    SPI_SELECT(bus.dev, device_desc.device, true);
+    SPI_EXCHANGE(bus.dev, send, recv, len);
+    SPI_SELECT(bus.dev, device_desc.device, false);
+    irqrestore(state);
+}
+
 bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
                          uint8_t *recv, uint32_t recv_len)
 {
@@ -81,22 +93,20 @@ bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
     if (recv_len > 0) {
         memset(&buf[send_len], 0, recv_len);
     }
-    bool ret = px4dev.do_transfer(buf, buf, send_len+recv_len);
+    do_transfer(buf, buf, send_len+recv_len);
-    if (recv_len > 0 && ret) {
+    if (recv_len > 0) {
         memcpy(recv, &buf[send_len], recv_len);
     }
-    return ret;
+    return true;
 }
 
 bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len)
 {
     uint8_t buf[len];
     memcpy(buf, send, len);
-    bool ret = px4dev.do_transfer(buf, buf, len);
+    do_transfer(buf, buf, len);
-    if (ret) {
     memcpy(recv, buf, len);
-    }
+    return true;
-    return ret;
 }
 
 AP_HAL::Semaphore *SPIDevice::get_semaphore()
@@ -104,76 +114,10 @@ AP_HAL::Semaphore *SPIDevice::get_semaphore()
     return &bus.semaphore;
 }
 
-/*
-  per-bus spi callback thread
-*/
-void *SPIDevice::spi_thread(void *arg)
-{
-    struct SPIBus *binfo = (struct SPIBus *)arg;
-    while (!_px4_thread_should_exit) {
-        uint64_t now = AP_HAL::micros64();
-        uint64_t next_needed = 0;
-        SPIBus::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;
-}
     
 AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
 {
-    if (!bus.thread_started) {
+    return bus.register_periodic_callback(period_usec, cb);
-        bus.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_SPI_PRIORITY;
-        (void)pthread_attr_setschedparam(&thread_attr, &param);
-        pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
-    
-        pthread_create(&bus.thread_ctx, &thread_attr, &SPIDevice::spi_thread, &bus);
-    }
-    SPIBus::callback_info *callback = new SPIBus::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 = bus.callbacks;
-    bus.callbacks = callback;
-
-    return callback;
 }
 
 bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
@@ -202,8 +146,8 @@ SPIDeviceManager::get_device(const char *name)
     SPIDesc &desc = device_table[i];
 
     // find the bus
-    struct SPIBus *busp;
+    SPIBus *busp;
-    for (busp = buses; busp; busp = busp->next) {
+    for (busp = buses; busp; busp = (SPIBus *)busp->next) {
         if (busp->bus == desc.bus) {
             break;
         }
@@ -216,6 +160,7 @@ SPIDeviceManager::get_device(const char *name)
         }
         busp->next = buses;
         busp->bus = desc.bus;
+		busp->dev = up_spiinitialize(desc.bus);
         buses = busp;
     }
 

libraries/AP_HAL_PX4/SPIDevice.h

@@ -19,23 +19,18 @@
 #include <AP_HAL/SPIDevice.h>
 #include <drivers/device/spi.h>
 #include "Semaphores.h"
-#include "SPIWrapper.h"
+#include "Device.h"
+#include "Scheduler.h"
 
 namespace PX4 {
 
 class SPIDesc;
     
-struct SPIBus {
+class SPIBus : public DeviceBus {
-    struct SPIBus *next;
+public:
-    struct callback_info {
+    SPIBus(void) :
-        struct callback_info *next;
+        DeviceBus(APM_SPI_PRIORITY) {}
-        AP_HAL::Device::PeriodicCb cb;
+    struct spi_dev_s *dev;
-        uint32_t period_usec;
-        uint64_t next_usec;        
-    } *callbacks;
-    Semaphore semaphore;
-    pthread_t thread_ctx;
-    bool thread_started;
     uint8_t bus;
 };
 
@@ -68,6 +63,9 @@ public:
     /* See AP_HAL::Device::set_speed() */
     bool set_speed(AP_HAL::Device::Speed speed) override;
 
+    // low level transfer function
+    void do_transfer(uint8_t *send, uint8_t *recv, uint32_t len);
+    
     /* See AP_HAL::Device::transfer() */
     bool transfer(const uint8_t *send, uint32_t send_len,
                   uint8_t *recv, uint32_t recv_len) override;
@@ -89,7 +87,7 @@ public:
 private:
     SPIBus &bus;
     SPIDesc &device_desc;
-    PX4_SPI px4dev;
+    uint32_t frequency;
     
     static void *spi_thread(void *arg);
 };
@@ -107,7 +105,7 @@ public:
 
 private:
     static SPIDesc device_table[];
-    struct SPIBus *buses;
+    SPIBus *buses;
 };
 
 }

libraries/AP_HAL_PX4/SPIWrapper.h

@@ -1,41 +0,0 @@
-#include <AP_HAL/AP_HAL.h>
-
-#include <arch/board/board.h>
-#include "board_config.h"
-#include <drivers/device/spi.h>
-#include "AP_HAL_PX4.h"
-
-extern const AP_HAL::HAL& hal;
-/*
-  wrapper class for SPI to expose protected functions from PX4NuttX
- */
-class PX4::PX4_SPI {
-public:
-    PX4_SPI(uint8_t bus, enum spi_dev_e device, enum spi_mode_e mode, uint32_t frequency) :
-        _bus(bus), _device(device), _mode(mode), _frequency(frequency) {
-		_dev = up_spiinitialize(_bus);
-        SPI_SELECT(_dev, _device, false);
-    }
-    
-    bool do_transfer(uint8_t *send, uint8_t *recv, uint32_t len) {
-        irqstate_t state = irqsave();
-        SPI_SETFREQUENCY(_dev, _frequency);
-        SPI_SETMODE(_dev, _mode);
-        SPI_SETBITS(_dev, 8);
-        SPI_SELECT(_dev, _device, true);
-        SPI_EXCHANGE(_dev, send, recv, len);
-        SPI_SELECT(_dev, _device, false);
-        irqrestore(state);
-        return true;
-    }
-    void set_speed(uint32_t speed_hz) {
-        _frequency = speed_hz;
-    }
-private:
-    struct spi_dev_s *_dev;
-    uint8_t _bus;
-    enum spi_dev_e _device;
-    enum spi_mode_e _mode;
-    uint32_t _frequency;
-};
-