From e4a21f291d99ac830634b53dba296e0149d871bf Mon Sep 17 00:00:00 2001 From: Staroselskii Georgii Date: Fri, 7 Nov 2014 13:17:50 +0300 Subject: [PATCH] AP_HAL_Linux: added LinuxSPIUARTDriver that can handle SPI-driven Ublox --- libraries/AP_HAL/UARTDriver.h | 9 + .../AP_HAL_Linux/AP_HAL_Linux_Namespace.h | 1 + libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h | 1 + libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 4 + libraries/AP_HAL_Linux/SPIUARTDriver.cpp | 202 ++++++++++++++++++ libraries/AP_HAL_Linux/SPIUARTDriver.h | 32 +++ libraries/AP_HAL_Linux/Scheduler.cpp | 1 + libraries/AP_HAL_Linux/UARTDriver.cpp | 9 - libraries/AP_HAL_Linux/UARTDriver.h | 43 ++-- 9 files changed, 273 insertions(+), 29 deletions(-) create mode 100644 libraries/AP_HAL_Linux/SPIUARTDriver.cpp create mode 100644 libraries/AP_HAL_Linux/SPIUARTDriver.h diff --git a/libraries/AP_HAL/UARTDriver.h b/libraries/AP_HAL/UARTDriver.h index c5ea81ae37..cf11b5c273 100644 --- a/libraries/AP_HAL/UARTDriver.h +++ b/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: diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index f9a1c8589a..270578bfae 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/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; diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h index c541cced6d..a792710d44 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h +++ b/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" diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index de521c6e33..39d5504e70 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/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; diff --git a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp new file mode 100644 index 0000000000..c15b487a61 --- /dev/null +++ b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp @@ -0,0 +1,202 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include +#include +#include "SPIUARTDriver.h" + +extern const AP_HAL::HAL& hal; + +#define SPIUART_DEBUG 1 +#if SPIUART_DEBUG +#include +#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 diff --git a/libraries/AP_HAL_Linux/SPIUARTDriver.h b/libraries/AP_HAL_Linux/SPIUARTDriver.h new file mode 100644 index 0000000000..325cc417a8 --- /dev/null +++ b/libraries/AP_HAL_Linux/SPIUARTDriver.h @@ -0,0 +1,32 @@ +#ifndef __AP_HAL_LINUX_SPIUARTDRIVER_H__ +#define __AP_HAL_LINUX_SPIUARTDRIVER_H__ + +#include + +#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__ diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 420769b09a..abcd64d33a 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -7,6 +7,7 @@ #include "RCInput.h" #include "UARTDriver.h" #include "Util.h" +#include "SPIUARTDriver.h" #include #include #include diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp index 55e2a8124d..f07ca0a5f5 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.cpp +++ b/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? */ diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h index 0c53b1cc30..dcde89abf8 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.h +++ b/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__