Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL: support %lld and %llu in internal printf

Browse Source 
useful for log messages with 64 bit timestamps
This commit is contained in:
Andrew Tridgell 2015-05-27 11:50:22 +10:00
parent 8be9e99fad
commit 4705be97bf
3 changed files with 84 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_HAL/utility/print_vprintf.cpp
View File

@ -59,6 +59,7 @@
#define FL_WIDTH 0x20 #define FL_WIDTH 0x20
#define FL_PREC 0x40 #define FL_PREC 0x40
#define FL_LONG 0x80 #define FL_LONG 0x80
#define FL_LONGLONG 0x100
#define FL_PGMSTRING FL_LONG #define FL_PGMSTRING FL_LONG
#define FL_NEGATIVE 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) 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 c; /* holds a char from the format string */
unsigned char flags; uint16_t flags;
unsigned char width; unsigned char width;
unsigned char prec; unsigned char prec;
unsigned char buf[13]; unsigned char buf[23];
for (;;) { for (;;) {
@ -147,6 +148,9 @@ void print_vprintf (AP_HAL::Print *s, unsigned char in_progmem, const char *fmt,
} }
if (c == 'h') if (c == 'h')
continue; continue;
} else if ((flags & FL_LONG) && c == 'l') {
flags |= FL_LONGLONG;
continue;
} }
break; 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 */ if (flags & FL_FLTFIX) { /* 'f' format */
n = exp > 0 ? exp : 0; /* exponent of left digit */ n = exp > 0 ? exp : 0; /* exponent of left digit */
unsigned char v = 0;
do { do {
if (n == -1) if (n == -1)
s->write('.'); s->write('.');
flags = (n <= exp && n > exp - ndigs) v = (n <= exp && n > exp - ndigs)
? buf[exp - n + 1] : '0'; ? buf[exp - n + 1] : '0';
if (--n < -prec || flags == 0) if (--n < -prec || v == 0)
break; break;
s->write(flags); s->write(v);
} while (1); } while (1);
if (n == exp if (n == exp
&& (buf[1] > '5' && (buf[1] > '5'
|| (buf[1] == '5' && !(vtype & FTOA_CARRY))) ) || (buf[1] == '5' && !(vtype & FTOA_CARRY))) )
{ {
flags = '1'; v = '1';
} }
if (flags) s->write(flags); if (v) s->write(v);
} else { /* 'e(E)' format */ } 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. * Handle integer formats variations for d/i, u, o, p, x, X.
*/ */
if (c == 'd' || c == 'i') { if (c == 'd' || c == 'i') {
long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int); if (flags & FL_LONGLONG) {
flags &= ~(FL_NEGATIVE | FL_ALT); int64_t x = va_arg(ap,long long);
if (x < 0) { flags &= ~(FL_NEGATIVE | FL_ALT);
x = -x; if (x < 0) {
flags |= FL_NEGATIVE; 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 { } else {
int base; 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); flags |= (FL_ALTHEX | FL_ALTUPP);
base = 16 | XTOA_UPPER; base = 16 | XTOA_UPPER;
ultoa: ultoa:
c = ultoa_invert ((flags & FL_LONG) if (flags & FL_LONGLONG) {
? va_arg(ap, unsigned long) c = ulltoa_invert (va_arg(ap, unsigned long long),
: va_arg(ap, unsigned int), (char *)buf, base) - (char *)buf;
(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; flags &= ~FL_NEGATIVE;
break; break;

46
libraries/AP_HAL/utility/utoa_invert.cpp
View File

@ -75,3 +75,49 @@ char * ultoa_invert (uint32_t val, char *s, uint8_t base) {
return s; 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;
}

1
libraries/AP_HAL/utility/xtoa_fast.h
View File

@ -36,6 +36,7 @@
/* Internal function for use from `printf'. */ /* Internal function for use from `printf'. */
char *ultoa_invert (uint32_t val, char *s, uint8_t base); 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. */ /* Next flags are to use with `base'. Unused fields are reserved. */
#define XTOA_PREFIX 0x0100 /* put prefix for octal or hex */ #define XTOA_PREFIX 0x0100 /* put prefix for octal or hex */