From 7b720aae46d7643d243ada088a706da938f58c0f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 26 May 2018 15:21:24 +1000
Subject: [PATCH] HAL_ChibiOS: allow printf() to work on systems without debug
 console

map to hal.console once initialised
---
 libraries/AP_HAL_ChibiOS/UARTDriver.cpp       | 18 ++++++++++++++++++
 libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c |  5 ++++-
 libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h |  8 +++++++-
 3 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
index 69710b5eec..f7a6fd4924 100644
--- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
+++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
@@ -114,6 +114,17 @@ void UARTDriver::thread_init(void)
 }
 
 
+/*
+  hook to allow printf() to work on hal.console when we don't have a
+  dedicated debug console
+ */
+static int hal_console_vprintf(const char *fmt, va_list arg)
+{
+    hal.console->vprintf(fmt, arg);
+    return 1; // wrong length, but doesn't matter for what this is used for
+}
+
+
 void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 {
     thread_init();
@@ -261,6 +272,13 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 
     // setup flow control
     set_flow_control(_flow_control);
+
+    if (serial_num == 0 && _initialised) {
+#ifndef HAL_STDOUT_SERIAL
+        // setup hal.console to take printf() output
+        vprintf_console_hook = hal_console_vprintf;
+#endif
+    }
 }
 
 void UARTDriver::dma_tx_allocate(Shared_DMA *ctx)
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c
index d47945c06c..88e5ae7a16 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c
@@ -107,13 +107,16 @@ int vprintf(const char *fmt, va_list arg)
 #endif
 }
 
+// hook to allow for printf() on systems without HAL_STDOUT_SERIAL
+int (*vprintf_console_hook)(const char *fmt, va_list arg) = vprintf;
+
 int printf(const char *fmt, ...)
 {
    va_list arg;
    int done;
  
    va_start (arg, fmt);
-   done =  vprintf(fmt, arg);
+   done =  vprintf_console_hook(fmt, arg);
    va_end (arg);
  
    return done;
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h
index 5664d03fcd..a13f59b449 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h
@@ -13,6 +13,9 @@
  * You should have received a copy of the GNU General Public License along
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
+
+#pragma once
+
 #include "posix.h"
 #include <stdarg.h>
 #include <stdint.h>
@@ -37,6 +40,9 @@ int vsscanf (const char *buf, const char *s, va_list ap);
 void *malloc(size_t size);
 void *calloc(size_t nmemb, size_t size);
 void free(void *ptr);
+
+extern int (*vprintf_console_hook)(const char *fmt, va_list arg);
+
 #ifdef __cplusplus
 }
-#endif
\ No newline at end of file
+#endif