From 4bc53acbdac786e311a0eb7aebbe15f4a7a7405e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 8 May 2013 16:17:36 +1000
Subject: [PATCH] SITL: support vsnprintf with %S on SITL

---
 libraries/AP_HAL_AVR_SITL/Util.cpp            |  7 +++--
 .../AP_HAL_AVR_SITL/utility/print_vprintf.cpp | 27 +++++++++++++++++++
 .../AP_HAL_AVR_SITL/utility/print_vprintf.h   |  1 +
 3 files changed, 33 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_AVR_SITL/Util.cpp b/libraries/AP_HAL_AVR_SITL/Util.cpp
index 2bcf872761..0bf8d4a13f 100644
--- a/libraries/AP_HAL_AVR_SITL/Util.cpp
+++ b/libraries/AP_HAL_AVR_SITL/Util.cpp
@@ -1,11 +1,14 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 #include <AP_HAL.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 #include <stdio.h>
 #include <stdarg.h>
+#include "utility/print_vprintf.h"
 
-int libc_vsnprintf(char* str, size_t size, const char *format, va_list ap) {
-    return vsnprintf(str, size, format, ap);
+int libc_vsnprintf(char* str, size_t size, const char *format, va_list ap) 
+{
+    return print_vsnprintf(str, size, format, ap);
 }
 
 #include "Util.h"
diff --git a/libraries/AP_HAL_AVR_SITL/utility/print_vprintf.cpp b/libraries/AP_HAL_AVR_SITL/utility/print_vprintf.cpp
index 3b2d2515bc..51b735e19e 100644
--- a/libraries/AP_HAL_AVR_SITL/utility/print_vprintf.cpp
+++ b/libraries/AP_HAL_AVR_SITL/utility/print_vprintf.cpp
@@ -18,6 +18,9 @@
 #include <stdio.h>
 #include <stdlib.h>
 
+/*
+  implement vprintf with support for %S meaning a progmem string
+ */
 void print_vprintf(AP_HAL::Print *s, unsigned char in_progmem, const char *fmt, va_list ap)
 {
         char *str = NULL;
@@ -36,4 +39,28 @@ void print_vprintf(AP_HAL::Print *s, unsigned char in_progmem, const char *fmt,
         free(str);
         free(fmt2);
 }
+
+
+/*
+  implement vsnprintf with support for %S meaning a progmem string
+ */
+int print_vsnprintf(char *s, size_t n, const char *fmt, va_list ap)
+{
+    int i, ret;
+    char *fmt2 = (char *)fmt;
+    if (strstr(fmt2, "%S") != NULL) {
+        fmt2 = strdup(fmt);
+        for (i=0; fmt2[i]; i++) {
+            // cope with %S
+            if (fmt2[i] == '%' && fmt2[i+1] == 'S') {
+                fmt2[i+1] = 's';
+            }
+        }
+    }
+    ret = vsnprintf(s, n, fmt2, ap);
+    if (fmt2 != fmt) {
+        free(fmt2);
+    }
+    return ret;
+}
 #endif
diff --git a/libraries/AP_HAL_AVR_SITL/utility/print_vprintf.h b/libraries/AP_HAL_AVR_SITL/utility/print_vprintf.h
index 0344abaea6..8e0f9b4c01 100644
--- a/libraries/AP_HAL_AVR_SITL/utility/print_vprintf.h
+++ b/libraries/AP_HAL_AVR_SITL/utility/print_vprintf.h
@@ -6,6 +6,7 @@
 #include <stdarg.h>
 
 void print_vprintf (AP_HAL::Print *s, unsigned char in_progmem, const char *fmt, va_list ap);
+int print_vsnprintf(char *s, size_t n, const char *fmt, va_list ap);
 
 
 #endif //__AP_HAL_AVR_SITL_UTILITY_VPRINTF_H__