AP_HAL_Linux: added LinuxSPIUARTDriver that can handle SPI-driven Ublox

libraries/AP_HAL/UARTDriver.h

@@ -7,6 +7,15 @@
 #include "AP_HAL_Namespace.h"
 #include "utility/BetterStream.h"
 
+/*
+  buffer handling macros
+ */
+#define BUF_AVAILABLE(buf) ((buf##_head > (_tail=buf##_tail))? (buf##_size - buf##_head) + _tail: _tail - buf##_head)
+#define BUF_SPACE(buf) (((_head=buf##_head) > buf##_tail)?(_head - buf##_tail) - 1:((buf##_size - buf##_tail) + _head) - 1)
+#define BUF_EMPTY(buf) (buf##_head == buf##_tail)
+#define BUF_ADVANCETAIL(buf, n) buf##_tail = (buf##_tail + n) % buf##_size
+#define BUF_ADVANCEHEAD(buf, n) buf##_head = (buf##_head + n) % buf##_size
+
 /* Pure virtual UARTDriver class */
 class AP_HAL::UARTDriver : public AP_HAL::BetterStream {
 public:

libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h

@@ -9,6 +9,7 @@
 
 namespace Linux {
     class LinuxUARTDriver;
+    class LinuxSPIUARTDriver;
     class LinuxI2CDriver;
     class LinuxSPIDeviceManager;
     class LinuxSPIDeviceDriver;

libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h

@@ -7,6 +7,7 @@
  */
 
 #include "UARTDriver.h"
+#include "SPIUARTDriver.h"
 #include "I2CDriver.h"
 #include "SPIDriver.h"
 #include "AnalogIn.h"

libraries/AP_HAL_Linux/HAL_Linux_Class.cpp

@@ -16,7 +16,11 @@ using namespace Linux;
 
 // 3 serial ports on Linux for now
 static LinuxUARTDriver uartADriver(true);
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
+static LinuxSPIUARTDriver uartBDriver;
+#else
 static LinuxUARTDriver uartBDriver(false);
+#endif
 static LinuxUARTDriver uartCDriver(false);
 
 static LinuxSemaphore  i2cSemaphore;

libraries/AP_HAL_Linux/SPIUARTDriver.cpp

@@ -0,0 +1,202 @@
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#include <stdlib.h>
+#include <cstdio>
+#include "SPIUARTDriver.h"
+
+extern const AP_HAL::HAL& hal;
+
+#define SPIUART_DEBUG 1
+#if SPIUART_DEBUG
+#include <cassert>
+#define debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
+#define error(fmt, args ...)  do {fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
+#else
+#define debug(fmt, args ...)  
+#define error(fmt, args ...)  
+#endif
+
+using namespace Linux;
+
+LinuxSPIUARTDriver::LinuxSPIUARTDriver() :
+    LinuxUARTDriver(false),
+    _spi(NULL),
+    _spi_sem(NULL),
+    _last_update_timestamp(0),
+    _buffer(NULL),
+    _external(false)
+{
+    _readbuf = NULL;
+    _writebuf = NULL;
+}
+
+bool LinuxSPIUARTDriver::sem_take_nonblocking()
+{
+    return _spi_sem->take_nonblocking();
+}
+
+void LinuxSPIUARTDriver::sem_give()
+{
+    _spi_sem->give();
+}
+
+void LinuxSPIUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
+{
+    if (device_path != NULL) {
+        LinuxUARTDriver::begin(b,rxS,txS);
+        if ( is_initialized()) {
+            _external = true;
+            return;
+        }
+    }
+
+   if (rxS < 1024) {
+       rxS = 2048;
+   }
+   if (txS < 1024) {
+       txS = 2048;
+   }
+
+   /*
+     allocate the read buffer
+   */
+   if (rxS != 0 && rxS != _readbuf_size) {
+       _readbuf_size = rxS;
+       if (_readbuf != NULL) {
+           free(_readbuf);
+       }
+       _readbuf = (uint8_t *)malloc(_readbuf_size);
+       _readbuf_head = 0;
+       _readbuf_tail = 0;
+   }
+
+   /*
+     allocate the write buffer
+   */
+   if (txS != 0 && txS != _writebuf_size) {
+       _writebuf_size = txS;
+       if (_writebuf != NULL) {
+           free(_writebuf);
+       }
+       _writebuf = (uint8_t *)malloc(_writebuf_size);
+       _writebuf_head = 0;
+       _writebuf_tail = 0;
+   }
+
+   _buffer = new uint8_t[rxS];
+
+   _spi = hal.spi->device(AP_HAL::SPIDevice_Ublox);
+
+   if (_spi == NULL) {
+       hal.scheduler->panic("Cannot get SPIDevice_MPU9250");
+   }
+
+   _spi_sem = _spi->get_semaphore();
+
+   _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
+   _initialised = true;
+}
+
+int LinuxSPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size)
+{
+    if (_external) {
+        return LinuxUARTDriver::_write_fd(buf,size);
+    } 
+
+    if (!sem_take_nonblocking()) {
+        return 0;
+    }
+
+    _spi->transaction(buf, _buffer, size);
+
+    BUF_ADVANCEHEAD(_writebuf, size);
+
+    uint16_t ret = size;
+
+    /* Since all SPI-transactions are transfers we need update
+     * the _readbuf. I do believe there is a way to encapsulate 
+     * this operation since it's the same as in the
+     * LinuxUARTDriver::write().  
+     */
+
+    uint8_t *buffer = _buffer;
+
+    uint16_t _head, space;
+    space = BUF_SPACE(_readbuf);
+
+    if (space == 0) {
+        sem_give();
+        return ret;
+    }
+
+    if (size > space) {
+        size = space;
+    }
+
+    if (_readbuf_tail < _head) {
+        // perform as single memcpy
+        assert(_readbuf_tail+size <= _readbuf_size);
+        memcpy(&_readbuf[_readbuf_tail], buffer, size);
+        BUF_ADVANCETAIL(_readbuf, size);
+        sem_give();
+        return ret;
+    }
+
+    // perform as two memcpy calls
+    uint16_t n = _readbuf_size - _readbuf_tail;
+    if (n > size) n = size;
+    assert(_readbuf_tail+n <= _readbuf_size);
+    memcpy(&_readbuf[_readbuf_tail], buffer, n);
+    BUF_ADVANCETAIL(_readbuf, n);
+    buffer += n;
+    n = size - n;
+    if (n > 0) {
+        assert(_readbuf_tail+n <= _readbuf_size);
+        memcpy(&_readbuf[_readbuf_tail], buffer, n);
+        BUF_ADVANCETAIL(_readbuf, n);
+    }
+
+    sem_give();
+
+    return ret;
+    
+}
+
+static const uint8_t ff_stub[3000] = {0xff};
+int LinuxSPIUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
+{
+    if (_external) {
+        return LinuxUARTDriver::_read_fd(buf, n);
+    }
+
+    if (!sem_take_nonblocking()) {
+        return 0;
+    }
+
+    _spi->transaction(ff_stub, buf, n);
+
+    BUF_ADVANCETAIL(_readbuf, n);
+
+    sem_give();
+    return n;
+}
+
+void LinuxSPIUARTDriver::_timer_tick(void)
+{
+    if (_external) {
+        LinuxUARTDriver::_timer_tick();
+        return;
+    }
+    /* lower the update rate */
+    if (hal.scheduler->micros() - _last_update_timestamp < 50000) {
+        return;
+    }
+
+    LinuxUARTDriver::_timer_tick();
+
+    _last_update_timestamp = hal.scheduler->micros();
+}
+
+#endif

libraries/AP_HAL_Linux/SPIUARTDriver.h

@@ -0,0 +1,32 @@
+#ifndef __AP_HAL_LINUX_SPIUARTDRIVER_H__
+#define __AP_HAL_LINUX_SPIUARTDRIVER_H__
+
+#include <AP_HAL_Linux.h>
+
+#include "UARTDriver.h"
+
+
+class Linux::LinuxSPIUARTDriver : public Linux::LinuxUARTDriver {
+public:
+    LinuxSPIUARTDriver();
+    void begin(uint32_t b, uint16_t rxS, uint16_t txS);
+    void _timer_tick(void);
+
+protected:
+    int _write_fd(const uint8_t *buf, uint16_t n);
+    int _read_fd(uint8_t *buf, uint16_t n);
+
+private:
+    bool sem_take_nonblocking();
+    void sem_give();
+
+    AP_HAL::SPIDeviceDriver *_spi;
+    AP_HAL::Semaphore *_spi_sem;
+
+    uint32_t _last_update_timestamp;
+
+    uint8_t *_buffer;
+    bool _external;
+};
+
+#endif //__AP_HAL_LINUX_SPIUARTDRIVER_H__

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -7,6 +7,7 @@
 #include "RCInput.h"
 #include "UARTDriver.h"
 #include "Util.h"
+#include "SPIUARTDriver.h"
 #include <sys/time.h>
 #include <poll.h>
 #include <unistd.h>

libraries/AP_HAL_Linux/UARTDriver.cpp

@@ -419,15 +419,6 @@ void LinuxUARTDriver::set_blocking_writes(bool blocking)
 }
 
 
-/*
-  buffer handling macros
- */
-#define BUF_AVAILABLE(buf) ((buf##_head > (_tail=buf##_tail))? (buf##_size - buf##_head) + _tail: _tail - buf##_head)
-#define BUF_SPACE(buf) (((_head=buf##_head) > buf##_tail)?(_head - buf##_tail) - 1:((buf##_size - buf##_tail) + _head) - 1)
-#define BUF_EMPTY(buf) (buf##_head == buf##_tail)
-#define BUF_ADVANCETAIL(buf, n) buf##_tail = (buf##_tail + n) % buf##_size
-#define BUF_ADVANCEHEAD(buf, n) buf##_head = (buf##_head + n) % buf##_size
-
 /*
   do we have any bytes pending transmission?
  */

libraries/AP_HAL_Linux/UARTDriver.h

@@ -27,42 +27,23 @@ public:
 
     void set_device_path(char *path);
 
-    void _timer_tick(void);
+    virtual void _timer_tick(void);
 
     enum flow_control get_flow_control(void) { return _flow_control; }
 
 private:
-    char *device_path;
     int _rd_fd;
     int _wr_fd;
     bool _nonblocking_writes;
     bool _console;
-    volatile bool _initialised;
     volatile bool _in_timer;
     uint16_t _base_port;
     char *_ip;
     char *_flag;
     bool _connected; // true if a client has connected         
     bool _packetise; // true if writes should try to be on mavlink boundaries
-
-    // we use in-task ring buffers to reduce the system call cost
-    // of ::read() and ::write() in the main loop
-    uint8_t *_readbuf;
-    uint16_t _readbuf_size;
     enum flow_control _flow_control;
 
-    // _head is where the next available data is. _tail is where new
-    // data is put
-    volatile uint16_t _readbuf_head;
-    volatile uint16_t _readbuf_tail;
-
-    uint8_t *_writebuf;
-    uint16_t _writebuf_size;
-    volatile uint16_t _writebuf_head;
-    volatile uint16_t _writebuf_tail;
-
-    int _write_fd(const uint8_t *buf, uint16_t n);
-    int _read_fd(uint8_t *buf, uint16_t n);
     void _tcp_start_connection(bool wait_for_connection);
     void _udp_start_connection(void);
 
@@ -75,6 +56,28 @@ private:
 
     enum device_type _parseDevicePath(const char *arg);
     uint64_t _last_write_time;    
+
+protected:
+    char *device_path;
+    volatile bool _initialised;
+    // we use in-task ring buffers to reduce the system call cost
+    // of ::read() and ::write() in the main loop
+    uint8_t *_readbuf;
+    uint16_t _readbuf_size;
+
+    // _head is where the next available data is. _tail is where new
+    // data is put
+    volatile uint16_t _readbuf_head;
+    volatile uint16_t _readbuf_tail;
+
+    uint8_t *_writebuf;
+    uint16_t _writebuf_size;
+    volatile uint16_t _writebuf_head;
+    volatile uint16_t _writebuf_tail;
+
+    virtual int _write_fd(const uint8_t *buf, uint16_t n);
+    virtual int _read_fd(uint8_t *buf, uint16_t n);
+
 };
 
 #endif // __AP_HAL_LINUX_UARTDRIVER_H__