diff --git a/libraries/AP_HAL_AVR/Console.cpp b/libraries/AP_HAL_AVR/Console.cpp index b6479d91a8..4416ef8502 100644 --- a/libraries/AP_HAL_AVR/Console.cpp +++ b/libraries/AP_HAL_AVR/Console.cpp @@ -12,13 +12,14 @@ AVRConsoleDriver::AVRConsoleDriver() : {} // ConsoleDriver method implementations /////////////////////////////////////// -// void AVRConsoleDriver::init(void* base_uart) { _base_uart = (AP_HAL::UARTDriver*) base_uart; } void AVRConsoleDriver::backend_open() { + _txbuf.allocate(128); + _rxbuf.allocate(16); _user_backend = true; } @@ -27,22 +28,29 @@ void AVRConsoleDriver::backend_close() { } int AVRConsoleDriver::backend_read(uint8_t *data, int len) { - return 0; + for (int i = 0; i < len; i++) { + int b = _txbuf.pop(); + if (b != -1) { + data[i] = (uint8_t) b; + } else { + return i; + } + } + return len; } 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); + for (int i = 0; i < len; i++) { + bool valid = _rxbuf.push(data[i]); + if (!valid) { + return i; + } } + return len; } +// AVRConsoleDriver private method implementations //////////////////////////// + // BetterStream method implementations ///////////////////////////////////////// void AVRConsoleDriver::print_P(const prog_char_t *s) { char c; @@ -72,6 +80,7 @@ void AVRConsoleDriver::_printf_P(const prog_char *fmt, ...) { // Stream method implementations ///////////////////////////////////////// int AVRConsoleDriver::available(void) { if (_user_backend) { + // XXX TODO return INT_MAX; } else { return _base_uart->available(); @@ -80,6 +89,7 @@ int AVRConsoleDriver::available(void) { int AVRConsoleDriver::txspace(void) { if (_user_backend) { + // XXX TODO return INT_MAX; } else { return _base_uart->txspace(); @@ -88,7 +98,7 @@ int AVRConsoleDriver::txspace(void) { int AVRConsoleDriver::read() { if (_user_backend) { - return -1; + return _rxbuf.pop(); } else { return _base_uart->read(); } @@ -96,9 +106,66 @@ int AVRConsoleDriver::read() { int AVRConsoleDriver::peek() { if (_user_backend) { + // XXX TODO return -1; } else { return _base_uart->peek(); } } +// Print method implementations ///////////////////////////////////////// + +size_t AVRConsoleDriver::write(uint8_t c) { + if (_user_backend) { + return (_txbuf.push(c) ? 1 : 0); + } else { + return _base_uart->write(c); + } +} + + +/** + * AVRConsoleDriver::Buffer implementation. + * A synchronous nonblocking ring buffer, based on the AVRUARTDriver::Buffer + */ + +bool AVRConsoleDriver::Buffer::allocate(int size) { + _head = 0; + _tail = 0; + uint8_t shift; + /* Hardcoded max size of 1024. sue me. */ + for ( shift = 1; + ( 1 << shift ) < 1024 && ( 1 << shift) < size; + shift++ + ) ; + uint16_t tmpmask = (1 << shift) - 1; + + if ( _bytes != NULL ) { + if ( _mask == tmpmask ) { + return true; + } + free(_bytes); + } + _mask = tmpmask; + _bytes = (uint8_t*) malloc(_mask+1); + return (_bytes != NULL); +} + +bool AVRConsoleDriver::Buffer::push(uint8_t b) { + uint16_t next = (_head + 1) & _mask; + if ( next == _tail ) { + return false; + } + _bytes[_head] = b; + _head = next; +} + +int AVRConsoleDriver::Buffer::pop() { + if ( _tail == _head ) { + return -1; + } + uint8_t b = _bytes[_tail]; + _tail = ( _tail + 1 ) & _mask; + return (int) b; +} + diff --git a/libraries/AP_HAL_AVR/Console.h b/libraries/AP_HAL_AVR/Console.h index 2d74302dcf..8dbf34170f 100644 --- a/libraries/AP_HAL_AVR/Console.h +++ b/libraries/AP_HAL_AVR/Console.h @@ -2,6 +2,8 @@ #ifndef __AP_HAL_AVR_CONSOLE_DRIVER_H__ #define __AP_HAL_AVR_CONSOLE_DRIVER_H__ +#include + #include #include #include "AP_HAL_AVR_Namespace.h" @@ -32,6 +34,20 @@ public: /* Implementations of Print virtual methods */ size_t write(uint8_t c); private: + struct Buffer { + /* public methods:*/ + bool allocate(int size); + bool push(uint8_t b); + int pop(); + private: + uint16_t _head, _tail; /* Head and tail indicies */ + uint16_t _mask; /* Buffer size mask for index wrap */ + uint8_t *_bytes; /* Pointer to allocated buffer */ + }; + + Buffer _txbuf; + Buffer _rxbuf; + AP_HAL::UARTDriver* _base_uart; bool _user_backend; }; diff --git a/libraries/AP_HAL_AVR/examples/Console/Console.pde b/libraries/AP_HAL_AVR/examples/Console/Console.pde index be994a6f96..0712f7f7c3 100644 --- a/libraries/AP_HAL_AVR/examples/Console/Console.pde +++ b/libraries/AP_HAL_AVR/examples/Console/Console.pde @@ -12,6 +12,62 @@ const AP_HAL::HAL& hal = AP_HAL_AVR_APM1; + +void stream_loopback(AP_HAL::Stream* s, uint32_t time) { + uint32_t end = hal.scheduler->millis() + time; + for(;;) { + if (hal.scheduler->millis() >= end && time != 0) { + return; + } + if (s->available() > 0) { + int c; + c = s->read(); + if (-1 != c) { + s->write((uint8_t)c); + } + } + } +} + +void stream_console_loopback(AP_HAL::Stream* s, AP_HAL::ConsoleDriver* c, + uint32_t time) { + uint32_t end = hal.scheduler->millis() + time; + for(;;) { + if (hal.scheduler->millis() >= end && time != 0) { + return; + } + + /* Read the uart, write to the console backend. */ + if (s->available() > 0) { + int b; + b = s->read(); + if (-1 != b) { + uint8_t tmp[1]; + tmp[0] = (uint8_t) b; + c->backend_write(tmp, 1); + } + } + /* Loop back the console upon itself. */ + { + int b; + b = c->read(); + if (-1 != b) { + c->write((uint8_t)b); + } + + } + + /* Read the console backend, print to the uart. */ + { + uint8_t tmp[1]; + int readback = c->backend_read(tmp, 1); + if (readback > 0) { + s->write(tmp[0]); + } + + } + } +} void setup(void) { // @@ -30,21 +86,33 @@ void setup(void) 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); + hal.console->println("loopback for 10sec:"); + stream_loopback(hal.console, 10000); + hal.console->println("loopback done"); + + hal.console->println("opening backend:"); + + hal.console->backend_open(); + + const char hello[] = "hello world\r\n"; + hal.console->backend_write((const uint8_t*)hello, strlen(hello)); + + hal.console->println("loopback for 10sec:"); + stream_console_loopback(hal.uart0, hal.console, 10000); + hal.console->println("loopback done"); + + hal.console->backend_close(); + hal.console->println("closed backend."); + + hal.console->println("done."); + for(;;); + } +void loop(void){} + extern "C" { int main (void) { hal.init(NULL);