diff --git a/libraries/AP_HAL/Console.cpp b/libraries/AP_HAL/Console.cpp new file mode 100644 index 0000000000..2ee509ebaa --- /dev/null +++ b/libraries/AP_HAL/Console.cpp @@ -0,0 +1,61 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#include "utility/print_vprintf.h" +#include "Console.h" + +/* + BetterStream method implementations + These are implemented in AP_HAL to ensure consistent behaviour on + all boards, although they can be overridden by a port + */ +void AP_HAL::ConsoleDriver::print_P(const prog_char_t *s) +{ + char c; + while ('\0' != (c = pgm_read_byte((const prog_char *)s++))) + write(c); +} + +void AP_HAL::ConsoleDriver::println_P(const prog_char_t *s) +{ + print_P(s); + println(); +} + +void AP_HAL::ConsoleDriver::printf(const char *fmt, ...) { + va_list ap; + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); +} + +void AP_HAL::ConsoleDriver::_printf_P(const prog_char *fmt, ...) { + va_list ap; + va_start(ap, fmt); + vprintf_P(fmt, ap); + va_end(ap); +} + +void AP_HAL::ConsoleDriver::vprintf(const char *fmt, va_list ap) +{ + print_vprintf((AP_HAL::Print*)this, 0, fmt, ap); +} + +void AP_HAL::ConsoleDriver::vprintf_P(const prog_char *fmt, va_list ap) +{ + print_vprintf((AP_HAL::Print*)this, 1, fmt, ap); +} diff --git a/libraries/AP_HAL/Console.h b/libraries/AP_HAL/Console.h index 3645de3af6..745333584d 100644 --- a/libraries/AP_HAL/Console.h +++ b/libraries/AP_HAL/Console.h @@ -11,6 +11,21 @@ public: virtual void backend_close() = 0; virtual size_t backend_read(uint8_t *data, size_t len) = 0; virtual size_t backend_write(const uint8_t *data, size_t len) = 0; + + + /* Implementations of BetterStream virtual methods. These are + * provided by AP_HAL to ensure consistency between ports to + * different boards + */ + 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))); + + void vprintf(const char *s, va_list ap); + void vprintf_P(const prog_char *s, va_list ap); }; #endif // __AP_HAL_CONSOLE_DRIVER_H__ diff --git a/libraries/AP_HAL/UARTDriver.cpp b/libraries/AP_HAL/UARTDriver.cpp new file mode 100644 index 0000000000..d121d56284 --- /dev/null +++ b/libraries/AP_HAL/UARTDriver.cpp @@ -0,0 +1,65 @@ +// -*- Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include + +#include "utility/print_vprintf.h" +#include "UARTDriver.h" + +/* + BetterStream method implementations + These are implemented in AP_HAL to ensure consistent behaviour on + all boards, although they can be overridden by a port + */ + +void AP_HAL::UARTDriver::print_P(const prog_char_t *s) +{ + char c; + while ('\0' != (c = pgm_read_byte((const prog_char *)s++))) + write(c); +} + +void AP_HAL::UARTDriver::println_P(const prog_char_t *s) +{ + print_P(s); + println(); +} + +void AP_HAL::UARTDriver::printf(const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); +} + +void AP_HAL::UARTDriver::vprintf(const char *fmt, va_list ap) +{ + print_vprintf((AP_HAL::Print*)this, 0, fmt, ap); +} + +void AP_HAL::UARTDriver::_printf_P(const prog_char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + vprintf_P(fmt, ap); + va_end(ap); +} + +void AP_HAL::UARTDriver::vprintf_P(const prog_char *fmt, va_list ap) +{ + print_vprintf((AP_HAL::Print*)this, 1, fmt, ap); +} diff --git a/libraries/AP_HAL/UARTDriver.h b/libraries/AP_HAL/UARTDriver.h index 36d7dbee83..429f8900c8 100644 --- a/libraries/AP_HAL/UARTDriver.h +++ b/libraries/AP_HAL/UARTDriver.h @@ -38,6 +38,20 @@ public: virtual bool is_initialized() = 0; virtual void set_blocking_writes(bool blocking) = 0; virtual bool tx_pending() = 0; + + /* Implementations of BetterStream virtual methods. These are + * provided by AP_HAL to ensure consistency between ports to + * different boards + */ + 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))); + + void vprintf(const char *s, va_list ap); + void vprintf_P(const prog_char *s, va_list ap); }; #endif // __AP_HAL_UART_DRIVER_H__