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