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