Fix dtostrf

teensy3/nonstd.c

@@ -30,7 +30,7 @@
 
 #include "avr_functions.h"
 #include <string.h>
-#include <stdio.h>
+#include <stdlib.h>
 
 size_t strlen(const char *s)
 {
@@ -74,12 +74,48 @@ char * ltoa(long val, char *buf, int radix)
 	}
 }
 
-// TODO: actually write an efficient dtostrf()....
 char * dtostrf(float val, int width, unsigned int precision, char *buf)
 {
-	char format[20];
-	sprintf(format, "%%%d.%df", width, precision);
-	sprintf(buf, format, val);
+	int decpt, sign, reqd, pad;
+	const char *s, *e;
+	char *p;
+
+	s = fcvt(val, precision, &decpt, &sign);
+	if (precision == 0 && decpt == 0) {
+		s = (*s < '5') ? "0" : "1";
+		reqd = 1;
+	} else {
+		reqd = strlen(s);
+		if (reqd > decpt) reqd++;
+		if (decpt == 0) reqd++;
+	}
+	if (sign) reqd++;
+	p = buf;
+	e = p + reqd;
+	pad = width - reqd;
+	if (pad > 0) {
+		e += pad;
+		while (pad-- > 0) *p++ = ' ';
+	}
+	if (sign) *p++ = '-';
+	if (decpt == 0 && precision > 0) {
+		*p++ = '0';
+		*p++ = '.';
+	}
+	while (p < e) {
+		*p++ = *s++;
+		if (p == e) break;
+		if (--decpt == 0) *p++ = '.';
+	}
+	if (width < 0) {
+		pad = (reqd + width) * -1;
+		while (pad-- > 0) *p++ = ' ';
+	}
+	*p = 0;
+
+	//char format[20];
+	//sprintf(format, "%%%d.%df", width, precision);
+	//sprintf(buf, format, val);
 	return buf;
 }