Line data Source code
1 : /* Copyright (c) 2005, Dmitry Xmelkov
2 : All rights reserved.
3 :
4 : Rewritten in C by Soren Kuula
5 :
6 : Redistribution and use in source and binary forms, with or without
7 : modification, are permitted provided that the following conditions are met:
8 :
9 : * Redistributions of source code must retain the above copyright
10 : notice, this list of conditions and the following disclaimer.
11 : * Redistributions in binary form must reproduce the above copyright
12 : notice, this list of conditions and the following disclaimer in
13 : the documentation and/or other materials provided with the
14 : distribution.
15 : * Neither the name of the copyright holders nor the names of
16 : contributors may be used to endorse or promote products derived
17 : from this software without specific prior written permission.
18 :
19 : THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20 : AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 : IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22 : ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23 : LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24 : CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 : SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 : INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 : CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 : ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 : POSSIBILITY OF SUCH DAMAGE. */
30 :
31 : #include "ftoa_engine.h"
32 :
33 : #include <stdint.h>
34 :
35 : #include <AP_Common/AP_Common.h>
36 : #include <AP_HAL/AP_HAL.h>
37 :
38 : /*
39 : * 2^b ~= f * r * 10^e
40 : * where
41 : * i = b div 8
42 : * r = 2^(b mod 8)
43 : * f = factorTable[i]
44 : * e = exponentTable[i]
45 : */
46 : static const int8_t exponentTable[32] = {
47 : -36, -33, -31, -29, -26, -24, -21, -19,
48 : -17, -14, -12, -9, -7, -4, -2, 0,
49 : 3, 5, 8, 10, 12, 15, 17, 20,
50 : 22, 24, 27, 29, 32, 34, 36, 39
51 : };
52 :
53 : static const uint32_t factorTable[32] = {
54 : 2295887404UL,
55 : 587747175UL,
56 : 1504632769UL,
57 : 3851859889UL,
58 : 986076132UL,
59 : 2524354897UL,
60 : 646234854UL,
61 : 1654361225UL,
62 : 4235164736UL,
63 : 1084202172UL,
64 : 2775557562UL,
65 : 710542736UL,
66 : 1818989404UL,
67 : 465661287UL,
68 : 1192092896UL,
69 : 3051757813UL,
70 : 781250000UL,
71 : 2000000000UL,
72 : 512000000UL,
73 : 1310720000UL,
74 : 3355443200UL,
75 : 858993459UL,
76 : 2199023256UL,
77 : 562949953UL,
78 : 1441151881UL,
79 : 3689348815UL,
80 : 944473297UL,
81 : 2417851639UL,
82 : 618970020UL,
83 : 1584563250UL,
84 : 4056481921UL,
85 : 1038459372UL
86 : };
87 :
88 66763 : int16_t ftoa_engine(float val, char *buf, uint8_t precision, uint8_t maxDecimals)
89 : {
90 : uint8_t flags;
91 :
92 : // Bit reinterpretation hacks. This will ONLY work on little endian machines.
93 66763 : uint8_t *valbits = (uint8_t*)&val;
94 : union {
95 : float v;
96 : uint32_t u;
97 : } x;
98 66763 : x.v = val;
99 66763 : uint32_t frac = x.u & 0x007fffffUL;
100 :
101 66763 : if (precision>7) precision=7;
102 :
103 : // Read the sign, shift the exponent in place and delete it from frac.
104 66763 : if (valbits[3] & (1<<7)) flags = FTOA_MINUS; else flags = 0;
105 66763 : uint8_t exp = valbits[3]<<1;
106 66763 : if(valbits[2] & (1<<7)) exp++;
107 :
108 : // Test for easy cases
109 66763 : if(exp==0) { // Zero or subnormal
110 31528 : if(frac == 0) { // Zero
111 31522 : buf[0] = flags | FTOA_ZERO;
112 : uint8_t i;
113 283698 : for(i=0; i<=precision; i++) {
114 252176 : buf[i+1] = '0';
115 : }
116 31522 : return 0;
117 : } else { // Subnormal
118 6 : exp = 1; // Subnormal mantissa has same significance as exp=1
119 : }
120 : } else {
121 35235 : if(exp==0xff) { // Infinity or NaN
122 38 : if(frac==0) flags |= FTOA_INF; else flags |= FTOA_NAN;
123 : } // Else normal number
124 : // Note this ensures infinities have a non-zero frac and don't hang!
125 35235 : frac |= (1UL<<23); // The implicit leading 1 is made explicit.
126 : }
127 :
128 35241 : uint8_t idx = exp>>3;
129 35241 : int8_t exp10 = exponentTable[idx];
130 :
131 : // We COULD try making the multiplication in situ, where we make
132 : // frac and a 64 bit int overlap in memory and select/weigh the
133 : // upper 32 bits that way. For starters, this is less risky:
134 35241 : int64_t prod = (int64_t)frac * (int64_t)factorTable[idx];
135 :
136 : // The expConvFactorTable are factor are correct iff the lower 3 exponent
137 : // bits are 1 (=7). Else we need to compensate by dividing frac.
138 : // If the lower 3 bits are 7 we are right.
139 : // If the lower 3 bits are 6 we right-shift once
140 : // ..
141 : // If the lower 3 bits are 0 we right-shift 7x
142 35241 : prod >>= (15-(exp & 7));
143 :
144 : // Now convert to decimal.
145 35241 : uint8_t hadNonzeroDigit = 0; // a flag
146 35241 : uint8_t outputIdx = 0;
147 35241 : int64_t decimal = 100000000000000ull;
148 :
149 : do {
150 120706 : char digit = '0';
151 120706 : if(decimal == 0) {
152 : // Abnormal case, can't ever add enough digits to exit the loop,
153 : // bail out. This will happen for subnormals with frac <= 7 and
154 : // precision == 7.
155 2 : break;
156 : }
157 : while(1) {// find the first nonzero digit or any of the next digits.
158 662275 : while ((prod -= decimal) >= 0)
159 477777 : digit++;
160 : // Now we got too low. Fix it by adding again, once.
161 : // it might appear more efficient to check before subtract, or
162 : // to save and restore last nonnegative value - but in fact
163 : // they take as long time and more space.
164 184498 : prod += decimal;
165 184498 : decimal /= 10;
166 :
167 : // If already found a leading nonzero digit, accept zeros.
168 184498 : if (hadNonzeroDigit) break;
169 :
170 : // Else, don't return results with a leading zero! Instead
171 : // skip those and decrement exp10 accordingly.
172 99035 : if(digit == '0') {
173 63794 : exp10--;
174 63794 : continue;
175 : }
176 :
177 35241 : hadNonzeroDigit = 1;
178 :
179 : // Compute how many digits N to output.
180 35241 : if(maxDecimals != 0) { // If limiting decimals...
181 35209 : int8_t beforeDP = exp10+1; // Digits before point
182 35209 : if (beforeDP < 1) beforeDP = 1; // Numbers < 1 should also output at least 1 digit.
183 : /*
184 : * Below a simpler version of this:
185 : int8_t afterDP = outputNum - beforeDP;
186 : if (afterDP > maxDecimals-1)
187 : afterDP = maxDecimals-1;
188 : outputNum = beforeDP + afterDP;
189 : */
190 35209 : maxDecimals = maxDecimals+beforeDP-1;
191 35209 : if (precision > maxDecimals)
192 32365 : precision = maxDecimals;
193 :
194 : } else {
195 32 : precision++; // Output one more digit than the param value.
196 : }
197 :
198 35241 : break;
199 63794 : }
200 :
201 : // Now have a digit.
202 120704 : outputIdx++;
203 120704 : if(digit < '0' + 10) // normal case.
204 120704 : buf[outputIdx] = digit;
205 : else {
206 : // Abnormal case, write 9s and bail.
207 : // We might as well abuse hadNonzeroDigit as counter, it will not be used again.
208 0 : for(hadNonzeroDigit=outputIdx; hadNonzeroDigit>0; hadNonzeroDigit--)
209 0 : buf[hadNonzeroDigit] = '9';
210 0 : goto roundup; // this is ugly but it _is_ code derived from assembler :)
211 : }
212 120704 : } while (outputIdx<precision);
213 :
214 : // Rounding:
215 35241 : decimal *= 10;
216 :
217 35241 : if (prod - (decimal >> 1) >= 0) {
218 :
219 16763 : roundup:
220 : // Increment digit, cascade
221 20127 : while(outputIdx != 0) {
222 20127 : if(++buf[outputIdx] == '0' + 10) {
223 3922 : if(outputIdx == 1) {
224 558 : buf[outputIdx] = '1';
225 558 : exp10++;
226 558 : flags |= FTOA_CARRY;
227 558 : break;
228 : } else
229 3364 : buf[outputIdx--] = '0'; // and the loop continues, carrying to next digit.
230 : }
231 16205 : else break;
232 : }
233 : }
234 :
235 35241 : buf[0] = flags;
236 35241 : return exp10;
237 : }
238 :
|
