AP_HAL: implement betterstream functions in AP_HAL

this gives more consistancy between ports

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL.h>
+#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); 
+}

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__

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL.h>
+
+#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);
+}

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__