fix implementation of dtostrf

cores/avr/dtostrf.c

@@ -20,20 +20,86 @@
 // @Project Includes
 //****************************************************************************
 #include "dtostrf.h"
-#include <string.h>
 #include <stdio.h>
-
+#include <string.h>
+#include <stdbool.h>
+#include <math.h>
 
 //****************************************************************************
 // @Local Functions
 //****************************************************************************
 
-char* dtostrf(float val, int width, unsigned int precision, char* buf)
-{
-    char format[20];
-    sprintf(format, "%%%d.%df", width, precision);
-    sprintf(buf, format, val);
-    return buf;
+char * dtostrf(double number, signed int width, unsigned int prec, char *s) {
+    bool negative = false;
+
+    if (isnan(number)) {
+        strcpy(s, "nan");
+        return s;
+    }
+    if (isinf(number)) {
+        strcpy(s, "inf");
+        return s;
+    }
+
+    char* out = s;
+
+    int fillme = width; // how many cells to fill for the integer part
+    if (prec > 0) {
+        fillme -= (prec+1);
+    }
+
+    // Handle negative numbers
+    if (number < 0.0) {
+        negative = true;
+        fillme--;
+        number = -number;
+    }
+
+    // Round correctly so that print(1.999, 2) prints as "2.00"
+    // I optimized out most of the divisions
+    double rounding = 2.0;
+    for (unsigned int  i = 0; i < prec; ++i)
+        rounding *= 10.0;
+    rounding = 1.0 / rounding;
+
+    number += rounding;
+
+    // Figure out how big our number really is
+    double tenpow = 1.0;
+    unsigned int digitcount = 1;
+    while (number >= 10.0 * tenpow) {
+        tenpow *= 10.0;
+        digitcount++;
+    }
+
+    number /= tenpow;
+    fillme -= digitcount;
+
+    // Pad unused cells with spaces
+    while (fillme-- > 0) {
+        *out++ = ' ';
+    }
+
+    // Handle negative sign
+    if (negative) *out++ = '-';
+
+    // Print the digits, and if necessary, the decimal point
+    digitcount += prec;
+    int8_t digit = 0;
+    while (digitcount-- > 0) {
+        digit = (int8_t)number;
+        if (digit > 9) digit = 9; // insurance
+        *out++ = (char)('0' | digit);
+        if ((digitcount == prec) && (prec > 0)) {
+            *out++ = '.';
+        }
+        number -= digit;
+        number *= 10.0;
+    }
+
+    // make sure the string is terminated
+    *out = 0;
+    return s;
 }
 
 

cores/avr/dtostrf.h

@@ -25,14 +25,15 @@
 //****************************************************************************
 
 /*
- * \brief Converts a float into string..
+ * \brief Converts a double into string..
  *
- * \param val The float value to convert
+ * \param val The double value to convert
  * \param width Number of digits before the "."
- * \param precision Number of digits after the "."
- * \param buf Resultant output string
+ * \param prec Number of digits after the "."
+ * \param s Resultant output string
  */
-char* dtostrf(float val, int width, unsigned int precision, char* buf);
+
+char* dtostrf (double val, signed int width, unsigned int prec, char *s);
 
 
 #endif /* DTOSTRF_H_ */