src/main/common/typeconversion.c

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