HAL_PX4: implement peek() and available()

keep a 1 byte buffer

libraries/AP_HAL_PX4/UARTDriver.cpp

@@ -2,20 +2,43 @@
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
 #include "UARTDriver.h"
+
 #include <stdio.h>
 #include <unistd.h>
+#include <stdlib.h>
+#include <poll.h>
 
 using namespace PX4;
 
 PX4UARTDriver::PX4UARTDriver() {}
 
+extern const AP_HAL::HAL& hal;
+
 /*
   this UART driver just maps to fd 0/1, which goes to whatever is
   setup for the PX4 console. Baud rate control is not available.
  */
 
-void PX4UARTDriver::begin(uint32_t b) {}
-void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) {}
+void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) {
+	if (!_initialised) {
+		_rxBufSize = 128;
+		_txBufSize = 16;
+		_rxChar = -1;
+		_initialised = true;
+	}
+	if (rxS != 0) {
+		_rxBufSize = rxS;
+	}
+	if (txS != 0) {
+		_txBufSize = txS;
+	}
+}
+
+void PX4UARTDriver::begin(uint32_t b) {
+	begin(b, 0, 0);
+}
+
+
 void PX4UARTDriver::end() {}
 void PX4UARTDriver::flush() {}
 bool PX4UARTDriver::is_initialized() { return true; }
@@ -54,19 +77,44 @@ void PX4UARTDriver::vprintf_P(const prog_char *fmt, va_list ap) {
 }
 
 /* PX4 implementations of Stream virtual methods */
-int16_t PX4UARTDriver::available() { return 0; }
-int16_t PX4UARTDriver::txspace() { return 128; }
+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;
+			}
+		}
+	}
+	if (_rxChar != -1) {
+		return 1;
+	}
+	return 0;
+}
+
+int16_t PX4UARTDriver::txspace() { 
+	return _txBufSize;
+}
 
 int16_t PX4UARTDriver::read() { 
-	uint8_t c;
-	if (::read(0, &c, 1) == 1) {
-		return c;
+	if (available() > 0) {
+		uint8_t ret = _rxChar;
+		_rxChar = -1;
+		return ret;
 	}
 	return -1;
 }
 
 int16_t PX4UARTDriver::peek() { 
-	return -1; 
+	if (available() > 0) {
+		return _rxChar;
+	}
+	return -1;
 }
 
 /* PX4 implementations of Print virtual methods */

libraries/AP_HAL_PX4/UARTDriver.h

@@ -34,8 +34,13 @@ public:
     /* PX4 implementations of Print virtual methods */
     size_t write(uint8_t c);
 
+    bool _initialised;
+
 private:
     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__