src/main/io/gps_nmea.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
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <ctype.h>
  21 #include <string.h>
  22 #include <math.h>
  23
  24 #include "platform.h"
  25 #include "build_config.h"
  26 #include "debug.h"
  27
  28 #include "common/maths.h"
  29 #include "common/axis.h"
  30 #include "common/utils.h"
  31
  32 #include "drivers/system.h"
  33 #include "drivers/serial.h"
  34 #include "drivers/serial_uart.h"
  35
  36 #include "io/serial.h"
  37 #include "io/gps.h"
  38 #include "io/gps_private.h"
  39
  40 #include "flight/gps_conversion.h"
  41
  42 #include "config/config.h"
  43 #include "config/runtime_config.h"
  44
  45 #if defined(GPS) && defined(GPS_PROTO_NMEA)
  46
  47 /* This is a light implementation of a GPS frame decoding
  48    This should work with most of modern GPS devices configured to output 5 frames.
  49    It assumes there are some NMEA GGA frames to decode on the serial bus
  50    Now verifies checksum correctly before applying data
  51
  52    Here we use only the following data :
  53      - latitude
  54      - longitude
  55      - GPS fix is/is not ok
  56      - GPS num sat (4 is enough to be +/- reliable)
  57      // added by Mis
  58      - GPS altitude (for OSD displaying)
  59      - GPS speed (for OSD displaying)
  60 */
  61
  62 #define NO_FRAME   0
  63 #define FRAME_GGA  1
  64 #define FRAME_RMC  2
  65
  66 static uint32_t grab_fields(char *src, uint8_t mult)
  67 {                               // convert string to uint32
  68     uint32_t i;
  69     uint32_t tmp = 0;
  70     for (i = 0; src[i] != 0; i++) {
  71         if (src[i] == '.') {
  72             i++;
  73             if (mult == 0)
  74                 break;
  75             else
  76                 src[i + mult] = 0;
  77         }
  78         tmp *= 10;
  79         if (src[i] >= '0' && src[i] <= '9')
  80             tmp += src[i] - '0';
  81         if (i >= 15)
  82             return 0; // out of bounds
  83     }
  84     return tmp;
  85 }
  86
  87 typedef struct gpsDataNmea_s {
  88     bool fix;
  89     int32_t latitude;
  90     int32_t longitude;
  91     uint8_t numSat;
  92     uint16_t altitude;
  93     uint16_t speed;
  94     uint16_t ground_course;
  95     uint16_t hdop;
  96 } gpsDataNmea_t;
  97
  98 static bool gpsNewFrameNMEA(char c)
  99 {
 100     static gpsDataNmea_t gps_Msg;
 101
 102     uint8_t frameOK = 0;
 103     static uint8_t param = 0, offset = 0, parity = 0;
 104     static char string[15];
 105     static uint8_t checksum_param, gps_frame = NO_FRAME;
 106
 107     switch (c) {
 108         case '$':
 109             param = 0;
 110             offset = 0;
 111             parity = 0;
 112             break;
 113         case ',':
 114         case '*':
 115             string[offset] = 0;
 116             if (param == 0) {       //frame identification
 117                 gps_frame = NO_FRAME;
 118                 if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
 119                     gps_frame = FRAME_GGA;
 120                 if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
 121                     gps_frame = FRAME_RMC;
 122             }
 123
 124             switch (gps_frame) {
 125                 case FRAME_GGA:        //************* GPGGA FRAME parsing
 126                     switch(param) {
 127             //          case 1:             // Time information
 128             //              break;
 129                         case 2:
 130                             gps_Msg.latitude = GPS_coord_to_degrees(string);
 131                             break;
 132                         case 3:
 133                             if (string[0] == 'S')
 134                                 gps_Msg.latitude *= -1;
 135                             break;
 136                         case 4:
 137                             gps_Msg.longitude = GPS_coord_to_degrees(string);
 138                             break;
 139                         case 5:
 140                             if (string[0] == 'W')
 141                                 gps_Msg.longitude *= -1;
 142                             break;
 143                         case 6:
 144                             if (string[0] > '0') {
 145                                 gps_Msg.fix = true;
 146                             } else {
 147                                 gps_Msg.fix = false;
 148                             }
 149                             break;
 150                         case 7:
 151                             gps_Msg.numSat = grab_fields(string, 0);
 152                             break;
 153                         case 8:
 154                             gps_Msg.hdop = grab_fields(string, 1) * 10;          // hdop
 155                             break;
 156                         case 9:
 157                             gps_Msg.altitude = grab_fields(string, 1) * 10;     // altitude in cm
 158                             break;
 159                     }
 160                     break;
 161                 case FRAME_RMC:        //************* GPRMC FRAME parsing
 162                     switch(param) {
 163                         case 7:
 164                             gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
 165                             break;
 166                         case 8:
 167                             gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
 168                             break;
 169                     }
 170                     break;
 171             }
 172
 173             param++;
 174             offset = 0;
 175             if (c == '*')
 176                 checksum_param = 1;
 177             else
 178                 parity ^= c;
 179             break;
 180         case '\r':
 181         case '\n':
 182             if (checksum_param) {   //parity checksum
 183                 uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
 184                 if (checksum == parity) {
 185                     gpsStats.packetCount++;
 186                     switch (gps_frame) {
 187                     case FRAME_GGA:
 188                         frameOK = 1;
 189                         gpsSol.numSat = gps_Msg.numSat;
 190                         if (gps_Msg.fix) {
 191                             gpsSol.fixType = GPS_FIX_3D;    // NMEA doesn't report fix type, assume 3D
 192
 193                             gpsSol.llh.lat = gps_Msg.latitude;
 194                             gpsSol.llh.lon = gps_Msg.longitude;
 195                             gpsSol.llh.alt = gps_Msg.altitude;
 196
 197                             // EPH/EPV are unreliable for NMEA as they are not real accuracy
 198                             gpsSol.hdop = gpsConstrainHDOP(gps_Msg.hdop);
 199                             gpsSol.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
 200                             gpsSol.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
 201                             gpsSol.flags.validEPE = 0;
 202                         }
 203                         else {
 204                             gpsSol.fixType = GPS_NO_FIX;
 205                         }
 206
 207                         // NMEA does not report VELNED
 208                         gpsSol.flags.validVelNE = 0;
 209                         gpsSol.flags.validVelD = 0;
 210                         break;
 211                     case FRAME_RMC:
 212                         gpsSol.groundSpeed = gps_Msg.speed;
 213                         gpsSol.groundCourse = gps_Msg.ground_course;
 214                         break;
 215                     } // end switch
 216                 }
 217                 else {
 218                     gpsStats.errors++;
 219                 }
 220             }
 221             checksum_param = 0;
 222             break;
 223         default:
 224             if (offset < 15)
 225                 string[offset++] = c;
 226             if (!checksum_param)
 227                 parity ^= c;
 228     }
 229     return frameOK;
 230 }
 231
 232 static bool gpsReceiveData(void)
 233 {
 234     bool hasNewData = false;
 235
 236     if (gpsState.gpsPort) {
 237         while (serialRxBytesWaiting(gpsState.gpsPort)) {
 238             uint8_t newChar = serialRead(gpsState.gpsPort);
 239             if (gpsNewFrameNMEA(newChar)) {
 240                 gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
 241                 gpsSol.flags.validVelNE = 0;
 242                 gpsSol.flags.validVelD = 0;
 243                 hasNewData = true;
 244             }
 245         }
 246     }
 247
 248     return hasNewData;
 249 }
 250
 251 static bool gpsInitialize(void)
 252 {
 253     gpsSetState(GPS_CHANGE_BAUD);
 254     return false;
 255 }
 256
 257 static bool gpsChangeBaud(void)
 258 {
 259     gpsFinalizeChangeBaud();
 260     return false;
 261 }
 262
 263 bool gpsHandleNMEA(void)
 264 {
 265     // Receive data
 266     bool hasNewData = gpsReceiveData();
 267
 268     // Process state
 269     switch(gpsState.state) {
 270     default:
 271         return false;
 272
 273     case GPS_INITIALIZING:
 274         return gpsInitialize();
 275
 276     case GPS_CHANGE_BAUD:
 277         return gpsChangeBaud();
 278
 279
 280     case GPS_CHECK_VERSION:
 281     case GPS_CONFIGURE:
 282         // No autoconfig, switch straight to receiving data
 283         gpsSetState(GPS_RECEIVING_DATA);
 284         return false;
 285
 286     case GPS_RECEIVING_DATA:
 287         return hasNewData;
 288     }
 289 }
 290
 291 #endif