From dbb70fc57c67e946af238381dc152023d5e58b03 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 22 Sep 2013 12:21:22 +1000 Subject: [PATCH] AP_HAL_AVR_SITL: implement betterstream functions in AP_HAL this gives more consistancy between ports --- libraries/AP_HAL_AVR_SITL/Console.cpp | 43 ------------------------ libraries/AP_HAL_AVR_SITL/Console.h | 11 ------ libraries/AP_HAL_AVR_SITL/UARTDriver.cpp | 38 --------------------- libraries/AP_HAL_AVR_SITL/UARTDriver.h | 10 ------ 4 files changed, 102 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/Console.cpp b/libraries/AP_HAL_AVR_SITL/Console.cpp index b59702292d..2e369c5905 100644 --- a/libraries/AP_HAL_AVR_SITL/Console.cpp +++ b/libraries/AP_HAL_AVR_SITL/Console.cpp @@ -5,7 +5,6 @@ #include #include -#include "utility/print_vprintf.h" #include "Console.h" using namespace AVR_SITL; @@ -37,48 +36,6 @@ size_t SITLConsoleDriver::backend_write(const uint8_t *data, size_t len) return 0; } -// SITLConsoleDriver private method implementations //////////////////////////// - -// BetterStream method implementations ///////////////////////////////////////// -void SITLConsoleDriver::print_P(const prog_char_t *s) -{ - char c; - while ('\0' != (c = pgm_read_byte((const prog_char *)s++))) - write(c); -} - -void SITLConsoleDriver::println_P(const prog_char_t *s) -{ - print_P(s); - println(); -} - -void SITLConsoleDriver::printf(const char *fmt, ...) -{ - va_list ap; - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); -} - -void SITLConsoleDriver::vprintf(const char *fmt, va_list ap) -{ - print_vprintf((AP_HAL::Print*)this, 0, fmt, ap); -} - -void SITLConsoleDriver::_printf_P(const prog_char *fmt, ...) -{ - va_list ap; - va_start(ap, fmt); - vprintf_P(fmt, ap); - va_end(ap); -} - -void SITLConsoleDriver::vprintf_P(const prog_char *fmt, va_list ap) -{ - print_vprintf((AP_HAL::Print*)this, 1, fmt, ap); -} - // Stream method implementations ///////////////////////////////////////// int16_t SITLConsoleDriver::available(void) { diff --git a/libraries/AP_HAL_AVR_SITL/Console.h b/libraries/AP_HAL_AVR_SITL/Console.h index aeb0b74439..42c278df99 100644 --- a/libraries/AP_HAL_AVR_SITL/Console.h +++ b/libraries/AP_HAL_AVR_SITL/Console.h @@ -17,17 +17,6 @@ public: size_t backend_read(uint8_t *data, size_t len); size_t backend_write(const uint8_t *data, size_t 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))); - - void vprintf(const char *s, va_list ap); - void vprintf_P(const prog_char *s, va_list ap); - /* Implementations of Stream virtual methods */ int16_t available(); int16_t txspace(); diff --git a/libraries/AP_HAL_AVR_SITL/UARTDriver.cpp b/libraries/AP_HAL_AVR_SITL/UARTDriver.cpp index ac150bad24..9f79afe303 100644 --- a/libraries/AP_HAL_AVR_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_AVR_SITL/UARTDriver.cpp @@ -35,7 +35,6 @@ #include #include -#include "utility/print_vprintf.h" #include "UARTDriver.h" #include "SITL_State.h" @@ -175,43 +174,6 @@ size_t SITLUARTDriver::write(uint8_t c) return send(_fd, &c, 1, flags); } -// BetterStream method implementations ///////////////////////////////////////// -void SITLUARTDriver::print_P(const prog_char_t *s) -{ - char c; - while ('\0' != (c = pgm_read_byte((const prog_char *)s++))) - write(c); -} - -void SITLUARTDriver::println_P(const prog_char_t *s) -{ - print_P(s); - println(); -} - -void SITLUARTDriver::printf(const char *fmt, ...) -{ - va_list ap; - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); -} - -void SITLUARTDriver::vprintf(const char *fmt, va_list ap) { - print_vprintf((AP_HAL::Print*)this, 0, fmt, ap); -} - -void SITLUARTDriver::_printf_P(const prog_char *fmt, ...) -{ - va_list ap; - va_start(ap, fmt); - vprintf_P(fmt, ap); - va_end(ap); -} - -void SITLUARTDriver::vprintf_P(const prog_char *fmt, va_list ap) { - print_vprintf((AP_HAL::Print*)this, 1, fmt, ap); -} /* start a TCP connection for the serial port. If wait_for_connection is true then block until a client connects diff --git a/libraries/AP_HAL_AVR_SITL/UARTDriver.h b/libraries/AP_HAL_AVR_SITL/UARTDriver.h index 13e37d99af..861a77b213 100644 --- a/libraries/AP_HAL_AVR_SITL/UARTDriver.h +++ b/libraries/AP_HAL_AVR_SITL/UARTDriver.h @@ -39,16 +39,6 @@ public: return false; } - /* 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))); - void vprintf(const char *s, va_list ap); - void vprintf_P(const prog_char *s, va_list ap); - /* Implementations of Stream virtual methods */ int16_t available(); int16_t txspace();