src/main/common/typeconversion.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17 #include <stdint.h>
  18 #include <string.h>
  19 #include <math.h>
  20 #include "build/build_config.h"
  21
  22 #include "maths.h"
  23 #include "platform.h"
  24 FILE_COMPILE_FOR_SPEED
  25
  26 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
  27
  28 void uli2a(unsigned long int num, unsigned int base, int uc, char *bf)
  29 {
  30     int n = 0;
  31     unsigned int d = 1;
  32     while (num / d >= base)
  33         d *= base;
  34     while (d != 0) {
  35         int dgt = num / d;
  36         num %= d;
  37         d /= base;
  38         if (n || dgt > 0 || d == 0) {
  39             *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
  40             ++n;
  41         }
  42     }
  43     *bf = 0;
  44 }
  45
  46 void li2a(long num, char *bf)
  47 {
  48     if (num < 0) {
  49         num = -num;
  50         *bf++ = '-';
  51     }
  52     uli2a(num, 10, 0, bf);
  53 }
  54
  55 #endif
  56
  57 void ui2a(unsigned int num, unsigned int base, int uc, char *bf)
  58 {
  59     int n = 0;
  60     unsigned int d = 1;
  61     while (num / d >= base)
  62         d *= base;
  63     while (d != 0) {
  64         int dgt = num / d;
  65         num %= d;
  66         d /= base;
  67         if (n || dgt > 0 || d == 0) {
  68             *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
  69             ++n;
  70         }
  71     }
  72     *bf = 0;
  73 }
  74
  75 void i2a(int num, char *bf)
  76 {
  77     if (num < 0) {
  78         num = -num;
  79         *bf++ = '-';
  80     }
  81     ui2a(num, 10, 0, bf);
  82 }
  83
  84 int a2d(char ch)
  85 {
  86     if (ch >= '0' && ch <= '9')
  87         return ch - '0';
  88     else if (ch >= 'a' && ch <= 'f')
  89         return ch - 'a' + 10;
  90     else if (ch >= 'A' && ch <= 'F')
  91         return ch - 'A' + 10;
  92     else
  93         return -1;
  94 }
  95
  96 char a2i(char ch, const char **src, int base, int *nump)
  97 {
  98     const char *p = *src;
  99     int num = 0;
 100     int digit;
 101     while ((digit = a2d(ch)) >= 0) {
 102         if (digit > base)
 103             break;
 104         num = num * base + digit;
 105         ch = *p++;
 106     }
 107     *src = p;
 108     *nump = num;
 109     return ch;
 110 }
 111
 112 #ifndef HAVE_ITOA_FUNCTION
 113
 114 /*
 115  ** The following two functions together make up an itoa()
 116  ** implementation. Function i2a() is a 'private' function
 117  ** called by the public itoa() function.
 118  **
 119  ** itoa() takes three arguments:
 120  **        1) the integer to be converted,
 121  **        2) a pointer to a character conversion buffer,
 122  **        3) the radix for the conversion
 123  **           which can range between 2 and 36 inclusive
 124  **           range errors on the radix default it to base10
 125  ** Code from http://groups.google.com/group/comp.lang.c/msg/66552ef8b04fe1ab?pli=1
 126  */
 127
 128 static char *_i2a(unsigned i, char *a, unsigned base)
 129 {
 130     if (i / base > 0)
 131         a = _i2a(i / base, a, base);
 132     *a = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"[i % base];
 133     return a + 1;
 134 }
 135
 136 char *itoa(int i, char *a, int base)
 137 {
 138     if ((base < 2) || (base > 36))
 139         base = 10;
 140     if (i < 0) {
 141         *a = '-';
 142         *_i2a(-(unsigned) i, a + 1, base) = 0;
 143     } else
 144         *_i2a(i, a, base) = 0;
 145     return a;
 146 }
 147
 148 #endif
 149
 150 char *ftoa(float x, char *floatString)
 151 {
 152     int32_t value;
 153     char intString1[12];
 154     char intString2[12] = { 0, };
 155     char *decimalPoint = ".";
 156     uint8_t dpLocation;
 157
 158     if (x > 0)                  // Rounding for x.xxx display format
 159         x += 0.0005f;
 160     else
 161         x -= 0.0005f;
 162
 163     value = (int32_t)(x * 1000.0f);    // Convert float * 1000 to an integer
 164
 165     itoa(ABS(value), intString1, 10);   // Create string from abs of integer value
 166
 167     if (value >= 0)
 168         intString2[0] = ' ';    // Positive number, add a pad space
 169     else
 170         intString2[0] = '-';    // Negative number, add a negative sign
 171
 172     if (strlen(intString1) == 1) {
 173         intString2[1] = '0';
 174         intString2[2] = '0';
 175         intString2[3] = '0';
 176         strcat(intString2, intString1);
 177     } else if (strlen(intString1) == 2) {
 178         intString2[1] = '0';
 179         intString2[2] = '0';
 180         strcat(intString2, intString1);
 181     } else if (strlen(intString1) == 3) {
 182         intString2[1] = '0';
 183         strcat(intString2, intString1);
 184     } else {
 185         strcat(intString2, intString1);
 186     }
 187
 188     dpLocation = strlen(intString2) - 3;
 189
 190     memcpy(floatString, intString2, dpLocation);
 191     floatString[dpLocation] = '\0';
 192     strcat(floatString, decimalPoint);
 193     strcat(floatString, intString2 + dpLocation);
 194
 195     return floatString;
 196 }
 197
 198 // Simple and fast atof (ascii to float) function.
 199 //
 200 // - Executes about 5x faster than standard MSCRT library atof().
 201 // - An attractive alternative if the number of calls is in the millions.
 202 // - Assumes input is a proper integer, fraction, or scientific format.
 203 // - Matches library atof() to 15 digits (except at extreme exponents).
 204 // - Follows atof() precedent of essentially no error checking.
 205 //
 206 // 09-May-2009 Tom Van Baak (tvb) www.LeapSecond.com
 207 #define white_space(c) ((c) == ' ' || (c) == '\t')
 208 #define valid_digit(c) ((c) >= '0' && (c) <= '9')
 209 float fastA2F(const char *p)
 210 {
 211     int frac = 0;
 212     float sign, value, scale;
 213
 214     // Skip leading white space, if any.
 215     while (white_space(*p)) {
 216         p += 1;
 217     }
 218
 219     // Get sign, if any.
 220     sign = 1.0f;
 221     if (*p == '-') {
 222         sign = -1.0f;
 223         p += 1;
 224
 225     } else if (*p == '+') {
 226         p += 1;
 227     }
 228
 229     // Get digits before decimal point or exponent, if any.
 230     value = 0.0f;
 231     while (valid_digit(*p)) {
 232         value = value * 10.0f + (*p - '0');
 233         p += 1;
 234     }
 235
 236     // Get digits after decimal point, if any.
 237     if (*p == '.') {
 238         float pow10 = 10.0f;
 239         p += 1;
 240
 241         while (valid_digit(*p)) {
 242             value += (*p - '0') / pow10;
 243             pow10 *= 10.0f;
 244             p += 1;
 245         }
 246     }
 247
 248     // Handle exponent, if any.
 249     scale = 1.0f;
 250     if ((*p == 'e') || (*p == 'E')) {
 251         unsigned int expon;
 252         p += 1;
 253
 254         // Get sign of exponent, if any.
 255         frac = 0;
 256         if (*p == '-') {
 257             frac = 1;
 258             p += 1;
 259
 260         } else if (*p == '+') {
 261             p += 1;
 262         }
 263
 264         // Get digits of exponent, if any.
 265         expon = 0;
 266         while (valid_digit(*p)) {
 267             expon = expon * 10 + (*p - '0');
 268             p += 1;
 269         }
 270         if (expon > 308)
 271             expon = 308;
 272
 273         // Calculate scaling factor.
 274         // while (expon >= 50) { scale *= 1E50f; expon -= 50; }
 275         while (expon >= 8) {
 276             scale *= 1E8f;
 277             expon -= 8;
 278         }
 279         while (expon > 0) {
 280             scale *= 10.0f;
 281             expon -= 1;
 282         }
 283     }
 284
 285     // Return signed and scaled floating point result.
 286     return sign * (frac ? (value / scale) : (value * scale));
 287 }
 288
 289 unsigned long int fastA2UL(const char *p)
 290 {
 291     unsigned long int result = 0;
 292     unsigned char digit;
 293
 294     while (white_space(*p)) {
 295         p += 1;
 296     }
 297
 298     for ( ; ; p++) {
 299         digit = *p - '0';
 300         if (digit > 9) {
 301             break;
 302         }
 303         result *= 10;
 304         result += digit;
 305     }
 306     return result;
 307 }
 308
 309 int fastA2I(const char *s)
 310 {
 311     int sign = 1;
 312     int num = 0;
 313     int digit;
 314
 315     while (white_space(*s)) {
 316         s++;
 317     }
 318
 319     if (*s == '-') {
 320         sign = -1;
 321         s++;
 322     }
 323
 324     while ((digit = a2d(*s)) >= 0) {
 325         if (digit > 9)
 326             break;
 327         num = num * 10 + digit;
 328         s++;
 329     }
 330
 331     return sign * num;
 332 }