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

@@ -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;
 

libraries/AP_HAL_PX4/UARTDriver.cpp

@@ -7,10 +7,19 @@
 #include <unistd.h>
 #include <stdlib.h>
 #include <poll.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
 
 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);
+		}
 	}
 }
 

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__