HAL_PX4: added a uartA driver

uses stdin/stdout for IO

libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h

@@ -5,6 +5,7 @@
 namespace PX4 {
 	class PX4ConsoleDriver;
 	class PX4Scheduler;
+	class PX4UARTDriver;
 }
 
 #endif //__AP_HAL_PX4_NAMESPACE_H__

libraries/AP_HAL_PX4/Console.cpp

@@ -3,14 +3,16 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
 #include <stdarg.h>
 #include "Console.h"
+#include "UARTDriver.h"
 #include <stdio.h>
 
 using namespace PX4;
 
 PX4ConsoleDriver::PX4ConsoleDriver() {}
 
-void PX4ConsoleDriver::init(void* unused)
+void PX4ConsoleDriver::init(void* uart)
 {
+	_uart = (PX4UARTDriver *)uart;
 }
 
 void PX4ConsoleDriver::backend_open()
@@ -38,44 +40,43 @@ void PX4ConsoleDriver::println_P(const prog_char_t *pstr) {
 void PX4ConsoleDriver::printf(const char *fmt, ...) {
     va_list ap;
     va_start(ap, fmt);
-    vfprintf(stdout, fmt, ap);
+    _uart->vprintf(fmt, ap);
     va_end(ap);
 }
 
 void PX4ConsoleDriver::_printf_P(const prog_char *fmt, ...) {
     va_list ap;
-    va_start(ap,fmt);
+    va_start(ap, fmt);
-    vfprintf(stdout, fmt, ap);
+    _uart->vprintf(fmt, ap);
     va_end(ap);
 }
 
 void PX4ConsoleDriver::vprintf(const char *fmt, va_list ap) {
-	vfprintf(stdout, fmt, ap);
+	_uart->vprintf(fmt, ap);
 }
 
 void PX4ConsoleDriver::vprintf_P(const prog_char *fmt, va_list ap) {
-	vfprintf(stdout, fmt, ap);
+	_uart->vprintf(fmt, ap);
 }
 
 int16_t PX4ConsoleDriver::available() {
-	return 0;
+	return _uart->available();
 }
 
 int16_t PX4ConsoleDriver::txspace() {
-	return 0;
+	return _uart->txspace();
 }
 
 int16_t PX4ConsoleDriver::read() {
-	return 0;
+	return _uart->read();
 }
 
 int16_t PX4ConsoleDriver::peek() {
-	return 0;
+	return _uart->peek();
 }
 
 size_t PX4ConsoleDriver::write(uint8_t c) {
-	fputc(c, stdout);
+	return _uart->write(c);
-	return 1;
 }
 
 #endif

libraries/AP_HAL_PX4/Console.h

@@ -27,6 +27,9 @@ public:
     int16_t peek();
 
     size_t write(uint8_t c);
+
+private:
+    PX4UARTDriver *_uart;
 };
 
 #endif // __AP_HAL_PX4_CONSOLE_H__

libraries/AP_HAL_PX4/HAL_PX4_Class.cpp

@@ -9,6 +9,7 @@
 #include "HAL_PX4_Class.h"
 #include "Console.h"
 #include "Scheduler.h"
+#include "UARTDriver.h"
 
 #include <AP_HAL_Empty.h>
 #include <AP_HAL_Empty_Private.h>
@@ -17,9 +18,6 @@
 
 using namespace PX4;
 
-static Empty::EmptyUARTDriver uartADriver;
-static Empty::EmptyUARTDriver uartBDriver;
-static Empty::EmptyUARTDriver uartCDriver;
 static Empty::EmptyI2CDriver  i2cDriver;
 static Empty::EmptySPIDeviceManager spiDeviceManager;
 static Empty::EmptyAnalogIn analogIn;
@@ -32,6 +30,11 @@ static Empty::EmptyUtil utilInstance;
 static PX4ConsoleDriver consoleDriver;
 static PX4Scheduler schedulerInstance;
 
+// only one real UART driver for now
+static PX4UARTDriver uartADriver;
+static Empty::EmptyUARTDriver uartBDriver;
+static Empty::EmptyUARTDriver uartCDriver;
+
 HAL_PX4::HAL_PX4() :
     AP_HAL::HAL(
 	    &uartADriver,  /* uartA */

libraries/AP_HAL_PX4/UARTDriver.cpp

@@ -0,0 +1,78 @@
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#include "UARTDriver.h"
+#include <stdio.h>
+#include <unistd.h>
+
+using namespace PX4;
+
+PX4UARTDriver::PX4UARTDriver() {}
+
+/*
+  this UART driver just maps to stdin/stdout, 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::end() {}
+void PX4UARTDriver::flush() {}
+bool PX4UARTDriver::is_initialized() { return true; }
+void PX4UARTDriver::set_blocking_writes(bool blocking) {}
+bool PX4UARTDriver::tx_pending() { return false; }
+
+/* PX4 implementations of BetterStream virtual methods */
+void PX4UARTDriver::print_P(const prog_char_t *pstr) {
+	print(pstr);
+}
+
+void PX4UARTDriver::println_P(const prog_char_t *pstr) {
+	println(pstr);
+}
+
+void PX4UARTDriver::printf(const char *fmt, ...) {
+    va_list ap;
+    va_start(ap, fmt);
+    vfprintf(stdout, fmt, ap);
+    va_end(ap);	
+}
+
+void PX4UARTDriver::_printf_P(const prog_char *fmt, ...) {
+    va_list ap;
+    va_start(ap, fmt);
+    vfprintf(stdout, fmt, ap);
+    va_end(ap);	
+}
+
+void PX4UARTDriver::vprintf(const char *fmt, va_list ap) {
+	vfprintf(stdout, fmt, ap);
+}
+
+void PX4UARTDriver::vprintf_P(const prog_char *fmt, va_list ap) {
+	vfprintf(stdout, fmt, ap);
+}
+
+/* PX4 implementations of Stream virtual methods */
+int16_t PX4UARTDriver::available() { return 0; }
+int16_t PX4UARTDriver::txspace() { return 128; }
+
+int16_t PX4UARTDriver::read() { 
+	uint8_t c;
+	if (fread(&c, 1, 1, stdin) == 1) {
+		return c;
+	}
+	return -1;
+}
+
+int16_t PX4UARTDriver::peek() { 
+	return -1; 
+}
+
+/* PX4 implementations of Print virtual methods */
+size_t PX4UARTDriver::write(uint8_t c) { 
+	return fwrite(&c, 1, 1, stdout);
+}
+
+#endif

libraries/AP_HAL_PX4/UARTDriver.h

@@ -0,0 +1,38 @@
+
+#ifndef __AP_HAL_PX4_UARTDRIVER_H__
+#define __AP_HAL_PX4_UARTDRIVER_H__
+
+#include <AP_HAL_PX4.h>
+
+class PX4::PX4UARTDriver : public AP_HAL::UARTDriver {
+public:
+    PX4UARTDriver();
+    /* PX4 implementations of UARTDriver virtual methods */
+    void begin(uint32_t b);
+    void begin(uint32_t b, uint16_t rxS, uint16_t txS);
+    void end();
+    void flush();
+    bool is_initialized();
+    void set_blocking_writes(bool blocking);
+    bool tx_pending();
+
+    /* PX4 implementations of BetterStream virtual methods */
+    void print_P(const prog_char_t *pstr);
+    void println_P(const prog_char_t *pstr);
+    void printf(const char *pstr, ...);
+    void _printf_P(const prog_char *pstr, ...);
+
+    void vprintf(const char* fmt, va_list ap);
+    void vprintf_P(const prog_char* fmt, va_list ap);
+
+    /* PX4 implementations of Stream virtual methods */
+    int16_t available();
+    int16_t txspace();
+    int16_t read();
+    int16_t peek();
+
+    /* PX4 implementations of Print virtual methods */
+    size_t write(uint8_t c);
+};
+
+#endif // __AP_HAL_PX4_UARTDRIVER_H__