added printf() support via SpareTimeLabs printf lib. this cleaned up some mess inside cli.c

example of usage: http://bcas.tv/paste/results/xToE9w26.html
hover-tested quadx loaded as custom mix, works.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@207 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

Makefile

@@ -52,6 +52,7 @@ COMMON_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drv_i2c_soft.c \
 		   drv_system.c \
 		   drv_uart.c \
+		   printf.c \
 		   $(CMSIS_SRC) \
 		   $(STDPERIPH_SRC)
 

src/board.h

@@ -13,6 +13,7 @@
 
 #include "stm32f10x_conf.h"
 #include "core_cm3.h"
+#include "printf.h"
 
 #ifndef M_PI
 #define M_PI       3.14159265358979323846f

src/cli.c

@@ -3,6 +3,7 @@
 
 // we unset this on 'exit'
 extern uint8_t cliMode;
+static void cliCMix(char *cmdline);
 static void cliDefaults(char *cmdline);
 static void cliExit(char *cmdline);
 static void cliFeature(char *cmdline);
@@ -25,6 +26,9 @@ extern const char rcChannelLetters[];
 static char cliBuffer[48];
 static uint32_t bufferIndex = 0;
 
+static float _atof(const char *p);
+static char *ftoa(float x, char *floatString);
+
 // sync this with MultiType enum from mw.h
 const char *mixerNames[] = {
     "TRI", "QUADP", "QUADX", "BI",
@@ -59,6 +63,7 @@ typedef struct {
 
 // should be sorted a..z for bsearch()
 const clicmd_t cmdTable[] = {
+    { "cmix", "design custom mixer", cliCMix },
     { "defaults", "reset to defaults and reboot", cliDefaults },
     { "exit", "", cliExit },
     { "feature", "list or -val or val", cliFeature },
@@ -66,7 +71,7 @@ const clicmd_t cmdTable[] = {
     { "map", "mapping of rc channel order", cliMap },
     { "mixer", "mixer name or list", cliMixer },
     { "save", "save and reboot", cliSave },
-    { "set", "name=value or blank for list", cliSet },
+    { "set", "name=value or blank or * for list", cliSet },
     { "status", "show system status", cliStatus },
     { "version", "", cliVersion },
 };
@@ -205,6 +210,145 @@ char *itoa(int i, char *a, int r)
 
 #endif
 
+////////////////////////////////////////////////////////////////////////////////
+// String to Float Conversion
+///////////////////////////////////////////////////////////////////////////////
+// Simple and fast atof (ascii to float) function.
+//
+// - Executes about 5x faster than standard MSCRT library atof().
+// - An attractive alternative if the number of calls is in the millions.
+// - Assumes input is a proper integer, fraction, or scientific format.
+// - Matches library atof() to 15 digits (except at extreme exponents).
+// - Follows atof() precedent of essentially no error checking.
+//
+// 09-May-2009 Tom Van Baak (tvb) www.LeapSecond.com
+//
+#define white_space(c) ((c) == ' ' || (c) == '\t')
+#define valid_digit(c) ((c) >= '0' && (c) <= '9')
+static float _atof(const char *p)
+{
+    int frac = 0;
+    double sign, value, scale;
+
+    // Skip leading white space, if any.
+    while (white_space(*p) ) {
+        p += 1;
+    }
+
+    // Get sign, if any.
+    sign = 1.0;
+    if (*p == '-') {
+        sign = -1.0;
+        p += 1;
+
+    } else if (*p == '+') {
+        p += 1;
+    }
+
+    // Get digits before decimal point or exponent, if any.
+    value = 0.0;
+    while (valid_digit(*p)) {
+        value = value * 10.0 + (*p - '0');
+        p += 1;
+    }
+
+    // Get digits after decimal point, if any.
+    if (*p == '.') {
+        double pow10 = 10.0;
+        p += 1;
+
+        while (valid_digit(*p)) {
+            value += (*p - '0') / pow10;
+            pow10 *= 10.0;
+            p += 1;
+        }
+    }
+
+    // Handle exponent, if any.
+    scale = 1.0;
+    if ((*p == 'e') || (*p == 'E')) {
+        unsigned int expon;
+        p += 1;
+
+        // Get sign of exponent, if any.
+        frac = 0;
+        if (*p == '-') {
+            frac = 1;
+            p += 1;
+
+        } else if (*p == '+') {
+            p += 1;
+        }
+
+        // Get digits of exponent, if any.
+        expon = 0;
+        while (valid_digit(*p)) {
+            expon = expon * 10 + (*p - '0');
+            p += 1;
+        }
+        if (expon > 308) expon = 308;
+
+        // Calculate scaling factor.
+        while (expon >= 50) { scale *= 1E50; expon -= 50; }
+        while (expon >=  8) { scale *= 1E8;  expon -=  8; }
+        while (expon >   0) { scale *= 10.0; expon -=  1; }
+    }
+
+    // Return signed and scaled floating point result.
+    return sign * (frac ? (value / scale) : (value * scale));
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// FTOA
+///////////////////////////////////////////////////////////////////////////////
+static char *ftoa(float x, char *floatString)
+{
+    int32_t value;
+    char intString1[12];
+    char intString2[12] = { 0, };
+    char *decimalPoint = ".";
+    uint8_t dpLocation;
+
+    if (x > 0)                  // Rounding for x.xxx display format
+        x += 0.0005f;
+    else
+        x -= 0.0005f;
+
+    value = (int32_t) (x * 1000.0f);    // Convert float * 1000 to an integer
+
+    itoa(abs(value), intString1, 10);   // Create string from abs of integer value
+
+    if (value >= 0)
+        intString2[0] = ' ';    // Positive number, add a pad space
+    else
+        intString2[0] = '-';    // Negative number, add a negative sign
+
+    if (strlen(intString1) == 1) {
+        intString2[1] = '0';
+        intString2[2] = '0';
+        intString2[3] = '0';
+        strcat(intString2, intString1);
+    } else if (strlen(intString1) == 2) {
+        intString2[1] = '0';
+        intString2[2] = '0';
+        strcat(intString2, intString1);
+    } else if (strlen(intString1) == 3) {
+        intString2[1] = '0';
+        strcat(intString2, intString1);
+    } else {
+        strcat(intString2, intString1);
+    }
+
+    dpLocation = strlen(intString2) - 3;
+
+    strncpy(floatString, intString2, dpLocation);
+    floatString[dpLocation] = '\0';
+    strcat(floatString, decimalPoint);
+    strcat(floatString, intString2 + dpLocation);
+
+    return floatString;
+}
+
 static void cliPrompt(void)
 {
     uartPrint("\r\n# ");
@@ -216,6 +360,92 @@ static int cliCompare(const void *a, const void *b)
     return strncasecmp(ca->name, cb->name, strlen(cb->name));
 }
 
+static void cliCMix(char *cmdline)
+{
+    int i, check = 0;
+    int num_motors = 0;
+    uint8_t len;
+    char buf[16];
+    float mixsum[3];
+    char *ptr;
+
+    len = strlen(cmdline);
+
+    if (len == 0) {
+        uartPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
+        for (i = 0; i < MAX_MOTORS; i++) {
+            if (cfg.customMixer[i].throttle == 0.0f)
+                break;
+            mixsum[i] = 0.0f;
+            num_motors++;
+            printf("#%d:\t", i + 1);
+            printf("%s\t", ftoa(cfg.customMixer[i].throttle, buf));
+            printf("%s\t", ftoa(cfg.customMixer[i].roll, buf));
+            printf("%s\t", ftoa(cfg.customMixer[i].pitch, buf));
+            printf("%s\r\n", ftoa(cfg.customMixer[i].yaw, buf));
+        }
+        for (i = 0; i < num_motors; i++) {
+            mixsum[0] += cfg.customMixer[i].roll;
+            mixsum[1] += cfg.customMixer[i].pitch;
+            mixsum[2] += cfg.customMixer[i].yaw;
+        }
+        uartPrint("Sanity check:\t");
+        for (i = 0; i < 3; i++)
+            uartPrint(fabs(mixsum[i]) > 0.01f ? "NG\t" : "OK\t");
+        uartPrint("\r\n");
+        return;
+    } else if (strncasecmp(cmdline, "load", 4) == 0) {
+        ptr = strchr(cmdline, ' ');
+        if (ptr) {
+            len = strlen(++ptr);
+            for (i = 0; ; i++) {
+                if (mixerNames[i] == NULL) {
+                    uartPrint("Invalid mixer type...\r\n");
+                    break;
+                }
+                if (strncasecmp(ptr, mixerNames[i], len) == 0) {
+                    mixerLoadMix(i);
+                    printf("Loaded %s mix...\r\n", mixerNames[i]);
+                    cliCMix("");
+                    break;
+                }
+            }
+        }
+    } else {
+        ptr = cmdline;
+        i = atoi(ptr); // get motor number
+        if (--i < MAX_MOTORS) {
+            ptr = strchr(ptr, ' ');
+            if (ptr) {
+                cfg.customMixer[i].throttle = _atof(++ptr); 
+                check++;
+            }
+            ptr = strchr(ptr, ' ');
+            if (ptr) {
+                cfg.customMixer[i].roll = _atof(++ptr);
+                check++;
+            }
+            ptr = strchr(ptr, ' ');
+            if (ptr) {
+                cfg.customMixer[i].pitch = _atof(++ptr);
+                check++;
+            }
+            ptr = strchr(ptr, ' ');
+            if (ptr) {
+                cfg.customMixer[i].yaw = _atof(++ptr);
+                check++;
+            }
+            if (check != 4) {
+                uartPrint("Wrong number of arguments, needs idx thr roll pitch yaw\r\n");
+            } else {
+                cliCMix("");
+            }
+        } else {
+            printf("Motor number must be between 1 and %d\r\n", MAX_MOTORS);
+        }
+    }
+}
+
 static void cliDefaults(char *cmdline)
 {
     uartPrint("Resetting to defaults...\r\n");
@@ -250,8 +480,7 @@ static void cliFeature(char *cmdline)
             if (featureNames[i] == NULL)
                 break;
             if (mask & (1 << i))
-                uartPrint((char *)featureNames[i]);
+                printf("%s ", featureNames[i]);
-            uartWrite(' ');
         }
         uartPrint("\r\n");
     } else if (strncasecmp(cmdline, "list", len) == 0) {
@@ -259,8 +488,7 @@ static void cliFeature(char *cmdline)
         for (i = 0; ; i++) {
             if (featureNames[i] == NULL)
                 break;
-            uartPrint((char *)featureNames[i]);
+            printf("%s ", featureNames[i]);
-            uartWrite(' ');
         }
         uartPrint("\r\n");
         return;
@@ -286,8 +514,7 @@ static void cliFeature(char *cmdline)
                     featureSet(1 << i);
                     uartPrint("Enabled ");
                 }
-                uartPrint((char *)featureNames[i]);
+                printf("%s\r\n", featureNames[i]);
-                uartPrint("\r\n");
                 break;
             }
         }
@@ -299,14 +526,8 @@ static void cliHelp(char *cmdline)
     uint32_t i = 0;
 
     uartPrint("Available commands:\r\n");    
-
+    for (i = 0; i < CMD_COUNT; i++)
-    for (i = 0; i < CMD_COUNT; i++) {
+        printf("%s\t%s\r\n", cmdTable[i].name, cmdTable[i].param);
-        uartPrint(cmdTable[i].name);
-        uartWrite('\t');
-        uartPrint(cmdTable[i].param);
-        uartPrint("\r\n");
-        while (!uartTransmitEmpty());
-    }
 }
 
 static void cliMap(char *cmdline)
@@ -333,8 +554,7 @@ static void cliMap(char *cmdline)
     for (i = 0; i < 8; i++)
         out[cfg.rcmap[i]] = rcChannelLetters[i];
     out[i] = '\0';
-    uartPrint(out);
+    printf("%s\r\n", out);
-    uartPrint("\r\n");
 }
 
 static void cliMixer(char *cmdline)
@@ -345,17 +565,14 @@ static void cliMixer(char *cmdline)
     len = strlen(cmdline);
 
     if (len == 0) {
-        uartPrint("Current mixer: ");
+        printf("Current mixer: %s\r\n", mixerNames[cfg.mixerConfiguration - 1]);
-        uartPrint((char *)mixerNames[cfg.mixerConfiguration - 1]);
-        uartPrint("\r\n");
         return;
     } else if (strncasecmp(cmdline, "list", len) == 0) {
         uartPrint("Available mixers: ");
         for (i = 0; ; i++) {
             if (mixerNames[i] == NULL)
                 break;
-            uartPrint((char *)mixerNames[i]);
+            printf("%s ", mixerNames[i]);
-            uartWrite(' ');
         }
         uartPrint("\r\n");
         return;
@@ -368,9 +585,7 @@ static void cliMixer(char *cmdline)
         }
         if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
             cfg.mixerConfiguration = i + 1;
-            uartPrint("Mixer set to ");
+            printf("Mixer set to %s\r\n", mixerNames[i]);
-            uartPrint((char *)mixerNames[i]);
-            uartPrint("\r\n");
             break;
         }
     }
@@ -388,7 +603,6 @@ static void cliSave(char *cmdline)
 static void cliPrintVar(const clivalue_t *var, uint32_t full)
 {
     int32_t value = 0;
-    char buf[16];
 
     switch (var->type) {
         case VAR_UINT8:
@@ -411,16 +625,9 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full)
             value = *(uint32_t *)var->ptr;
             break;
     }
-    itoa(value, buf, 10);
+    printf("%d", value);
-    uartPrint(buf);
+    if (full)
-    if (full) {
+        printf(" %d %d", var->min, var->max);
-        uartPrint(" ");
-        itoa(var->min, buf, 10);
-        uartPrint(buf);
-        uartPrint(" ");
-        itoa(var->max, buf, 10);
-        uartPrint(buf);
-    }
 }
 
 static void cliSetVar(const clivalue_t *var, const int32_t value)
@@ -456,11 +663,9 @@ static void cliSet(char *cmdline)
         uartPrint("Current settings: \r\n");
         for (i = 0; i < VALUE_COUNT; i++) {
             val = &valueTable[i];
-            uartPrint((char *)valueTable[i].name);
+            printf("%s = ", valueTable[i].name);
-            uartPrint(" = ");
             cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui
             uartPrint("\r\n");
-            while (!uartTransmitEmpty());
         }
     } else if ((eqptr = strstr(cmdline, "="))) {
         // has equal, set var
@@ -473,8 +678,7 @@ static void cliSet(char *cmdline)
                 // found
                 if (value >= valueTable[i].min && value <= valueTable[i].max) {
                     cliSetVar(val, value);
-                    uartPrint((char *)valueTable[i].name);
+                    printf("%s set to ", valueTable[i].name);
-                    uartPrint(" set to ");
                     cliPrintVar(val, 0);
                 } else {
                     uartPrint("ERR: Value assignment out of range\r\n");
@@ -488,21 +692,11 @@ static void cliSet(char *cmdline)
 
 static void cliStatus(char *cmdline)
 {
-    char buf[16];
     uint8_t i;
     uint32_t mask;
 
-    uartPrint("System Uptime: ");
+    printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n",
-    itoa(millis() / 1000, buf, 10);
+        millis() / 1000, vbat, batteryCellCount);
-    uartPrint(buf);
-    uartPrint(" seconds, Voltage: ");
-    itoa(vbat, buf, 10);
-    uartPrint(buf);
-    uartPrint(" * 0.1V (");
-    itoa(batteryCellCount, buf, 10);
-    uartPrint(buf);
-    uartPrint("S battery)\r\n");
-
     mask = sensorsMask();
 
     uartPrint("Detected sensors: ");
@@ -510,22 +704,13 @@ static void cliStatus(char *cmdline)
         if (sensorNames[i] == NULL)
             break;
         if (mask & (1 << i))
-            uartPrint((char *)sensorNames[i]);
+            printf("%s ", sensorNames[i]);
-        uartWrite(' ');
-    }
-    if (sensors(SENSOR_ACC)) {
-        uartPrint("ACCHW: ");
-        uartPrint((char *)accNames[accHardware]);
     }
+    if (sensors(SENSOR_ACC))
+        printf("ACCHW: %s", accNames[accHardware]);
     uartPrint("\r\n");
 
-    uartPrint("Cycle Time: ");
+    printf("Cycle Time: %d, I2C Errors: %d\r\n", cycleTime, i2cGetErrorCounter());
-    itoa(cycleTime, buf, 10);
-    uartPrint(buf);
-    uartPrint(", I2C Errors: ");
-    itoa(i2cGetErrorCounter(), buf, 10);
-    uartPrint(buf);
-    uartPrint("\r\n");
 }
 
 static void cliVersion(char *cmdline)

src/config.c

@@ -196,7 +196,7 @@ void checkFirstTime(bool reset)
     // serial (USART1) baudrate
     cfg.serial_baudrate = 115200;
 
-    // custom mixer
+    // custom mixer. clear by defaults.
     for (i = 0; i < MAX_MOTORS; i++)
         cfg.customMixer[i].throttle = 0.0f;
 

src/main.c

@@ -8,6 +8,12 @@ extern rcReadRawDataPtr rcReadRawFunc;
 extern uint16_t pwmReadRawRC(uint8_t chan);
 extern uint16_t spektrumReadRawRC(uint8_t chan);
 
+static void _putc(void *p, char c)
+{
+    uartWrite(c);
+}
+
+
 int main(void)
 {
     uint8_t i;
@@ -35,6 +41,7 @@ int main(void)
 #endif
 
     systemInit();
+    init_printf(NULL, _putc);
 
     readEEPROM();
     checkFirstTime(false);

src/mixer.c

@@ -108,7 +108,7 @@ static const motorMixer_t mixerVtail4[] = {
 };
 
 // Keep this synced with MultiType struct in mw.h!
-static const mixer_t mixers[] = {
+const mixer_t mixers[] = {
 //    Mo Se Mixtable
     { 0, 0, NULL },                // entry 0
     { 3, 1, mixerTri },            // MULTITYPE_TRI
@@ -160,6 +160,23 @@ void mixerInit(void)
     }
 }
 
+void mixerLoadMix(int index)
+{
+    int i;
+
+    // we're 1-based
+    index++;
+    // clear existing
+    for (i = 0; i < MAX_MOTORS; i++)
+        cfg.customMixer[i].throttle = 0.0f;
+
+    // do we have anything here to begin with?
+    if (mixers[index].motor != NULL) {
+        for (i = 0; i < mixers[index].numberMotor; i++)
+            cfg.customMixer[i] = mixers[index].motor[i];
+    }
+}
+
 void writeServos(void)
 {
     if (!useServo)

src/mw.h

@@ -299,6 +299,7 @@ void Sonar_update(void);
 
 // Output
 void mixerInit(void);
+void mixerLoadMix(int index);
 void writeServos(void);
 void writeMotors(void);
 void writeAllMotors(int16_t mc);

src/printf.c

@@ -0,0 +1,248 @@
+/*
+ * Copyright (c) 2004,2012 Kustaa Nyholm / SpareTimeLabs
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification, 
+ * are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list 
+ * of conditions and the following disclaimer.
+ * 
+ * Redistributions in binary form must reproduce the above copyright notice, this 
+ * list of conditions and the following disclaimer in the documentation and/or other
+ * materials provided with the distribution.
+ *  
+ * Neither the name of the Kustaa Nyholm or SpareTimeLabs nor the names of its 
+ * contributors may be used to endorse or promote products derived from this software 
+ * without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 
+ * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
+ * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ */
+
+#include "board.h"
+#include "printf.h"
+
+#define PRINTF_LONG_SUPPORT
+
+typedef void (*putcf) (void *, char);
+static putcf stdout_putf;
+static void *stdout_putp;
+
+#ifdef PRINTF_LONG_SUPPORT
+
+static void uli2a(unsigned long int num, unsigned int base, int uc, char *bf)
+{
+    int n = 0;
+    unsigned int d = 1;
+    while (num / d >= base)
+        d *= base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+static void li2a(long num, char *bf)
+{
+    if (num < 0) {
+        num = -num;
+        *bf++ = '-';
+    }
+    uli2a(num, 10, 0, bf);
+}
+
+#endif
+
+static void ui2a(unsigned int num, unsigned int base, int uc, char *bf)
+{
+    int n = 0;
+    unsigned int d = 1;
+    while (num / d >= base)
+        d *= base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+static void i2a(int num, char *bf)
+{
+    if (num < 0) {
+        num = -num;
+        *bf++ = '-';
+    }
+    ui2a(num, 10, 0, bf);
+}
+
+static int a2d(char ch)
+{
+    if (ch >= '0' && ch <= '9')
+        return ch - '0';
+    else if (ch >= 'a' && ch <= 'f')
+        return ch - 'a' + 10;
+    else if (ch >= 'A' && ch <= 'F')
+        return ch - 'A' + 10;
+    else
+        return -1;
+}
+
+static char a2i(char ch, char **src, int base, int *nump)
+{
+    char *p = *src;
+    int num = 0;
+    int digit;
+    while ((digit = a2d(ch)) >= 0) {
+        if (digit > base)
+            break;
+        num = num * base + digit;
+        ch = *p++;
+    }
+    *src = p;
+    *nump = num;
+    return ch;
+}
+
+static void putchw(void *putp, putcf putf, int n, char z, char *bf)
+{
+    char fc = z ? '0' : ' ';
+    char ch;
+    char *p = bf;
+    while (*p++ && n > 0)
+        n--;
+    while (n-- > 0)
+        putf(putp, fc);
+    while ((ch = *bf++))
+        putf(putp, ch);
+}
+
+void tfp_format(void *putp, putcf putf, char *fmt, va_list va)
+{
+    char bf[12];
+
+    char ch;
+
+    while ((ch = *(fmt++))) {
+        if (ch != '%')
+            putf(putp, ch);
+        else {
+            char lz = 0;
+#ifdef 	PRINTF_LONG_SUPPORT
+            char lng = 0;
+#endif
+            int w = 0;
+            ch = *(fmt++);
+            if (ch == '0') {
+                ch = *(fmt++);
+                lz = 1;
+            }
+            if (ch >= '0' && ch <= '9') {
+                ch = a2i(ch, &fmt, 10, &w);
+            }
+#ifdef 	PRINTF_LONG_SUPPORT
+            if (ch == 'l') {
+                ch = *(fmt++);
+                lng = 1;
+            }
+#endif
+            switch (ch) {
+            case 0:
+                goto abort;
+            case 'u':{
+#ifdef 	PRINTF_LONG_SUPPORT
+                    if (lng)
+                        uli2a(va_arg(va, unsigned long int), 10, 0, bf);
+                    else
+#endif
+                        ui2a(va_arg(va, unsigned int), 10, 0, bf);
+                    putchw(putp, putf, w, lz, bf);
+                    break;
+                }
+            case 'd':{
+#ifdef 	PRINTF_LONG_SUPPORT
+                    if (lng)
+                        li2a(va_arg(va, unsigned long int), bf);
+                    else
+#endif
+                        i2a(va_arg(va, int), bf);
+                    putchw(putp, putf, w, lz, bf);
+                    break;
+                }
+            case 'x':
+            case 'X':
+#ifdef 	PRINTF_LONG_SUPPORT
+                if (lng)
+                    uli2a(va_arg(va, unsigned long int), 16, (ch == 'X'), bf);
+                else
+#endif
+                    ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf);
+                putchw(putp, putf, w, lz, bf);
+                break;
+            case 'c':
+                putf(putp, (char) (va_arg(va, int)));
+                break;
+            case 's':
+                putchw(putp, putf, w, 0, va_arg(va, char *));
+                break;
+            case '%':
+                putf(putp, ch);
+            default:
+                break;
+            }
+        }
+    }
+  abort:;
+}
+
+
+void init_printf(void *putp, void (*putf) (void *, char))
+{
+    stdout_putf = putf;
+    stdout_putp = putp;
+}
+
+void tfp_printf(char *fmt, ...)
+{
+    va_list va;
+    va_start(va, fmt);
+    tfp_format(stdout_putp, stdout_putf, fmt, va);
+    va_end(va);
+    while (!uartTransmitEmpty());
+}
+
+static void putcp(void *p, char c)
+{
+    *(*((char **) p))++ = c;
+}
+
+
+
+void tfp_sprintf(char *s, char *fmt, ...)
+{
+    va_list va;
+    va_start(va, fmt);
+    tfp_format(&s, putcp, fmt, va);
+    putcp(&s, 0);
+    va_end(va);
+}

src/printf.h

@@ -0,0 +1,121 @@
+/*
+File: printf.h
+ 
+Copyright (c) 2004,2012 Kustaa Nyholm / SpareTimeLabs
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list 
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this 
+list of conditions and the following disclaimer in the documentation and/or other
+materials provided with the distribution.
+ 
+Neither the name of the Kustaa Nyholm or SpareTimeLabs nor the names of its 
+contributors may be used to endorse or promote products derived from this software 
+without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 
+IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
+INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 
+NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
+OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+OF SUCH DAMAGE.
+
+----------------------------------------------------------------------
+
+This library is realy just two files: 'printf.h' and 'printf.c'.
+
+They provide a simple and small (+200 loc) printf functionality to 
+be used in embedded systems.
+
+I've found them so usefull in debugging that I do not bother with a 
+debugger at all.
+
+They are distributed in source form, so to use them, just compile them 
+into your project. 
+
+Two printf variants are provided: printf and sprintf. 
+
+The formats supported by this implementation are: 'd' 'u' 'c' 's' 'x' 'X'.
+
+Zero padding and field width are also supported.
+
+If the library is compiled with 'PRINTF_SUPPORT_LONG' defined then the 
+long specifier is also
+supported. Note that this will pull in some long math routines (pun intended!)
+and thus make your executable noticably longer.
+
+The memory foot print of course depends on the target cpu, compiler and 
+compiler options, but a rough guestimate (based on a H8S target) is about 
+1.4 kB for code and some twenty 'int's and 'char's, say 60 bytes of stack space. 
+Not too bad. Your milage may vary. By hacking the source code you can 
+get rid of some hunred bytes, I'm sure, but personally I feel the balance of 
+functionality and flexibility versus  code size is close to optimal for
+many embedded systems.
+
+To use the printf you need to supply your own character output function, 
+something like :
+
+void putc ( void* p, char c)
+	{
+	while (!SERIAL_PORT_EMPTY) ;
+	SERIAL_PORT_TX_REGISTER = c;
+	}
+
+Before you can call printf you need to initialize it to use your 
+character output function with something like:
+
+init_printf(NULL,putc);
+
+Notice the 'NULL' in 'init_printf' and the parameter 'void* p' in 'putc', 
+the NULL (or any pointer) you pass into the 'init_printf' will eventually be 
+passed to your 'putc' routine. This allows you to pass some storage space (or 
+anything realy) to the character output function, if necessary. 
+This is not often needed but it was implemented like that because it made 
+implementing the sprintf function so neat (look at the source code).
+
+The code is re-entrant, except for the 'init_printf' function, so it 
+is safe to call it from interupts too, although this may result in mixed output. 
+If you rely on re-entrancy, take care that your 'putc' function is re-entrant!
+
+The printf and sprintf functions are actually macros that translate to 
+'tfp_printf' and 'tfp_sprintf'. This makes it possible
+to use them along with 'stdio.h' printf's in a single source file. 
+You just need to undef the names before you include the 'stdio.h'.
+Note that these are not function like macros, so if you have variables
+or struct members with these names, things will explode in your face.
+Without variadic macros this is the best we can do to wrap these
+fucnction. If it is a problem just give up the macros and use the
+functions directly or rename them.
+
+For further details see source code.
+
+regs Kusti, 23.10.2004
+*/
+
+
+#ifndef __TFP_PRINTF__
+#define __TFP_PRINTF__
+
+#include <stdarg.h>
+
+void init_printf(void *putp, void (*putf) (void *, char));
+
+void tfp_printf(char *fmt, ...);
+void tfp_sprintf(char *s, char *fmt, ...);
+
+void tfp_format(void *putp, void (*putf) (void *, char), char *fmt, va_list va);
+
+#define printf tfp_printf
+#define sprintf tfp_sprintf
+
+#endif