HAL_PX4: use common printf implementation on UARTs

libraries/AP_HAL_PX4/UARTDriver.cpp

@@ -16,6 +16,7 @@
 #include <termios.h>
 #include <drivers/drv_hrt.h>
 #include <assert.h>
+#include "utility/print_vprintf.h"
 
 using namespace PX4;
 
@@ -138,67 +139,28 @@ void PX4UARTDriver::println_P(const prog_char_t *pstr) {
 	println(pstr);
 }
 
-void PX4UARTDriver::printf(const char *fmt, ...) {
+void PX4UARTDriver::printf(const char *fmt, ...) 
+{
     va_list ap;
     va_start(ap, fmt);
-    _vprintf(fmt, ap);
-    va_end(ap);	
-}
-
-void PX4UARTDriver::_printf_P(const prog_char *fmt, ...) {
-    va_list ap;
-    va_start(ap, fmt);
-    _vprintf(fmt, ap);
-    va_end(ap);	
+    vprintf(fmt, ap);
+    va_end(ap);
 }
 
 void PX4UARTDriver::vprintf(const char *fmt, va_list ap) {
-    _vprintf(fmt, ap);
+    print_vprintf((AP_HAL::Print*)this, 0, fmt, ap);
+}
+
+void PX4UARTDriver::_printf_P(const prog_char *fmt, ...) 
+{
+    va_list ap;
+    va_start(ap, fmt);
+    vprintf_P(fmt, ap);
+    va_end(ap);
 }
 
 void PX4UARTDriver::vprintf_P(const prog_char *fmt, va_list ap) {
-    _vprintf(fmt, ap);
-}
-
-
-void PX4UARTDriver::_internal_vprintf(const char *fmt, va_list ap)
-{
-    if (hal.scheduler->in_timerprocess()) {
-        // not allowed from timers
-        return;
-    }
-    char *buf = NULL;
-    int n = avsprintf(&buf, fmt, ap);
-    if (n > 0) {
-        write((const uint8_t *)buf, n);
-    }
-    if (buf != NULL) {
-        free(buf);    
-    }
-}
-
-// handle %S -> %s
-void PX4UARTDriver::_vprintf(const char *fmt, va_list ap)
-{
-    if (hal.scheduler->in_timerprocess()) {
-        // not allowed from timers
-        return;
-    }
-    // we don't use vdprintf() as it goes directly to the file descriptor
-	if (strstr(fmt, "%S")) {
-		char *fmt2 = strdup(fmt);
-		if (fmt2 != NULL) {
-			for (uint16_t i=0; fmt2[i]; i++) {
-				if (fmt2[i] == '%' && fmt2[i+1] == 'S') {
-					fmt2[i+1] = 's';
-				}
-			}
-            _internal_vprintf(fmt2, ap);
-			free(fmt2);
-		}
-	} else {
-        _internal_vprintf(fmt, ap);
-	}	
+    print_vprintf((AP_HAL::Print*)this, 1, fmt, ap);
 }
 
 

libraries/AP_HAL_PX4/UARTDriver.h

@@ -51,8 +51,6 @@ public:
 private:
     const char *_devpath;
     int _fd;
-    void _vprintf(const char *fmt, va_list ap);
-    void _internal_vprintf(const char *fmt, va_list ap);
 
     bool _nonblocking_writes;