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