src/main/common/gps_conversion.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 <stdbool.h>
  22 #include <stdint.h>
  23 #include <ctype.h>
  24 #include <string.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_GPS
  29
  30
  31 #define DIGIT_TO_VAL(_x)    (_x - '0')
  32 uint32_t GPS_coord_to_degrees(const char* coordinateString)
  33 {
  34     const char *fieldSeparator, *remainingString;
  35     uint8_t degrees = 0, minutes = 0;
  36     uint16_t fractionalMinutes = 0;
  37     uint8_t digitIndex;
  38
  39     // scan for decimal point or end of field
  40     for (fieldSeparator = coordinateString; isdigit((unsigned char)*fieldSeparator); fieldSeparator++) {
  41         if (fieldSeparator >= coordinateString + 15)
  42             return 0; // stop potential fail
  43     }
  44     remainingString = coordinateString;
  45
  46     // convert degrees
  47     while ((fieldSeparator - remainingString) > 2) {
  48         if (degrees)
  49             degrees *= 10;
  50         degrees += DIGIT_TO_VAL(*remainingString++);
  51     }
  52     // convert minutes
  53     while (fieldSeparator > remainingString) {
  54         if (minutes)
  55             minutes *= 10;
  56         minutes += DIGIT_TO_VAL(*remainingString++);
  57     }
  58     // convert fractional minutes
  59     // expect up to four digits, result is in
  60     // ten-thousandths of a minute
  61     if (*fieldSeparator == '.') {
  62         remainingString = fieldSeparator + 1;
  63         for (digitIndex = 0; digitIndex < 4; digitIndex++) {
  64             fractionalMinutes *= 10;
  65             if (isdigit((unsigned char)*remainingString))
  66                 fractionalMinutes += *remainingString++ - '0';
  67         }
  68     }
  69     return degrees * 10000000UL + (minutes * 1000000UL + fractionalMinutes * 100UL) / 6;
  70 }
  71 #endif