From 9373a4e5b352067a29b8ea9da1ebf676ead771d6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jan 2013 11:03:05 +1100 Subject: [PATCH] HAL_PX4: added a uartA driver uses stdin/stdout for IO --- libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h | 1 + libraries/AP_HAL_PX4/Console.cpp | 25 +++---- libraries/AP_HAL_PX4/Console.h | 3 + libraries/AP_HAL_PX4/HAL_PX4_Class.cpp | 9 ++- libraries/AP_HAL_PX4/UARTDriver.cpp | 78 +++++++++++++++++++++ libraries/AP_HAL_PX4/UARTDriver.h | 38 ++++++++++ 6 files changed, 139 insertions(+), 15 deletions(-) create mode 100644 libraries/AP_HAL_PX4/UARTDriver.cpp create mode 100644 libraries/AP_HAL_PX4/UARTDriver.h diff --git a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h index 5c83bceaec..3b8d86a0cd 100644 --- a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h +++ b/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__ diff --git a/libraries/AP_HAL_PX4/Console.cpp b/libraries/AP_HAL_PX4/Console.cpp index 037766f547..d57af5c846 100644 --- a/libraries/AP_HAL_PX4/Console.cpp +++ b/libraries/AP_HAL_PX4/Console.cpp @@ -3,14 +3,16 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #include #include "Console.h" +#include "UARTDriver.h" #include 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); - vfprintf(stdout, fmt, ap); + va_start(ap, fmt); + _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 1; + return _uart->write(c); } #endif diff --git a/libraries/AP_HAL_PX4/Console.h b/libraries/AP_HAL_PX4/Console.h index 04ece0c495..979c462ff0 100644 --- a/libraries/AP_HAL_PX4/Console.h +++ b/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__ diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp index aa8c466ff3..c339e1adbb 100644 --- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp +++ b/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 #include @@ -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 */ diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp new file mode 100644 index 0000000000..e6db89450d --- /dev/null +++ b/libraries/AP_HAL_PX4/UARTDriver.cpp @@ -0,0 +1,78 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#include "UARTDriver.h" +#include +#include + +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 diff --git a/libraries/AP_HAL_PX4/UARTDriver.h b/libraries/AP_HAL_PX4/UARTDriver.h new file mode 100644 index 0000000000..450a717663 --- /dev/null +++ b/libraries/AP_HAL_PX4/UARTDriver.h @@ -0,0 +1,38 @@ + +#ifndef __AP_HAL_PX4_UARTDRIVER_H__ +#define __AP_HAL_PX4_UARTDRIVER_H__ + +#include + +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__