From cf0493543826a2b07b5b9bf77025b5b4e72e73f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Jan 2013 10:23:51 +1100 Subject: [PATCH] HAL_AVR: make Console a direct wrapper of uartA this saves a bunch of memory, and we don't really need separate console support on AVR --- libraries/AP_HAL_AVR/Console.cpp | 129 +++---------------------------- libraries/AP_HAL_AVR/Console.h | 21 +---- 2 files changed, 13 insertions(+), 137 deletions(-) diff --git a/libraries/AP_HAL_AVR/Console.cpp b/libraries/AP_HAL_AVR/Console.cpp index c588393e7b..0e1ac8b9f4 100644 --- a/libraries/AP_HAL_AVR/Console.cpp +++ b/libraries/AP_HAL_AVR/Console.cpp @@ -1,3 +1,5 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + #include #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 @@ -8,10 +10,6 @@ #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; @@ -19,49 +17,28 @@ void AVRConsoleDriver::init(void* base_uart) { void AVRConsoleDriver::backend_open() { - _txbuf.allocate(128); - _rxbuf.allocate(16); - _user_backend = true; } void AVRConsoleDriver::backend_close() { - _user_backend = false; } size_t AVRConsoleDriver::backend_read(uint8_t *data, size_t len) { - for (size_t i = 0; i < len; i++) { - int16_t b = _txbuf.pop(); - if (b != -1) { - data[i] = (uint8_t) b; - } else { - return i; - } - } - return len; + return 0; } size_t AVRConsoleDriver::backend_write(const uint8_t *data, size_t len) { - for (size_t i = 0; i < len; i++) { - bool valid = _rxbuf.push(data[i]); - if (!valid) { - return i; - } - } - return len; + return 0; } // AVRConsoleDriver private method implementations //////////////////////////// // 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); + _base_uart->print_P(s); } void AVRConsoleDriver::println_P(const prog_char_t *s) { - print_P(s); - println(); + _base_uart->println_P(s); } @@ -89,108 +66,26 @@ void AVRConsoleDriver::vprintf_P(const prog_char *fmt, va_list ap){ // Stream method implementations ///////////////////////////////////////// int16_t AVRConsoleDriver::available(void) { - if (_user_backend) { - return _rxbuf.bytes_used(); - } else { - return _base_uart->available(); - } + return _base_uart->available(); } int16_t AVRConsoleDriver::txspace(void) { - if (_user_backend) { - return _rxbuf.bytes_free(); - } else { - return _base_uart->txspace(); - } + return _base_uart->txspace(); } int16_t AVRConsoleDriver::read() { - if (_user_backend) { - return _rxbuf.pop(); - } else { - return _base_uart->read(); - } + return _base_uart->read(); } int16_t AVRConsoleDriver::peek() { - if (_user_backend) { - return _rxbuf.peek(); - } else { - return _base_uart->peek(); - } + 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); - } + return _base_uart->write(c); } +#endif // CONFIG_HAL_BOARD -/** - * AVRConsoleDriver::Buffer implementation. - * A synchronous nonblocking ring buffer, based on the AVRUARTDriver::Buffer - */ - -bool AVRConsoleDriver::Buffer::allocate(uint16_t size) { - _head = 0; - _tail = 0; - uint8_t shift; - /* Hardcoded max size of 1024. sue me. */ - for ( shift = 1; - ( 1U << shift ) < 1024 && ( 1U << 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; - return true; -} - -int16_t AVRConsoleDriver::Buffer::pop() { - if ( _tail == _head ) { - return -1; - } - uint8_t b = _bytes[_tail]; - _tail = ( _tail + 1 ) & _mask; - return (int16_t) b; -} - -int16_t AVRConsoleDriver::Buffer::peek() { - if ( _tail == _head ) { - return -1; - } - uint8_t b = _bytes[_tail]; - return (int16_t) b; -} - -uint16_t AVRConsoleDriver::Buffer::bytes_used() { - return ((_head - _tail) & _mask); -} - -uint16_t AVRConsoleDriver::Buffer::bytes_free() { - return ((_mask+1) - ((_head - _tail) & _mask)); -} - -#endif diff --git a/libraries/AP_HAL_AVR/Console.h b/libraries/AP_HAL_AVR/Console.h index fe5624ab01..dbd43d5959 100644 --- a/libraries/AP_HAL_AVR/Console.h +++ b/libraries/AP_HAL_AVR/Console.h @@ -9,7 +9,6 @@ class AP_HAL_AVR::AVRConsoleDriver : public AP_HAL::ConsoleDriver { public: - AVRConsoleDriver(); void init(void* baseuartdriver); void backend_open(); void backend_close(); @@ -35,27 +34,9 @@ public: /* Implementations of Print virtual methods */ size_t write(uint8_t c); + private: - struct Buffer { - /* public methods:*/ - bool allocate(uint16_t size); - bool push(uint8_t b); - int16_t pop(); - int16_t peek(); - - uint16_t bytes_free(); - uint16_t bytes_used(); - 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; }; #endif // __AP_HAL_AVR_CONSOLE_DRIVER_H__