HAL_PX4: added Util instance

this fixes hal.util->vsnprintf_P()

libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h

@@ -11,6 +11,7 @@ namespace PX4 {
 	class PX4RCOutput;
 	class PX4AnalogIn;
 	class PX4AnalogSource;
+	class PX4Util;
 }
 
 #endif //__AP_HAL_PX4_NAMESPACE_H__

libraries/AP_HAL_PX4/HAL_PX4_Class.cpp

@@ -14,6 +14,7 @@
 #include "RCInput.h"
 #include "RCOutput.h"
 #include "AnalogIn.h"
+#include "Util.h"
 
 #include <AP_HAL_Empty.h>
 #include <AP_HAL_Empty_Private.h>
@@ -31,7 +32,6 @@ static Empty::EmptySemaphore  i2cSemaphore;
 static Empty::EmptyI2CDriver  i2cDriver(&i2cSemaphore);
 static Empty::EmptySPIDeviceManager spiDeviceManager;
 static Empty::EmptyGPIO gpioDriver;
-static Empty::EmptyUtil utilInstance;
 
 static PX4ConsoleDriver consoleDriver;
 static PX4Scheduler schedulerInstance;
@@ -39,6 +39,7 @@ static PX4EEPROMStorage storageDriver;
 static PX4RCInput rcinDriver;
 static PX4RCOutput rcoutDriver;
 static PX4AnalogIn analogIn;
+static PX4Util utilInstance;
 
 #define UARTA_DEFAULT_DEVICE "/dev/ttyS0"
 #define UARTB_DEFAULT_DEVICE "/dev/ttyS3"

libraries/AP_HAL_PX4/Util.cpp

@@ -0,0 +1,45 @@
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#include <stdio.h>
+#include <stdarg.h>
+
+static int libc_vsnprintf(char* str, size_t size, const char *format, va_list ap) 
+{
+    return vsnprintf(str, size, format, ap);
+}
+
+#include "Util.h"
+using namespace PX4;
+
+int PX4Util::snprintf(char* str, size_t size, const char *format, ...)
+{
+    va_list ap;
+    va_start(ap, format);
+    int res = libc_vsnprintf(str, size, format, ap);
+    va_end(ap);
+    return res;
+}
+
+int PX4Util::snprintf_P(char* str, size_t size, const prog_char_t *format, ...)
+{
+    va_list ap;
+    va_start(ap, format);
+    int res = libc_vsnprintf(str, size, format, ap);
+    va_end(ap);
+    return res;
+}
+
+
+int PX4Util::vsnprintf(char* str, size_t size, const char *format, va_list ap)
+{
+    return libc_vsnprintf(str, size, format, ap);
+}
+
+int PX4Util::vsnprintf_P(char* str, size_t size, const prog_char_t *format,
+            va_list ap)
+{
+    return libc_vsnprintf(str, size, format, ap);
+}
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

libraries/AP_HAL_PX4/Util.h

@@ -0,0 +1,17 @@
+
+#ifndef __AP_HAL_PX4_UTIL_H__
+#define __AP_HAL_PX4_UTIL_H__
+
+#include <AP_HAL.h>
+#include "AP_HAL_PX4_Namespace.h"
+
+class PX4::PX4Util : public AP_HAL::Util {
+public:
+    int snprintf(char* str, size_t size, const char *format, ...);
+    int snprintf_P(char* str, size_t size, const prog_char_t *format, ...);
+    int vsnprintf(char* str, size_t size, const char *format, va_list ap);
+    int vsnprintf_P(char* str, size_t size, const prog_char_t *format,
+            va_list ap);
+};
+
+#endif // __AP_HAL_PX4_UTIL_H__