diff --git a/libraries/AP_HAL/utility/print_vprintf.cpp b/libraries/AP_HAL/utility/print_vprintf.cpp index 6cae4141e3..44c81aa82f 100644 --- a/libraries/AP_HAL/utility/print_vprintf.cpp +++ b/libraries/AP_HAL/utility/print_vprintf.cpp @@ -59,6 +59,7 @@ #define FL_WIDTH 0x20 #define FL_PREC 0x40 #define FL_LONG 0x80 +#define FL_LONGLONG 0x100 #define FL_PGMSTRING FL_LONG #define FL_NEGATIVE FL_LONG @@ -73,10 +74,10 @@ void print_vprintf (AP_HAL::Print *s, unsigned char in_progmem, const char *fmt, va_list ap) { unsigned char c; /* holds a char from the format string */ - unsigned char flags; + uint16_t flags; unsigned char width; unsigned char prec; - unsigned char buf[13]; + unsigned char buf[23]; for (;;) { @@ -147,6 +148,9 @@ void print_vprintf (AP_HAL::Print *s, unsigned char in_progmem, const char *fmt, } if (c == 'h') continue; + } else if ((flags & FL_LONG) && c == 'l') { + flags |= FL_LONGLONG; + continue; } break; @@ -279,22 +283,23 @@ void print_vprintf (AP_HAL::Print *s, unsigned char in_progmem, const char *fmt, if (flags & FL_FLTFIX) { /* 'f' format */ n = exp > 0 ? exp : 0; /* exponent of left digit */ + unsigned char v = 0; do { if (n == -1) s->write('.'); - flags = (n <= exp && n > exp - ndigs) + v = (n <= exp && n > exp - ndigs) ? buf[exp - n + 1] : '0'; - if (--n < -prec || flags == 0) + if (--n < -prec || v == 0) break; - s->write(flags); + s->write(v); } while (1); if (n == exp && (buf[1] > '5' || (buf[1] == '5' && !(vtype & FTOA_CARRY))) ) { - flags = '1'; + v = '1'; } - if (flags) s->write(flags); + if (v) s->write(v); } else { /* 'e(E)' format */ @@ -375,13 +380,23 @@ void print_vprintf (AP_HAL::Print *s, unsigned char in_progmem, const char *fmt, * Handle integer formats variations for d/i, u, o, p, x, X. */ if (c == 'd' || c == 'i') { - long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int); - flags &= ~(FL_NEGATIVE | FL_ALT); - if (x < 0) { - x = -x; - flags |= FL_NEGATIVE; + if (flags & FL_LONGLONG) { + int64_t x = va_arg(ap,long long); + flags &= ~(FL_NEGATIVE | FL_ALT); + if (x < 0) { + x = -x; + flags |= FL_NEGATIVE; + } + c = ulltoa_invert (x, (char *)buf, 10) - (char *)buf; + } else { + long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int); + flags &= ~(FL_NEGATIVE | FL_ALT); + if (x < 0) { + x = -x; + flags |= FL_NEGATIVE; + } + c = ultoa_invert (x, (char *)buf, 10) - (char *)buf; } - c = ultoa_invert (x, (char *)buf, 10) - (char *)buf; } else { int base; @@ -411,10 +426,15 @@ void print_vprintf (AP_HAL::Print *s, unsigned char in_progmem, const char *fmt, flags |= (FL_ALTHEX | FL_ALTUPP); base = 16 | XTOA_UPPER; ultoa: - c = ultoa_invert ((flags & FL_LONG) - ? va_arg(ap, unsigned long) - : va_arg(ap, unsigned int), - (char *)buf, base) - (char *)buf; + if (flags & FL_LONGLONG) { + c = ulltoa_invert (va_arg(ap, unsigned long long), + (char *)buf, base) - (char *)buf; + } else { + c = ultoa_invert ((flags & FL_LONG) + ? va_arg(ap, unsigned long) + : va_arg(ap, unsigned int), + (char *)buf, base) - (char *)buf; + } flags &= ~FL_NEGATIVE; break; diff --git a/libraries/AP_HAL/utility/utoa_invert.cpp b/libraries/AP_HAL/utility/utoa_invert.cpp index 0f8584e842..30eaecdff2 100644 --- a/libraries/AP_HAL/utility/utoa_invert.cpp +++ b/libraries/AP_HAL/utility/utoa_invert.cpp @@ -75,3 +75,49 @@ char * ultoa_invert (uint32_t val, char *s, uint8_t base) { return s; } + +char * ulltoa_invert (uint64_t val, char *s, uint8_t base) { + if (base == 8) { + do { + *s = '0' + (val & 0x7); + val >>= 3; + } while(val); + return s; + } + + if (base == 16) { + do { + uint8_t digit = '0' + (val & 0xf); +#if XTOA_UPPER == 0 + if (digit > '0' + 9) + digit += ('a' - '0' - 10); +#else + if (digit > '0' + 9) + digit += ('A' - '0' - 10); +#endif + *s++ = digit; + val >>= 4; + } while(val); + return s; + } + + // Every base which in not hex and not oct is considered decimal. + + // 64 bits is not actually enough, we need 65, but it should + // be good enough for the log dumping we're using this for + uint64_t xval = val; + do { + uint8_t saved = xval; + xval &= ~1; + xval += 2; + xval += xval >> 1; // *1.5 + xval += xval >> 4; // *1.0625 + xval += xval >> 8; // *1.00390625 + xval += xval >> 16; // *1.000015259 + xval += xval >> 32; // it all amounts to *1.6 + xval >>= 4; // /16 ... so *1.6/16 is /10, fraction truncated. + *s++ = '0' + saved - 10 * (uint8_t)xval; + } while (xval); + return s; +} + diff --git a/libraries/AP_HAL/utility/xtoa_fast.h b/libraries/AP_HAL/utility/xtoa_fast.h index 024fd51eda..21462c7b2a 100644 --- a/libraries/AP_HAL/utility/xtoa_fast.h +++ b/libraries/AP_HAL/utility/xtoa_fast.h @@ -36,6 +36,7 @@ /* Internal function for use from `printf'. */ char *ultoa_invert (uint32_t val, char *s, uint8_t base); +char *ulltoa_invert (uint64_t val, char *s, uint8_t base); /* Next flags are to use with `base'. Unused fields are reserved. */ #define XTOA_PREFIX 0x0100 /* put prefix for octal or hex */