diff --git a/cores/esp8266/core_esp8266_noniso.cpp b/cores/esp8266/core_esp8266_noniso.cpp index 2d1fb71605..c742904d21 100644 --- a/cores/esp8266/core_esp8266_noniso.cpp +++ b/cores/esp8266/core_esp8266_noniso.cpp @@ -22,12 +22,12 @@ */ -#include #include #include #include #include #include +#include #include "stdlib_noniso.h" extern "C" { @@ -41,9 +41,79 @@ char* ultoa(unsigned long value, char* result, int base) { } char * dtostrf(double number, signed char width, unsigned char prec, char *s) { - char fmt[32]; - sprintf(fmt, "%%%d.%df", width, prec); - sprintf(s, fmt, number); + 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 (uint8_t 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; + int digitcount = 1; + double nextpow; + while (number >= (nextpow = (10.0 * tenpow))) { + tenpow = nextpow; + digitcount++; + } + + // minimal compensation for possible lack of precision (#7087 addition) + number *= 1 + std::numeric_limits::epsilon(); + + 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; }