aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_HAL_AVR: vprintf implementation exists outside of class hierchary.

Browse Source 
* Makes it easier to mix into other classes.
This commit is contained in:
Pat Hickey 2012-09-13 16:46:07 -07:00 committed by Andrew Tridgell
parent 36154559fc
commit a3a6482fd3
4 changed files with 44 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_HAL_AVR/UARTDriver.cpp
View File

@ -10,12 +10,14 @@
#include <limits.h> #include <limits.h>
#include <stdlib.h> #include <stdlib.h>
#include <stdarg.h>
#include <avr/pgmspace.h> #include <avr/pgmspace.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_HAL.h> #include <AP_HAL.h>
#include "vprintf.h"
#include "UARTDriver.h" #include "UARTDriver.h"
using namespace AP_HAL_AVR; using namespace AP_HAL_AVR;
@ -268,13 +270,13 @@ void AVRUARTDriver::println_P(const prog_char_t *s) {
void AVRUARTDriver::printf(const char *fmt, ...) { void AVRUARTDriver::printf(const char *fmt, ...) {
va_list ap; va_list ap;
va_start(ap, fmt); va_start(ap, fmt);
_vprintf(0, fmt, ap); vprintf((AP_HAL::Print*)this, 0, fmt, ap);
va_end(ap); va_end(ap);
} }
void AVRUARTDriver::_printf_P(const prog_char *fmt, ...) { void AVRUARTDriver::_printf_P(const prog_char *fmt, ...) {
va_list ap; va_list ap;
va_start(ap, fmt); va_start(ap, fmt);
_vprintf(1, fmt, ap); vprintf((AP_HAL::Print*)this, 1, fmt, ap);
va_end(ap); va_end(ap);
} }

4
libraries/AP_HAL_AVR/UARTDriver.h
View File

@ -65,10 +65,6 @@ public:
uint8_t *bytes; ///< pointer to allocated buffer uint8_t *bytes; ///< pointer to allocated buffer
}; };
private: private:
/* Helper function _vprintf for implementing BetterStream's printfs */
void _vprintf(unsigned char, const char *, va_list)
__attribute__((format(__printf__,3,0)));
/* Instance Variables */ /* Instance Variables */
bool _initialized; bool _initialized;

62
libraries/AP_HAL_AVR/utility/vprintf.cpp
View File

@ -41,15 +41,14 @@
#include <avr/pgmspace.h> #include <avr/pgmspace.h>
#include <stdarg.h> #include <stdarg.h>
#include <string.h> #include <string.h>
extern "C" { extern "C" {
#include "ftoa_engine.h" #include "ftoa_engine.h"
#include "ntz.h" #include "ntz.h"
#include "xtoa_fast.h" #include "xtoa_fast.h"
} }
#include <AP_HAL.h> #include "vprintf.h"
#include "../UARTDriver.h"
using namespace AP_HAL_AVR;
// workaround for GCC bug c++/34734 // workaround for GCC bug c++/34734
#undef PROGMEM #undef PROGMEM
@ -94,8 +93,7 @@ using namespace AP_HAL_AVR;
#define FL_FLTEXP FL_PREC #define FL_FLTEXP FL_PREC
#define FL_FLTFIX FL_LONG #define FL_FLTFIX FL_LONG
void void vprintf (AP_HAL::Print *s, unsigned char in_progmem, const char *fmt, va_list ap)
AVRUARTDriver::_vprintf (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; unsigned char flags;
@ -117,8 +115,8 @@ AVRUARTDriver::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
} }
/* emit cr before lf to make most terminals happy */ /* emit cr before lf to make most terminals happy */
if (c == '\n') if (c == '\n')
write('\r'); s->write('\r');
write(c); s->write(c);
} }
flags = 0; flags = 0;
@ -232,21 +230,21 @@ AVRUARTDriver::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
width -= ndigs; width -= ndigs;
if (!(flags & FL_LPAD)) { if (!(flags & FL_LPAD)) {
do { do {
write(' '); s->write(' ');
} while (--width); } while (--width);
} }
} else { } else {
width = 0; width = 0;
} }
if (sign) if (sign)
write(sign); s->write(sign);
p = PSTR("inf"); p = PSTR("inf");
if (vtype & FTOA_NAN) if (vtype & FTOA_NAN)
p = PSTR("nan"); p = PSTR("nan");
while ( (ndigs = pgm_read_byte((const prog_char *)p)) != 0) { while ( (ndigs = pgm_read_byte((const prog_char *)p)) != 0) {
if (flags & FL_FLTUPP) if (flags & FL_FLTUPP)
ndigs += 'I' - 'i'; ndigs += 'I' - 'i';
write(ndigs); s->write(ndigs);
p++; p++;
} }
goto tail; goto tail;
@ -285,14 +283,14 @@ AVRUARTDriver::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
/* Output before first digit */ /* Output before first digit */
if (!(flags & (FL_LPAD | FL_ZFILL))) { if (!(flags & (FL_LPAD | FL_ZFILL))) {
while (width) { while (width) {
write(' '); s->write(' ');
width--; width--;
} }
} }
if (sign) write(sign); if (sign) s->write(sign);
if (!(flags & FL_LPAD)) { if (!(flags & FL_LPAD)) {
while (width) { while (width) {
write('0'); s->write('0');
width--; width--;
} }
} }
@ -302,12 +300,12 @@ AVRUARTDriver::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
n = exp > 0 ? exp : 0; /* exponent of left digit */ n = exp > 0 ? exp : 0; /* exponent of left digit */
do { do {
if (n == -1) if (n == -1)
write('.'); s->write('.');
flags = (n <= exp && n > exp - ndigs) flags = (n <= exp && n > exp - ndigs)
? buf[exp - n + 1] : '0'; ? buf[exp - n + 1] : '0';
if (--n < -prec) if (--n < -prec)
break; break;
write(flags); s->write(flags);
} while (1); } while (1);
if (n == exp if (n == exp
&& (buf[1] > '5' && (buf[1] > '5'
@ -315,34 +313,34 @@ AVRUARTDriver::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
{ {
flags = '1'; flags = '1';
} }
write(flags); s->write(flags);
} else { /* 'e(E)' format */ } else { /* 'e(E)' format */
/* mantissa */ /* mantissa */
if (buf[1] != '1') if (buf[1] != '1')
vtype &= ~FTOA_CARRY; vtype &= ~FTOA_CARRY;
write(buf[1]); s->write(buf[1]);
if (prec) { if (prec) {
write('.'); s->write('.');
sign = 2; sign = 2;
do { do {
write(buf[sign++]); s->write(buf[sign++]);
} while (--prec); } while (--prec);
} }
/* exponent */ /* exponent */
write(flags & FL_FLTUPP ? 'E' : 'e'); s->write(flags & FL_FLTUPP ? 'E' : 'e');
ndigs = '+'; ndigs = '+';
if (exp < 0 || (exp == 0 && (vtype & FTOA_CARRY) != 0)) { if (exp < 0 || (exp == 0 && (vtype & FTOA_CARRY) != 0)) {
exp = -exp; exp = -exp;
ndigs = '-'; ndigs = '-';
} }
write(ndigs); s->write(ndigs);
for (ndigs = '0'; exp >= 10; exp -= 10) for (ndigs = '0'; exp >= 10; exp -= 10)
ndigs += 1; ndigs += 1;
write(ndigs); s->write(ndigs);
write('0' + exp); s->write('0' + exp);
} }
goto tail; goto tail;
@ -379,12 +377,12 @@ AVRUARTDriver::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
str_lpad: str_lpad:
if (!(flags & FL_LPAD)) { if (!(flags & FL_LPAD)) {
while (size < width) { while (size < width) {
write(' '); s->write(' ');
width--; width--;
} }
} }
while (size) { while (size) {
write(GETBYTE (flags, FL_PGMSTRING, pnt)); s->write(GETBYTE (flags, FL_PGMSTRING, pnt));
if (width) width -= 1; if (width) width -= 1;
size -= 1; size -= 1;
} }
@ -480,7 +478,7 @@ AVRUARTDriver::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
} }
} }
while (len < width) { while (len < width) {
write(' '); s->write(' ');
len++; len++;
} }
} }
@ -488,30 +486,30 @@ AVRUARTDriver::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
width = (len < width) ? width - len : 0; width = (len < width) ? width - len : 0;
if (flags & FL_ALT) { if (flags & FL_ALT) {
write('0'); s->write('0');
if (flags & FL_ALTHEX) if (flags & FL_ALTHEX)
write(flags & FL_ALTUPP ? 'X' : 'x'); s->write(flags & FL_ALTUPP ? 'X' : 'x');
} else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) { } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {
unsigned char z = ' '; unsigned char z = ' ';
if (flags & FL_PLUS) z = '+'; if (flags & FL_PLUS) z = '+';
if (flags & FL_NEGATIVE) z = '-'; if (flags & FL_NEGATIVE) z = '-';
write(z); s->write(z);
} }
while (prec > c) { while (prec > c) {
write('0'); s->write('0');
prec--; prec--;
} }
do { do {
write(buf[--c]); s->write(buf[--c]);
} while (c); } while (c);
} }
tail: tail:
/* Tail is possible. */ /* Tail is possible. */
while (width) { while (width) {
write(' '); s->write(' ');
width--; width--;
} }
} /* for (;;) */ } /* for (;;) */

10
libraries/AP_HAL_AVR/utility/vprintf.h Normal file
View File

@ -0,0 +1,10 @@
#ifndef __AP_HAL_AVR_UTILITY_VPRINTF_H__
#define __AP_HAL_AVR_UTILITY_VPRINTF_H__
#include <AP_HAL.h>
void vprintf (AP_HAL::Print *s, unsigned char in_progmem, const char *fmt, va_list ap);
#endif //__AP_HAL_AVR_UTILITY_VPRINTF_H__