Merge pull request #624 from ledvinap/improvement-blackbox

Simplify Blackbox code by improving printf() and using that instead of longer more specialised code

src/main/blackbox/blackbox.c

@@ -839,22 +839,11 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he
 
                 // Do we need to print an index in brackets after the name?
                 if (def->fieldNameIndex != -1) {
-                    blackboxWrite('[');
+                    charsWritten += blackboxPrintf("[%d]", def->fieldNameIndex);
-                    // Assume the field index is a single digit:
-                    blackboxWrite(def->fieldNameIndex + '0');
-                    blackboxWrite(']');
-                    charsWritten += 3;
                 }
             } else {
                 //The other headers are integers
-                if (def->arr[xmitState.headerIndex - 1] >= 10) {
+                charsWritten += blackboxPrintf("%d", def->arr[xmitState.headerIndex - 1]);
-                    blackboxWrite(def->arr[xmitState.headerIndex - 1] / 10 + '0');
-                    blackboxWrite(def->arr[xmitState.headerIndex - 1] % 10 + '0');
-                    charsWritten += 2;
-                } else {
-                    blackboxWrite(def->arr[xmitState.headerIndex - 1] + '0');
-                    charsWritten++;
-                }
             }
         }
     }

src/main/blackbox/blackbox_io.c

@@ -91,12 +91,13 @@ static void _putc(void *p, char c)
 }
 
 //printf() to the blackbox serial port with no blocking shenanigans (so it's caller's responsibility to not write too fast!)
-void blackboxPrintf(char *fmt, ...)
+int blackboxPrintf(const char *fmt, ...)
 {
     va_list va;
     va_start(va, fmt);
-    tfp_format(NULL, _putc, fmt, va);
+    int written = tfp_format(NULL, _putc, fmt, va);
     va_end(va);
+    return written;
 }
 
 // Print the null-terminated string 's' to the serial port and return the number of bytes written

src/main/blackbox/blackbox_io.h

@@ -36,7 +36,7 @@ uint8_t blackboxWriteChunkSize;
 
 void blackboxWrite(uint8_t value);
 
-void blackboxPrintf(char *fmt, ...);
+int blackboxPrintf(const char *fmt, ...);
 int blackboxPrint(const char *s);
 
 void blackboxWriteUnsignedVB(uint32_t value);

src/main/common/printf.c

@@ -56,29 +56,36 @@ typedef void (*putcf) (void *, char);
 static putcf stdout_putf;
 static void *stdout_putp;
 
-static void putchw(void *putp, putcf putf, int n, char z, char *bf)
+// print bf, padded from left to at least n characters. 
+// padding is zero ('0') if z!=0, space (' ') otherwise
+static int putchw(void *putp, putcf putf, int n, char z, char *bf)
 {
+    int written = 0;
     char fc = z ? '0' : ' ';
     char ch;
     char *p = bf;
     while (*p++ && n > 0)
         n--;
-    while (n-- > 0)
+    while (n-- > 0) {
-        putf(putp, fc);
+        putf(putp, fc); written++;
-    while ((ch = *bf++))
+    }
-        putf(putp, ch);
+    while ((ch = *bf++)) {
+        putf(putp, ch); written++;
+    }
+    return written;
 }
 
-void tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
+// retrun number of bytes written
+int tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
 {
     char bf[12];
-
+    int written = 0;
     char ch;
 
     while ((ch = *(fmt++))) {
-        if (ch != '%')
+        if (ch != '%') {
-            putf(putp, ch);
+            putf(putp, ch); written++;
-        else {
+        } else {
             char lz = 0;
 #ifdef 	REQUIRE_PRINTF_LONG_SUPPORT
             char lng = 0;
@@ -108,7 +115,7 @@ void tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
                     else
 #endif
                         ui2a(va_arg(va, unsigned int), 10, 0, bf);
-                    putchw(putp, putf, w, lz, bf);
+                    written += putchw(putp, putf, w, lz, bf);
                     break;
                 }
             case 'd':{
@@ -118,7 +125,7 @@ void tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
                     else
 #endif
                         i2a(va_arg(va, int), bf);
-                    putchw(putp, putf, w, lz, bf);
+                    written += putchw(putp, putf, w, lz, bf);
                     break;
                 }
             case 'x':
@@ -129,16 +136,19 @@ void tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
                 else
 #endif
                     ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf);
-                putchw(putp, putf, w, lz, bf);
+                written += putchw(putp, putf, w, lz, bf);
                 break;
             case 'c':
-                putf(putp, (char) (va_arg(va, int)));
+                putf(putp, (char) (va_arg(va, int))); written++;
                 break;
             case 's':
-                putchw(putp, putf, w, 0, va_arg(va, char *));
+                written += putchw(putp, putf, w, 0, va_arg(va, char *));
                 break;
             case '%':
-                putf(putp, ch);
+                putf(putp, ch); written++;
+                break;
+            case 'n':
+                *va_arg(va, int*) = written;
                 break;
             default:
                 break;
@@ -146,6 +156,7 @@ void tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
         }
     }
   abort:;
+    return written;
 }
 
 void init_printf(void *putp, void (*putf) (void *, char))
@@ -154,13 +165,14 @@ void init_printf(void *putp, void (*putf) (void *, char))
     stdout_putp = putp;
 }
 
-void tfp_printf(const char *fmt, ...)
+int tfp_printf(const char *fmt, ...)
 {
     va_list va;
     va_start(va, fmt);
-    tfp_format(stdout_putp, stdout_putf, fmt, va);
+    int written = tfp_format(stdout_putp, stdout_putf, fmt, va);
     va_end(va);
     while (!isSerialTransmitBufferEmpty(printfSerialPort));
+    return written;
 }
 
 static void putcp(void *p, char c)
@@ -168,14 +180,15 @@ static void putcp(void *p, char c)
     *(*((char **) p))++ = c;
 }
 
-void tfp_sprintf(char *s, const char *fmt, ...)
+int tfp_sprintf(char *s, const char *fmt, ...)
 {
     va_list va;
 
     va_start(va, fmt);
-    tfp_format(&s, putcp, fmt, va);
+    int written = tfp_format(&s, putcp, fmt, va);
     putcp(&s, 0);
     va_end(va);
+    return written;
 }
 
 
@@ -196,7 +209,7 @@ void printfSupportInit(void)
 int fputc(int c, FILE *f)
 {
     // let DMA catch up a bit when using set or dump, we're too fast.
-    while (!isSerialTransmitBufferEmpty(serialPorts.mainport));
+    while (!isSerialTransmitBufferEmpty(printfSerialPort));
     serialWrite(printfSerialPort, c);
     return c;
 }

src/main/common/printf.h

@@ -107,10 +107,10 @@ regs Kusti, 23.10.2004
 
 void init_printf(void *putp, void (*putf) (void *, char));
 
-void tfp_printf(const char *fmt, ...);
+int tfp_printf(const char *fmt, ...);
-void tfp_sprintf(char *s, const char *fmt, ...);
+int tfp_sprintf(char *s, const char *fmt, ...);
 
-void tfp_format(void *putp, void (*putf) (void *, char), const char *fmt, va_list va);
+int tfp_format(void *putp, void (*putf) (void *, char), const char *fmt, va_list va);
 
 #define printf tfp_printf
 #define sprintf tfp_sprintf