mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
parent
ea3b405959
commit
cf04935438
|
@ -1,3 +1,5 @@
|
||||||
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
#include <AP_HAL.h>
|
#include <AP_HAL.h>
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||||
|
|
||||||
|
@ -8,10 +10,6 @@
|
||||||
#include "Console.h"
|
#include "Console.h"
|
||||||
using namespace AP_HAL_AVR;
|
using namespace AP_HAL_AVR;
|
||||||
|
|
||||||
AVRConsoleDriver::AVRConsoleDriver() :
|
|
||||||
_user_backend(false)
|
|
||||||
{}
|
|
||||||
|
|
||||||
// ConsoleDriver method implementations ///////////////////////////////////////
|
// ConsoleDriver method implementations ///////////////////////////////////////
|
||||||
void AVRConsoleDriver::init(void* base_uart) {
|
void AVRConsoleDriver::init(void* base_uart) {
|
||||||
_base_uart = (AP_HAL::UARTDriver*) base_uart;
|
_base_uart = (AP_HAL::UARTDriver*) base_uart;
|
||||||
|
@ -19,49 +17,28 @@ void AVRConsoleDriver::init(void* base_uart) {
|
||||||
|
|
||||||
|
|
||||||
void AVRConsoleDriver::backend_open() {
|
void AVRConsoleDriver::backend_open() {
|
||||||
_txbuf.allocate(128);
|
|
||||||
_rxbuf.allocate(16);
|
|
||||||
_user_backend = true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void AVRConsoleDriver::backend_close() {
|
void AVRConsoleDriver::backend_close() {
|
||||||
_user_backend = false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t AVRConsoleDriver::backend_read(uint8_t *data, size_t len) {
|
size_t AVRConsoleDriver::backend_read(uint8_t *data, size_t len) {
|
||||||
for (size_t i = 0; i < len; i++) {
|
return 0;
|
||||||
int16_t b = _txbuf.pop();
|
|
||||||
if (b != -1) {
|
|
||||||
data[i] = (uint8_t) b;
|
|
||||||
} else {
|
|
||||||
return i;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return len;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t AVRConsoleDriver::backend_write(const uint8_t *data, size_t len) {
|
size_t AVRConsoleDriver::backend_write(const uint8_t *data, size_t len) {
|
||||||
for (size_t i = 0; i < len; i++) {
|
return 0;
|
||||||
bool valid = _rxbuf.push(data[i]);
|
|
||||||
if (!valid) {
|
|
||||||
return i;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return len;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// AVRConsoleDriver private method implementations ////////////////////////////
|
// AVRConsoleDriver private method implementations ////////////////////////////
|
||||||
|
|
||||||
// BetterStream method implementations /////////////////////////////////////////
|
// BetterStream method implementations /////////////////////////////////////////
|
||||||
void AVRConsoleDriver::print_P(const prog_char_t *s) {
|
void AVRConsoleDriver::print_P(const prog_char_t *s) {
|
||||||
char c;
|
_base_uart->print_P(s);
|
||||||
while ('\0' != (c = pgm_read_byte((const prog_char *)s++)))
|
|
||||||
write(c);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void AVRConsoleDriver::println_P(const prog_char_t *s) {
|
void AVRConsoleDriver::println_P(const prog_char_t *s) {
|
||||||
print_P(s);
|
_base_uart->println_P(s);
|
||||||
println();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -89,108 +66,26 @@ void AVRConsoleDriver::vprintf_P(const prog_char *fmt, va_list ap){
|
||||||
|
|
||||||
// Stream method implementations /////////////////////////////////////////
|
// Stream method implementations /////////////////////////////////////////
|
||||||
int16_t AVRConsoleDriver::available(void) {
|
int16_t AVRConsoleDriver::available(void) {
|
||||||
if (_user_backend) {
|
return _base_uart->available();
|
||||||
return _rxbuf.bytes_used();
|
|
||||||
} else {
|
|
||||||
return _base_uart->available();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int16_t AVRConsoleDriver::txspace(void) {
|
int16_t AVRConsoleDriver::txspace(void) {
|
||||||
if (_user_backend) {
|
return _base_uart->txspace();
|
||||||
return _rxbuf.bytes_free();
|
|
||||||
} else {
|
|
||||||
return _base_uart->txspace();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int16_t AVRConsoleDriver::read() {
|
int16_t AVRConsoleDriver::read() {
|
||||||
if (_user_backend) {
|
return _base_uart->read();
|
||||||
return _rxbuf.pop();
|
|
||||||
} else {
|
|
||||||
return _base_uart->read();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int16_t AVRConsoleDriver::peek() {
|
int16_t AVRConsoleDriver::peek() {
|
||||||
if (_user_backend) {
|
return _base_uart->peek();
|
||||||
return _rxbuf.peek();
|
|
||||||
} else {
|
|
||||||
return _base_uart->peek();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Print method implementations /////////////////////////////////////////
|
// Print method implementations /////////////////////////////////////////
|
||||||
|
|
||||||
size_t AVRConsoleDriver::write(uint8_t c) {
|
size_t AVRConsoleDriver::write(uint8_t c) {
|
||||||
if (_user_backend) {
|
return _base_uart->write(c);
|
||||||
return (_txbuf.push(c) ? 1 : 0);
|
|
||||||
} else {
|
|
||||||
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
|
|
||||||
|
|
|
@ -9,7 +9,6 @@
|
||||||
|
|
||||||
class AP_HAL_AVR::AVRConsoleDriver : public AP_HAL::ConsoleDriver {
|
class AP_HAL_AVR::AVRConsoleDriver : public AP_HAL::ConsoleDriver {
|
||||||
public:
|
public:
|
||||||
AVRConsoleDriver();
|
|
||||||
void init(void* baseuartdriver);
|
void init(void* baseuartdriver);
|
||||||
void backend_open();
|
void backend_open();
|
||||||
void backend_close();
|
void backend_close();
|
||||||
|
@ -35,27 +34,9 @@ public:
|
||||||
|
|
||||||
/* Implementations of Print virtual methods */
|
/* Implementations of Print virtual methods */
|
||||||
size_t write(uint8_t c);
|
size_t write(uint8_t c);
|
||||||
|
|
||||||
private:
|
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;
|
AP_HAL::UARTDriver* _base_uart;
|
||||||
bool _user_backend;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // __AP_HAL_AVR_CONSOLE_DRIVER_H__
|
#endif // __AP_HAL_AVR_CONSOLE_DRIVER_H__
|
||||||
|
|
Loading…
Reference in New Issue