From d492b72a2cc1bf9ab8dc5a90f30f9a937b54ed7f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jan 2013 21:12:10 +1100 Subject: [PATCH] HAL_PX4: use /dev/ttyS0 for uartA this also changes txspace and available to use FIONWRITE and FIONREAD --- libraries/AP_HAL_PX4/HAL_PX4_Class.cpp | 2 +- libraries/AP_HAL_PX4/UARTDriver.cpp | 99 ++++++++++++++++---------- libraries/AP_HAL_PX4/UARTDriver.h | 8 +-- 3 files changed, 66 insertions(+), 43 deletions(-) diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp index 44fa7d63bb..e8fe3de781 100644 --- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp +++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp @@ -32,7 +32,7 @@ static PX4Scheduler schedulerInstance; static PX4EEPROMStorage storageDriver; // only one real UART driver for now -static PX4UARTDriver uartADriver; +static PX4UARTDriver uartADriver("/dev/ttyS0"); static Empty::EmptyUARTDriver uartBDriver; static Empty::EmptyUARTDriver uartCDriver; diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp index eace26daf3..dba4b66d34 100644 --- a/libraries/AP_HAL_PX4/UARTDriver.cpp +++ b/libraries/AP_HAL_PX4/UARTDriver.cpp @@ -7,10 +7,19 @@ #include #include #include +#include +#include +#include +#include +#include using namespace PX4; -PX4UARTDriver::PX4UARTDriver() {} +extern const AP_HAL::HAL& hal; + +PX4UARTDriver::PX4UARTDriver(const char *devpath) { + _devpath = devpath; +} extern const AP_HAL::HAL& hal; @@ -21,17 +30,14 @@ extern const AP_HAL::HAL& hal; void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) { if (!_initialised) { - _rxBufSize = 128; - _txBufSize = 16; - _rxChar = -1; + _fd = open(_devpath, O_RDWR); + if (_fd == -1) { + fprintf(stdout, "Failed to open UART device %s - %s\n", + _devpath, strerror(errno)); + return; + } _initialised = true; } - if (rxS != 0) { - _rxBufSize = rxS; - } - if (txS != 0) { - _txBufSize = txS; - } } void PX4UARTDriver::begin(uint32_t b) { @@ -42,7 +48,9 @@ void PX4UARTDriver::begin(uint32_t b) { void PX4UARTDriver::end() {} void PX4UARTDriver::flush() {} bool PX4UARTDriver::is_initialized() { return true; } -void PX4UARTDriver::set_blocking_writes(bool blocking) {} +void PX4UARTDriver::set_blocking_writes(bool blocking) { + _nonblocking_writes = !blocking; +} bool PX4UARTDriver::tx_pending() { return false; } /* PX4 implementations of BetterStream virtual methods */ @@ -57,76 +65,91 @@ void PX4UARTDriver::println_P(const prog_char_t *pstr) { void PX4UARTDriver::printf(const char *fmt, ...) { va_list ap; va_start(ap, fmt); - _vdprintf(1, fmt, ap); + _vdprintf(_fd, fmt, ap); va_end(ap); } void PX4UARTDriver::_printf_P(const prog_char *fmt, ...) { va_list ap; va_start(ap, fmt); - _vdprintf(1, fmt, ap); + _vdprintf(_fd, fmt, ap); va_end(ap); } void PX4UARTDriver::vprintf(const char *fmt, va_list ap) { - _vdprintf(1, fmt, ap); + _vdprintf(_fd, fmt, ap); } void PX4UARTDriver::vprintf_P(const prog_char *fmt, va_list ap) { - _vdprintf(1, fmt, ap); + _vdprintf(_fd, fmt, ap); } /* PX4 implementations of Stream virtual methods */ int16_t PX4UARTDriver::available() { - if (_rxChar == -1) { - struct pollfd fds; - fds.fd = 0; - fds.events = POLLIN; - fds.revents = 0; - - if (poll(&fds, 1, 0) == 1) { - uint8_t c; - if (::read(0, &c, 1) == 1) { - _rxChar = c; - } - } + int ret = 0; + if (!_initialised) { + return 0; } - if (_rxChar != -1) { - return 1; + if (ioctl(_fd, FIONREAD, (long unsigned int)&ret) == 0 && ret > 0) { + if (ret > 90) ret = 90; + return ret; } return 0; } int16_t PX4UARTDriver::txspace() { - return _txBufSize; + int ret = 0; + if (!_initialised) { + return 0; + } + if (ioctl(_fd, FIONWRITE, (long unsigned int)&ret) == 0 && ret > 0) { + if (ret > 90) ret = 90; + return ret; + } + return 0; } int16_t PX4UARTDriver::read() { if (available() > 0) { - uint8_t ret = _rxChar; - _rxChar = -1; - return ret; + uint8_t c; + if (::read(_fd, &c, 1) == 1) { + return c; + } } return -1; } int16_t PX4UARTDriver::peek() { - if (available() > 0) { - return _rxChar; - } return -1; } /* PX4 implementations of Print virtual methods */ size_t PX4UARTDriver::write(uint8_t c) { - return ::write(1, &c, 1); + if (!_initialised) { + return 0; + } + if (_nonblocking_writes && txspace() == 0) { + return 0; + } + return ::write(_fd, &c, 1); } void PX4UARTDriver::_vdprintf(int fd, const char *fmt, va_list ap) { char buf[128]; + if (!_initialised) { + return; + } int len = vsnprintf(buf, sizeof(buf), fmt, ap); if (len > 0) { - ::write(1, buf, len); + if (_nonblocking_writes) { + int16_t space = txspace(); + if (space < len) { + len = space; + } + } + if (len > 0) { + ::write(_fd, buf, len); + } } } diff --git a/libraries/AP_HAL_PX4/UARTDriver.h b/libraries/AP_HAL_PX4/UARTDriver.h index 2e5e13fe2a..a58d05d93e 100644 --- a/libraries/AP_HAL_PX4/UARTDriver.h +++ b/libraries/AP_HAL_PX4/UARTDriver.h @@ -6,7 +6,7 @@ class PX4::PX4UARTDriver : public AP_HAL::UARTDriver { public: - PX4UARTDriver(); + PX4UARTDriver(const char *devpath); /* PX4 implementations of UARTDriver virtual methods */ void begin(uint32_t b); void begin(uint32_t b, uint16_t rxS, uint16_t txS); @@ -37,10 +37,10 @@ public: bool _initialised; private: + const char *_devpath; + int _fd; + bool _nonblocking_writes; void _vdprintf(int fd, const char *fmt, va_list ap); - uint16_t _rxBufSize; - uint16_t _txBufSize; - int16_t _rxChar; }; #endif // __AP_HAL_PX4_UARTDRIVER_H__