diff --git a/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h b/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h index 23ec98e7ea..dcc8a78caa 100644 --- a/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h +++ b/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h @@ -16,6 +16,7 @@ namespace AP_HAL_AVR { class CommonDataflash; class APM1Dataflash; class APM2Dataflash; + class AVRConsoleDriver; class ArduinoGPIO; class APM1RCInput; class APM2RCInput; diff --git a/libraries/AP_HAL_AVR/Console.cpp b/libraries/AP_HAL_AVR/Console.cpp new file mode 100644 index 0000000000..b6479d91a8 --- /dev/null +++ b/libraries/AP_HAL_AVR/Console.cpp @@ -0,0 +1,104 @@ + +#include + +#include "vprintf.h" + +#include +#include "Console.h" +using namespace AP_HAL_AVR; + +AVRConsoleDriver::AVRConsoleDriver() : + _user_backend(false) +{} + +// ConsoleDriver method implementations /////////////////////////////////////// +// +void AVRConsoleDriver::init(void* base_uart) { + _base_uart = (AP_HAL::UARTDriver*) base_uart; +} + + +void AVRConsoleDriver::backend_open() { + _user_backend = true; +} + +void AVRConsoleDriver::backend_close() { + _user_backend = false; +} + +int AVRConsoleDriver::backend_read(uint8_t *data, int len) { + return 0; +} + +int AVRConsoleDriver::backend_write(const uint8_t *data, int len) { + return 0; +} +// Print method implementations ///////////////////////////////////////// + +size_t AVRConsoleDriver::write(uint8_t c) { + if (_user_backend) { + return 1; + } else { + return _base_uart->write(c); + } +} + +// BetterStream method implementations ///////////////////////////////////////// +void AVRConsoleDriver::print_P(const prog_char_t *s) { + char c; + while ('\0' != (c = pgm_read_byte((const prog_char *)s++))) + write(c); +} + +void AVRConsoleDriver::println_P(const prog_char_t *s) { + print_P(s); + println(); +} + +void AVRConsoleDriver::printf(const char *fmt, ...) { + va_list ap; + va_start(ap, fmt); + vprintf((AP_HAL::Print*)this, 0, fmt, ap); + va_end(ap); +} + +void AVRConsoleDriver::_printf_P(const prog_char *fmt, ...) { + va_list ap; + va_start(ap, fmt); + vprintf((AP_HAL::Print*)this, 1, fmt, ap); + va_end(ap); +} + +// Stream method implementations ///////////////////////////////////////// +int AVRConsoleDriver::available(void) { + if (_user_backend) { + return INT_MAX; + } else { + return _base_uart->available(); + } +} + +int AVRConsoleDriver::txspace(void) { + if (_user_backend) { + return INT_MAX; + } else { + return _base_uart->txspace(); + } +} + +int AVRConsoleDriver::read() { + if (_user_backend) { + return -1; + } else { + return _base_uart->read(); + } +} + +int AVRConsoleDriver::peek() { + if (_user_backend) { + return -1; + } else { + return _base_uart->peek(); + } +} + diff --git a/libraries/AP_HAL_AVR/Console.h b/libraries/AP_HAL_AVR/Console.h new file mode 100644 index 0000000000..2d74302dcf --- /dev/null +++ b/libraries/AP_HAL_AVR/Console.h @@ -0,0 +1,40 @@ + +#ifndef __AP_HAL_AVR_CONSOLE_DRIVER_H__ +#define __AP_HAL_AVR_CONSOLE_DRIVER_H__ + +#include +#include +#include "AP_HAL_AVR_Namespace.h" + +class AP_HAL_AVR::AVRConsoleDriver : public AP_HAL::ConsoleDriver { +public: + AVRConsoleDriver(); + void init(void* baseuartdriver); + void backend_open(); + void backend_close(); + int backend_read(uint8_t *data, int len); + int backend_write(const uint8_t *data, int len); + + /* Implementations of BetterStream virtual methods */ + void print_P(const prog_char_t *s); + void println_P(const prog_char_t *s); + void printf(const char *s, ...) + __attribute__ ((format(__printf__, 2, 3))); + void _printf_P(const prog_char *s, ...) + __attribute__ ((format(__printf__, 2, 3))); + + /* Implementations of Stream virtual methods */ + int available(); + int txspace(); + int read(); + int peek(); + + /* Implementations of Print virtual methods */ + size_t write(uint8_t c); +private: + AP_HAL::UARTDriver* _base_uart; + bool _user_backend; +}; + +#endif // __AP_HAL_AVR_CONSOLE_DRIVER_H__ + diff --git a/libraries/AP_HAL_AVR/HAL_AVR.cpp b/libraries/AP_HAL_AVR/HAL_AVR.cpp index e0acf38ec8..0e965ff543 100644 --- a/libraries/AP_HAL_AVR/HAL_AVR.cpp +++ b/libraries/AP_HAL_AVR/HAL_AVR.cpp @@ -10,6 +10,7 @@ void HAL_AVR::init(void* opts) const { * it is initialized at boot */ // XXX maybe this should be 57600? uart0->begin(115200); + console->init((void*)uart0); /* The AVR RCInput drivers take an AP_HAL_AVR::ISRRegistry* * as the init argument */ rcin->init((void*)&isr_registry); diff --git a/libraries/AP_HAL_AVR/HAL_AVR.h b/libraries/AP_HAL_AVR/HAL_AVR.h index d20510cc82..7274decd6f 100644 --- a/libraries/AP_HAL_AVR/HAL_AVR.h +++ b/libraries/AP_HAL_AVR/HAL_AVR.h @@ -23,7 +23,7 @@ public: AP_HAL::AnalogIn* _analogin, AP_HAL::Storage* _storage, AP_HAL::Dataflash* _dataflash, - AP_HAL::BetterStream* _console, + AP_HAL::ConsoleDriver* _console, AP_HAL::GPIO* _gpio, AP_HAL::RCInput* _rcin, AP_HAL::RCOutput* _rcout, diff --git a/libraries/AP_HAL_AVR/examples/Console/Arduino.h b/libraries/AP_HAL_AVR/examples/Console/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_AVR/examples/Console/Console.pde b/libraries/AP_HAL_AVR/examples/Console/Console.pde new file mode 100644 index 0000000000..be994a6f96 --- /dev/null +++ b/libraries/AP_HAL_AVR/examples/Console/Console.pde @@ -0,0 +1,55 @@ +// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- + +// +// Example code for the AP_HAL AVRUARTDriver, based on FastSerial +// +// This code is placed into the public domain. +// + +#include +#include +#include + +const AP_HAL::HAL& hal = AP_HAL_AVR_APM1; + +void setup(void) +{ + // + // Set the speed for our replacement serial port. + // + hal.uart0->begin(115200); + // + // Test printing things + // + hal.console->print("test"); + hal.console->println(" begin"); + hal.console->println(1000); + hal.console->println(1000, 8); + hal.console->println(1000, 10); + hal.console->println(1000, 16); + hal.console->println_P(PSTR("progmem")); + hal.console->printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem")); + hal.console->printf_P(PSTR("printf_P %d %u %#x %p %f %S\n"), -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem")); + hal.console->println("done"); +} + +void loop(void) +{ + int c; + // + // Perform a simple loopback operation. + // + c = hal.console->read(); + if (-1 != c) + hal.console->write(c); +} + + +extern "C" { +int main (void) { + hal.init(NULL); + setup(); + for(;;) loop(); + return 0; +} +} diff --git a/libraries/AP_HAL_AVR/examples/Console/Makefile b/libraries/AP_HAL_AVR/examples/Console/Makefile new file mode 100644 index 0000000000..85b4d8856b --- /dev/null +++ b/libraries/AP_HAL_AVR/examples/Console/Makefile @@ -0,0 +1,2 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk diff --git a/libraries/AP_HAL_AVR/examples/Console/nocore.inoflag b/libraries/AP_HAL_AVR/examples/Console/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2