SITL: support vsnprintf with %S on SITL

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"

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

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__