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

HAL_AVR: changed to C implementation of float formatting

Browse Source 
Many thanks to Soren Kuula for this work!
This commit is contained in:
Andrew Tridgell 2013-05-28 13:21:59 +10:00
parent 36466e91ed
commit 16a0dfc38c
7 changed files with 316 additions and 771 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

535
libraries/AP_HAL_AVR/utility/ftoa_engine.S
View File

@ -1,535 +0,0 @@
/* Copyright (c) 2005, Dmitry Xmelkov
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. */
/* $Id: ftoa_engine.S,v 1.3 2009/04/01 23:11:00 arcanum Exp $ */
#include <AP_HAL_Boards.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
#ifndef __DOXYGEN__
#include "macros.inc"
#include "ftoa_engine.h"
#if defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__
# define AVR_ENH_LPM 1
#else
# define AVR_ENH_LPM 0
#endif
/*
int __ftoa_engine (double val, char *buf,
unsigned char prec, unsigned char maxdgs)
Input:
val - value to convert
buf - output buffer address
prec - precision: number of decimal digits is 'prec + 1'
maxdgs - (0 if unused) precision restriction for "%f" specification
Output:
return - decimal exponent of first digit
buf[0] - flags (FTOA_***)
buf[1],... - decimal digits
Number of digits:
maxdgs == 0 ? prec+1 :
(buf[0] & FTOA_CARRY) == 0 || buf[1] != '1' ?
aver(1, maxdgs+exp, prec+1) :
aver(1, masdgs+exp-1, prec+1)
Notes:
* Output string is not 0-terminated. For possibility of user's buffer
usage in any case.
* If used, 'maxdgs' is a number of digits for value with zero exponent.
*/
/* Input */
#define maxdgs r16
#define prec r18
#define buf_lo r20
#define buf_hi r21
#define val_lo r22
#define val_hi r23
#define val_hlo r24
#define val_hhi r25
/* Float value parse */
#define flag r19
/* Multiplication of mantisses */
#define exp_sv r17
#define mlt_1 r19 /* lowest result byte */
#define mlt_2 r14
#define mlt_3 r15
#define mlt_4 r20
#define mlt_5 r21
#define mlt_6 r28
#define mlt_7 r29
/* Conversion to string */
#define pwr_2 r1 /* lowest byte of 'powr10' element */
#define pwr_3 r17
#define pwr_4 r19
#define pwr_5 r22
#define pwr_6 r25
#define pwr_7 r0
#define digit r23
#define exp10 r24
/* Fixed */
#define zero r1
/* ASSEMBLY_CLIB_SECTION */
.global __ftoa_engine
.type __ftoa_engine, "function"
__ftoa_engine:
/* --------------------------------------------------------------------
Float value parse.
*/
; limit 'prec'
cpi prec, 8
brlo 1f
ldi prec, 7
1:
; init.
clr flag
X_movw XL, buf_lo
; val_hhi := exponent, sign test and remove
#if FTOA_MINUS != 1
# error FTOA_MINUS must be 1: add with carry used
#endif
lsl val_hhi
adc flag, zero ; FTOA_MINUS
sbrc val_hlo, 7
ori val_hhi, 1
; zero test
adiw val_hlo, 0
cpc val_lo, zero
cpc val_hi, zero
brne 3f
; return 0
ori flag, FTOA_ZERO
subi prec, -2
2: st X+, flag
ldi flag, '0'
dec prec
brne 2b
ret ; r24,r25 == 0
3:
; infinity, NaN ?
#if FTOA_NAN != 2 * FTOA_INF
# error Must: FTOA_NAN == 2*FTOA_INF: 'rjmp' is absent
#endif
cpi val_hhi, 0xff
brlo 6f
cpi val_hlo, 0x80
cpc val_hi, zero
cpc val_lo, zero
breq 5f
subi flag, -FTOA_INF ; FTOA_NAN
5: subi flag, -FTOA_INF
6:
; write flags byte
st X+, flag
; hidden bit
cpi val_hhi, 1
brlo 7f ; if subnormal value
ori val_hlo, 0x80
7: adc val_hhi, zero
; pushes
push r29
push r28
push r17
push r16
push r15
push r14
/* --------------------------------------------------------------------
Multiplication of mantisses (val and table).
At the begin:
val_hlo .. val_lo - input value mantisse
val_hhi - input value exponent
X - second byte address (string begin)
At the end:
mlt_7 .. mlt_2 - multiplication result
exp10 - decimal exponent
*/
; save
mov exp_sv, val_hhi
; Z := & base10[exp / 8] (sizeof(base10[0]) == 5)
andi val_hhi, ~7
lsr val_hhi ; (exp/8) * 4
mov ZL, val_hhi
lsr val_hhi
lsr val_hhi ; exp/8
add ZL, val_hhi ; (exp/8) * 5
clr ZH
subi ZL, lo8(-(.L_base10))
sbci ZH, hi8(-(.L_base10))
; highest mantissa byte (mult. shifting prepare)
clr val_hhi
; result initializ.
clr mlt_1
clr mlt_2
clr mlt_3
X_movw mlt_4, mlt_2
X_movw mlt_6, mlt_2
; multiply to 1-st table byte
#if AVR_ENH_LPM
lpm r0, Z+
#else
lpm
adiw ZL, 1
#endif
sec ; for loop end control
ror r0
; addition
10: brcc 11f
add mlt_1, val_lo
adc mlt_2, val_hi
adc mlt_3, val_hlo
adc mlt_4, val_hhi
adc mlt_5, zero
; arg shift
11: lsl val_lo
rol val_hi
rol val_hlo
rol val_hhi
; next bit
lsr r0
brne 10b
; second table byte
#if AVR_ENH_LPM
lpm r0, Z+ ; C flag is stay 1
#else
lpm
adiw ZL, 1
sec
#endif
ror r0
; addition
12: brcc 13f
add mlt_2, val_hi ; val_hi is the least byte now
adc mlt_3, val_hlo
adc mlt_4, val_hhi
adc mlt_5, val_lo
adc mlt_6, zero
; arg shift
13: lsl val_hi
rol val_hlo
rol val_hhi
rol val_lo
; next bit
lsr r0
brne 12b
; 3-t table byte
#if AVR_ENH_LPM
lpm r0, Z+ ; C flag is stay 1
#else
lpm
adiw ZL, 1
sec
#endif
ror r0
; addition
14: brcc 15f
add mlt_3, val_hlo ; val_hlo is the least byte now
adc mlt_4, val_hhi
adc mlt_5, val_lo
adc mlt_6, val_hi
adc mlt_7, zero
; arg shift
15: lsl val_hlo
rol val_hhi
rol val_lo
rol val_hi
; next bit
lsr r0
brne 14b
; 4-t table byte
#if AVR_ENH_LPM
lpm r0, Z+ ; C flag is stay 1
#else
lpm
#endif
ror r0
; addition
16: brcc 17f
add mlt_4, val_hhi ; val_hhi is the least byte now
adc mlt_5, val_lo
adc mlt_6, val_hi
adc mlt_7, val_hlo
; arg shift
17: lsl val_hhi
rol val_lo
rol val_hi
rol val_hlo
; next bit
lsr r0
brne 16b
; decimal exponent
#if AVR_ENH_LPM
lpm exp10, Z
#else
adiw ZL, 1
lpm
mov exp10, r0
#endif
; result shift: mlt_7..2 >>= (~exp & 7)
com exp_sv
andi exp_sv, 7
breq 19f
18: lsr mlt_7
ror mlt_6
ror mlt_5
ror mlt_4
ror mlt_3
ror mlt_2
dec exp_sv
brne 18b
19:
/* --------------------------------------------------------------------
Conversion to string.
Registers usage:
mlt_7 .. mlt_2 - new mantissa (multiplication result)
pwr_7 .. pwr_2 - 'powr10' table element
Z - 'powr10' table pointer
X - output string pointer
maxdgs - number of digits
prec - number of digits stays to output
exp10 - decimal exponent
digit - conversion process
At the end:
X - end of buffer (nonfilled byte)
exp10 - corrected dec. exponent
mlt_7 .. mlt_2 - remainder
pwr_7 .. pwr_2 - last powr10[] element
Notes:
* It is possible to leave out powr10'x table with subnormal value.
Result: accuracy degrease on the rounding phase. No matter: high
precision with subnormals is not needed. (Now 0x00000001 is converted
exactly on prec = 5, i.e. 6 digits.)
*/
; to find first digit
ldi ZL, lo8(.L_powr10)
ldi ZH, hi8(.L_powr10)
set
; 'pwr10' element reading
.L_digit:
X_lpm pwr_2, Z+
X_lpm pwr_3, Z+
X_lpm pwr_4, Z+
X_lpm pwr_5, Z+
X_lpm pwr_6, Z+
X_lpm pwr_7, Z+
; 'digit' init.
ldi digit, '0' - 1
; subtraction loop
20: inc digit
sub mlt_2, pwr_2
sbc mlt_3, pwr_3
sbc mlt_4, pwr_4
sbc mlt_5, pwr_5
sbc mlt_6, pwr_6
sbc mlt_7, pwr_7
brsh 20b
; restore mult
add mlt_2, pwr_2
adc mlt_3, pwr_3
adc mlt_4, pwr_4
adc mlt_5, pwr_5
adc mlt_6, pwr_6
adc mlt_7, pwr_7
; analisys
brtc 25f
cpi digit, '0'
brne 21f ; this is the first digit finded
dec exp10
rjmp .L_digit
; now is the first digit
21: clt
; number of digits
subi maxdgs, 1
brlo 23f ; maxdgs was 0
add maxdgs, exp10
brpl 22f
clr maxdgs
22: cp maxdgs, prec
brsh 23f
mov prec, maxdgs
23: inc prec
mov maxdgs, prec
; operate digit
25: cpi digit, '0' + 10
brlo 27f
; overflow, digit > '9'
ldi digit, '9'
26: st X+, digit
dec prec
brne 26b
rjmp .L_up
; write digit
27: st X+, digit
dec prec
brne .L_digit
/* --------------------------------------------------------------------
Rounding.
*/
.L_round:
; pwr10 /= 2
lsr pwr_7
ror pwr_6
ror pwr_5
ror pwr_4
ror pwr_3
ror pwr_2
; mult -= pwr10 (half of last 'pwr10' value)
sub mlt_2, pwr_2
sbc mlt_3, pwr_3
sbc mlt_4, pwr_4
sbc mlt_5, pwr_5
sbc mlt_6, pwr_6
sbc mlt_7, pwr_7
; rounding direction?
brlo .L_rest
; round to up
.L_up:
inc prec
ld digit, -X
inc digit
cpi digit, '9' + 1
brlo 31f
ldi digit, '0'
31: st X, digit
cpse prec, maxdgs
brsh .L_up
; it was a carry to master digit
ld digit, -X ; flags
ori digit, FTOA_CARRY ; 'C' is not changed
st X+, digit
brlo .L_rest ; above comparison
; overflow
inc exp10
ldi digit, '1'
32: st X+, digit
ldi digit, '0'
dec prec
brne 32b
; restore
.L_rest:
clr zero
pop r14
pop r15
pop r16
pop r17
pop r28
pop r29
; return
clr r25
sbrc exp10, 7 ; high byte
com r25
ret
.size __ftoa_engine, . - __ftoa_engine
/* --------------------------------------------------------------------
Tables. '.L_powr10' is placed first -- for subnormals stability.
*/
.section .progmem.data,"a",@progbits
.type .L_powr10, "object"
.L_powr10:
.byte 0, 64, 122, 16, 243, 90 ; 100000000000000
.byte 0, 160, 114, 78, 24, 9 ; 10000000000000
.byte 0, 16, 165, 212, 232, 0 ; 1000000000000
.byte 0, 232, 118, 72, 23, 0 ; 100000000000
.byte 0, 228, 11, 84, 2, 0 ; 10000000000
.byte 0, 202, 154, 59, 0, 0 ; 1000000000
.byte 0, 225, 245, 5, 0, 0 ; 100000000
.byte 128, 150, 152, 0, 0, 0 ; 10000000
.byte 64, 66, 15, 0, 0, 0 ; 1000000
.byte 160, 134, 1, 0, 0, 0 ; 100000
.byte 16, 39, 0, 0, 0, 0 ; 10000
.byte 232, 3, 0, 0, 0, 0 ; 1000
.byte 100, 0, 0, 0, 0, 0 ; 100
.byte 10, 0, 0, 0, 0, 0 ; 10
.byte 1, 0, 0, 0, 0, 0 ; 1
.size .L_powr10, . - .L_powr10
.type .L_base10, "object"
.L_base10:
.byte 44, 118, 216, 136, -36 ; 2295887404
.byte 103, 79, 8, 35, -33 ; 587747175
.byte 193, 223, 174, 89, -31 ; 1504632769
.byte 177, 183, 150, 229, -29 ; 3851859889
.byte 228, 83, 198, 58, -26 ; 986076132
.byte 81, 153, 118, 150, -24 ; 2524354897
.byte 230, 194, 132, 38, -21 ; 646234854
.byte 137, 140, 155, 98, -19 ; 1654361225
.byte 64, 124, 111, 252, -17 ; 4235164736
.byte 188, 156, 159, 64, -14 ; 1084202172
.byte 186, 165, 111, 165, -12 ; 2775557562
.byte 144, 5, 90, 42, -9 ; 710542736
.byte 92, 147, 107, 108, -7 ; 1818989404
.byte 103, 109, 193, 27, -4 ; 465661287
.byte 224, 228, 13, 71, -2 ; 1192092896
.byte 245, 32, 230, 181, 0 ; 3051757813
.byte 208, 237, 144, 46, 3 ; 781250000
.byte 0, 148, 53, 119, 5 ; 2000000000
.byte 0, 128, 132, 30, 8 ; 512000000
.byte 0, 0, 32, 78, 10 ; 1310720000
.byte 0, 0, 0, 200, 12 ; 3355443200
.byte 51, 51, 51, 51, 15 ; 858993459
.byte 152, 110, 18, 131, 17 ; 2199023256
.byte 65, 239, 141, 33, 20 ; 562949953
.byte 137, 59, 230, 85, 22 ; 1441151881
.byte 207, 254, 230, 219, 24 ; 3689348815
.byte 209, 132, 75, 56, 27 ; 944473297
.byte 247, 124, 29, 144, 29 ; 2417851639
.byte 164, 187, 228, 36, 32 ; 618970020
.byte 50, 132, 114, 94, 34 ; 1584563250
.byte 129, 0, 201, 241, 36 ; 4056481921
.byte 236, 161, 229, 61, 39 ; 1038459372
.size .L_base10, . - .L_base10
.end
#endif /* !__DOXYGEN__ */
#endif

230
libraries/AP_HAL_AVR/utility/ftoa_engine.cpp Normal file
View File

@ -0,0 +1,230 @@
/* Copyright (c) 2005, Dmitry Xmelkov
All rights reserved.
Rewritten in C by Soren Kuula
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. */
#include <AP_HAL.h>
#include <AP_Common.h>
#include "ftoa_engine.h"
#include <stdint.h>
#define PGM_INT8(addr) (int8_t)pgm_read_byte((const prog_char *)addr)
#define PGM_UINT32(addr) pgm_read_dword((const uint32_t *)addr)
/*
* 2^b ~= f * r * 10^e
* where
* i = b div 8
* r = 2^(b mod 8)
* f = factorTable[i]
* e = exponentTable[i]
*/
static const int8_t exponentTable[32] PROGMEM = {
-36, -33, -31, -29, -26, -24, -21, -19,
-17, -14, -12, -9, -7, -4, -2, 0,
3, 5, 8, 10, 12, 15, 17, 20,
22, 24, 27, 29, 32, 34, 36, 39
};
static const uint32_t factorTable[32] PROGMEM = {
2295887404UL,
587747175UL,
1504632769UL,
3851859889UL,
986076132UL,
2524354897UL,
646234854UL,
1654361225UL,
4235164736UL,
1084202172UL,
2775557562UL,
710542736UL,
1818989404UL,
465661287UL,
1192092896UL,
3051757813UL,
781250000UL,
2000000000UL,
512000000UL,
1310720000UL,
3355443200UL,
858993459UL,
2199023256UL,
562949953UL,
1441151881UL,
3689348815UL,
944473297UL,
2417851639UL,
618970020UL,
1584563250UL,
4056481921UL,
1038459372UL
};
int16_t ftoa_engine(float val, char *buf, uint8_t precision, uint8_t maxDecimals)
{
uint8_t flags;
// Bit reinterpretation hacks. This will ONLY work on little endian machines.
uint8_t *valbits = (uint8_t*)&val;
union {
float v;
uint32_t u;
} x;
x.v = val;
uint32_t frac = x.u & 0x007fffffUL;
if (precision>7) precision=7;
// Read the sign, shift the exponent in place and delete it from frac.
if (valbits[3] & (1<<7)) flags = FTOA_MINUS; else flags = 0;
uint8_t exp = valbits[3]<<1;
if(valbits[2] & (1<<7)) exp++; // TODO possible but in case of subnormal
// Test for easy cases, zero and NaN
if(exp==0 && frac==0) {
buf[0] = flags | FTOA_ZERO;
uint8_t i;
for(i=0; i<=precision; i++) {
buf[i+1] = '0';
}
return 0;
}
if(exp == 0xff) {
if(frac == 0) flags |= FTOA_INF; else flags |= FTOA_NAN;
}
// The implicit leading 1 is made explicit, except if value subnormal.
if (exp != 0) frac |= (1UL<<23);
uint8_t idx = exp>>3;
int8_t exp10 = PGM_INT8(&exponentTable[idx]);
// We COULD try making the multiplication in situ, where we make
// frac and a 64 bit int overlap in memory and select/weigh the
// upper 32 bits that way. For starters, this is less risky:
int64_t prod = (int64_t)frac * (int64_t)PGM_UINT32(&factorTable[idx]);
// The expConvFactorTable are factor are correct iff the lower 3 exponent
// bits are 1 (=7). Else we need to compensate by divding frac.
// If the lower 3 bits are 7 we are right.
// If the lower 3 bits are 6 we right-shift once
// ..
// If the lower 3 bits are 0 we right-shift 7x
prod >>= (15-(exp & 7));
// Now convert to decimal.
uint8_t hadNonzeroDigit = 0; // a flag
uint8_t outputIdx = 0;
int64_t decimal = 100000000000000ull;
do {
char digit = '0';
while(1) {// find the first nonzero digit or any of the next digits.
while ((prod -= decimal) >= 0)
digit++;
// Now we got too low. Fix it by adding again, once.
// it might appear more efficient to check before subtract, or
// to save and restore last nonnegative value - but in fact
// they take as long time and more space.
prod += decimal;
decimal /= 10;
// If already found a leading nonzero digit, accept zeros.
if (hadNonzeroDigit) break;
// Else, don't return results with a leading zero! Instead
// skip those and decrement exp10 accordingly.
if(digit == '0') {
exp10--;
continue;
}
hadNonzeroDigit = 1;
// Compute how many digits N to output.
if(maxDecimals != 0) { // If limiting decimals...
int8_t beforeDP = exp10+1; // Digits before point
if (beforeDP < 1) beforeDP = 1; // Numbers < 1 should also output at least 1 digit.
/*
* Below a simpler version of this:
int8_t afterDP = outputNum - beforeDP;
if (afterDP > maxDecimals-1)
afterDP = maxDecimals-1;
outputNum = beforeDP + afterDP;
*/
maxDecimals = maxDecimals+beforeDP-1;
if (precision > maxDecimals)
precision = maxDecimals;
} else {
precision++; // Output one more digit than the param value.
}
break;
}
// Now have a digit.
outputIdx++;
if(digit < '0' + 10) // normal case.
buf[outputIdx] = digit;
else {
// Abnormal case, write 9s and bail.
// We might as well abuse hadNonzeroDigit as counter, it will not be used again.
for(hadNonzeroDigit=outputIdx; hadNonzeroDigit>0; hadNonzeroDigit--)
buf[hadNonzeroDigit] = '9';
goto roundup; // this is ugly but it _is_ code derived from assembler :)
}
} while (outputIdx<precision);
// Rounding:
decimal *= 10;
if (prod - (decimal >> 1) >= 0) {
roundup:
// Increment digit, cascade
while(outputIdx != 0) {
if(++buf[outputIdx] == '0' + 10) {
if(outputIdx == 1) {
buf[outputIdx] = '1';
exp10++;
flags |= FTOA_CARRY;
break;
} else
buf[outputIdx--] = '0'; // and the loop continues, carrying to next digit.
}
else break;
}
}
buf[0] = flags;
return exp10;
}

9
libraries/AP_HAL_AVR/utility/ftoa_engine.h
View File

@ -31,12 +31,11 @@
#ifndef _FTOA_ENGINE_H
#define _FTOA_ENGINE_H
#ifndef __ASSEMBLER__
#include <AP_HAL.h>
#include <stdint.h>
int __ftoa_engine (double val, char *buf,
unsigned char prec, unsigned char maxdgs);
#endif
int16_t ftoa_engine(float val, char *buf,
uint8_t precision, uint8_t maxDecimals);
/* '__ftoa_engine' return next flags (in buf[0]): */
#define FTOA_MINUS 1

10
libraries/AP_HAL_AVR/utility/print_vprintf.cpp
View File

@ -44,11 +44,9 @@
#include <stdarg.h>
#include <string.h>
extern "C" {
#include "ftoa_engine.h"
#include "ntz.h"
#include "xtoa_fast.h"
}
#include "ntz.h"
#include "print_vprintf.h"
@ -219,7 +217,7 @@ void print_vprintf (AP_HAL::Print *s, unsigned char in_progmem, const char *fmt,
vtype = prec;
ndigs = 0;
}
exp = __ftoa_engine (va_arg(ap,double), (char *)buf, vtype, ndigs);
exp = ftoa_engine(va_arg(ap,double), (char *)buf, vtype, ndigs);
vtype = buf[0];
sign = 0;
@ -407,7 +405,7 @@ void print_vprintf (AP_HAL::Print *s, unsigned char in_progmem, const char *fmt,
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;
@ -437,7 +435,7 @@ 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)
c = ultoa_invert ((flags & FL_LONG)
? va_arg(ap, unsigned long)
: va_arg(ap, unsigned int),
(char *)buf, base) - (char *)buf;

220
libraries/AP_HAL_AVR/utility/ultoa_invert.S
View File

@ -1,220 +0,0 @@
/* Copyright (c) 2005,2007 Dmitry Xmelkov
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. */
/* $Id: ultoa_invert.S 1944 2009-04-01 23:12:20Z arcanum $ */
#ifndef __DOXYGEN__
#include <AP_HAL_Boards.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
#include "macros.inc"
#include "ntz.h"
#include "xtoa_fast.h"
/* --------------------------------------------------------------------
char * __ultoa_invert (unsigned long val, char * str, int base)
This function is intended for usage as internal printf's one.
It differs from others of `xtoa_fast' family:
* srt[] will NOT 0 terminated.
* Sequence of digits is inverted.
* It returns pointer to first byte after a string.
* Only `XTOA_UPPER' flag is operated.
Notes:
* base: check only 8 and 16, all others are treated as 10.
(internal printf's function).
*/
/* Input */
#define v_lo r22
#define v_hi r23
#define v_hlo r24
#define v_hhi r25
#define str_lo r20
#define str_hi r21
#define base r18
#define flags r19
/* Used */
#define v_fifth r26 /* val: bits 39..32 */
#define t_lo r18 /* temporary for shifted `val' */
#define t_hi r19
#define t_hlo r20
#define t_hhi r21
#define symb r20 /* write to string */
#define cnt r27 /* shift loop counter, local arg */
/* Fixed */
#define rzero r1
/* ASSEMBLY_CLIB_SECTION */
.global __ultoa_invert
.type __ultoa_invert, "function"
__ultoa_invert:
X_movw ZL, str_lo
clr v_fifth ; needed for all (ultoa_lsr)
cpi base, 8
breq .L_oct
cpi base, 16
breq .L_hex
; decimal format
clt ; flag of val == 0
.L_dec_loop:
push v_lo ; to calculate remander
; val &= ~1
andi v_lo, ~1
; val += 2
subi v_lo, lo8(-2)
sbci v_hi, hi8(-2)
sbci v_hlo, hlo8(-2)
sbci v_hhi, hhi8(-2)
sbci v_fifth, hhi8(-2)
; val += val/2
ldi cnt, 1
rcall .L_div_add
; val += val/16
ldi cnt, 4
rcall .L_div_add
; val += val/256
add v_lo, v_hi
adc v_hi, v_hlo
adc v_hlo, v_hhi
adc v_hhi, v_fifth
adc v_fifth, rzero
; val += val/65536
add v_lo, v_hlo
adc v_hi, v_hhi
adc v_hlo, v_fifth
adc v_hhi, rzero
adc v_fifth, rzero
; val += val >> 32
add v_lo, v_fifth
adc v_hi, rzero
adc v_hlo, rzero
adc v_hhi, rzero
adc v_fifth, rzero
; division result: val /= 16
rcall .L_lsr_4 ; v_fitth := 0
brne 1f
set ; T := Z flag
1:
; rem: val_original - 10*val
pop t_hi
#if defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
ldi t_lo, 10
mul t_lo, v_lo
clr r1
#else
mov r0, v_lo
lsl r0
sub t_hi, r0
lsl r0
lsl r0
#endif
sub t_hi, r0
; output digit
subi t_hi, lo8(-'0')
st Z+, t_hi
; quotient == 0 ?
brtc .L_dec_loop
; end of string
.L_eos:
X_movw r24, ZL
ret
; octal format
.L_oct:
mov symb, v_lo
andi symb, 7
subi symb, lo8(-'0')
st Z+, symb
ldi cnt, 3
rcall .L_lsr
brne .L_oct
rjmp .L_eos
; hex format
.L_hex:
mov symb, v_lo
andi symb, 0x0f
subi symb, lo8(-'0')
cpi symb, '9' + 1
brlo 3f
subi symb, lo8('9' + 1 - 'a')
sbrc flags, ntz(XTOA_UPPER) - 8
subi symb, lo8('a' - 'A')
3: st Z+, symb
rcall .L_lsr_4
brne .L_hex
rjmp .L_eos
.L_lsr_4:
ldi cnt, 4
.L_lsr:
lsr v_fifth
ror v_hhi
ror v_hlo
ror v_hi
ror v_lo
dec cnt
brne .L_lsr
; tst
sbiw v_hlo, 0 ; only Z flag is needed
cpc v_lo, rzero
cpc v_hi, rzero
ret
.L_div_add:
; copy to temporary
X_movw t_lo, v_lo
X_movw t_hlo, v_hlo
mov r0, v_fifth
; lsr temporary
7: lsr r0
ror t_hhi
ror t_hlo
ror t_hi
ror t_lo
dec cnt
brne 7b
; add
add v_lo, t_lo
adc v_hi, t_hi
adc v_hlo, t_hlo
adc v_hhi, t_hhi
adc v_fifth, r0 ; here r0 == 0
ret
.size __ultoa_invert, . - __ultoa_invert
.end
#endif /* !__DOXYGEN__ */
#endif

77
libraries/AP_HAL_AVR/utility/utoa_invert.cpp Normal file
View File

@ -0,0 +1,77 @@
/* Copyright (c) 2005, Dmitry Xmelkov
All rights reserved.
Rewritten in C by Soren Kuula
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. */
#include <stdint.h>
#include "xtoa_fast.h"
char * ultoa_invert (uint32_t val, char *s, int 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.
// 33 bits would have been enough.
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;
}

6
libraries/AP_HAL_AVR/utility/xtoa_fast.h
View File

@ -34,12 +34,8 @@
#ifndef _XTOA_FAST_H_
#define _XTOA_FAST_H_
#ifndef __ASSEMBLER__
/* Internal function for use from `printf'. */
char * __ultoa_invert (unsigned long val, char *s, int base);
#endif /* ifndef __ASSEMBLER__ */
char *ultoa_invert (unsigned long val, char *s, int base);
/* Next flags are to use with `base'. Unused fields are reserved. */
#define XTOA_PREFIX 0x0100 /* put prefix for octal or hex */