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 #define FRAME_GSV  3
  66
  67 static uint32_t grab_fields(char *src, uint8_t mult)
  68 {                               // convert string to uint32
  69     uint32_t i;
  70     uint32_t tmp = 0;
  71     for (i = 0; src[i] != 0; i++) {
  72         if (src[i] == '.') {
  73             i++;
  74             if (mult == 0)
  75                 break;
  76             else
  77                 src[i + mult] = 0;
  78         }
  79         tmp *= 10;
  80         if (src[i] >= '0' && src[i] <= '9')
  81             tmp += src[i] - '0';
  82         if (i >= 15)
  83             return 0; // out of bounds
  84     }
  85     return tmp;
  86 }
  87
  88 typedef struct gpsDataNmea_s {
  89     bool fix;
  90     int32_t latitude;
  91     int32_t longitude;
  92     uint8_t numSat;
  93     uint16_t altitude;
  94     uint16_t speed;
  95     uint16_t ground_course;
  96     uint16_t hdop;
  97 } gpsDataNmea_t;
  98
  99 static bool gpsNewFrameNMEA(char c)
 100 {
 101     static gpsDataNmea_t gps_Msg;
 102
 103     uint8_t frameOK = 0;
 104     static uint8_t param = 0, offset = 0, parity = 0;
 105     static char string[15];
 106     static uint8_t checksum_param, gps_frame = NO_FRAME;
 107     static uint8_t svMessageNum = 0;
 108     uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0;
 109
 110     switch (c) {
 111         case '$':
 112             param = 0;
 113             offset = 0;
 114             parity = 0;
 115             break;
 116         case ',':
 117         case '*':
 118             string[offset] = 0;
 119             if (param == 0) {       //frame identification
 120                 gps_frame = NO_FRAME;
 121                 if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
 122                     gps_frame = FRAME_GGA;
 123                 if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
 124                     gps_frame = FRAME_RMC;
 125                 if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'S' && string[4] == 'V')
 126                     gps_frame = FRAME_GSV;
 127             }
 128
 129             switch (gps_frame) {
 130                 case FRAME_GGA:        //************* GPGGA FRAME parsing
 131                     switch(param) {
 132             //          case 1:             // Time information
 133             //              break;
 134                         case 2:
 135                             gps_Msg.latitude = GPS_coord_to_degrees(string);
 136                             break;
 137                         case 3:
 138                             if (string[0] == 'S')
 139                                 gps_Msg.latitude *= -1;
 140                             break;
 141                         case 4:
 142                             gps_Msg.longitude = GPS_coord_to_degrees(string);
 143                             break;
 144                         case 5:
 145                             if (string[0] == 'W')
 146                                 gps_Msg.longitude *= -1;
 147                             break;
 148                         case 6:
 149                             if (string[0] > '0') {
 150                                 gps_Msg.fix = true;
 151                             } else {
 152                                 gps_Msg.fix = false;
 153                             }
 154                             break;
 155                         case 7:
 156                             gps_Msg.numSat = grab_fields(string, 0);
 157                             break;
 158                         case 8:
 159                             gps_Msg.hdop = grab_fields(string, 1) * 10;         // hdop (assume GPS is reporting it in meters)
 160                             break;
 161                         case 9:
 162                             gps_Msg.altitude = grab_fields(string, 1) * 10;     // altitude in cm
 163                             break;
 164                     }
 165                     break;
 166                 case FRAME_RMC:        //************* GPRMC FRAME parsing
 167                     switch(param) {
 168                         case 7:
 169                             gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
 170                             break;
 171                         case 8:
 172                             gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
 173                             break;
 174                     }
 175                     break;
 176                 case FRAME_GSV:
 177                     switch(param) {
 178                       /*case 1:
 179                             // Total number of messages of this type in this cycle
 180                             break; */
 181                         case 2:
 182                             // Message number
 183                             svMessageNum = grab_fields(string, 0);
 184                             break;
 185                         case 3:
 186                             // Total number of SVs visible
 187                             gpsSol.numCh = grab_fields(string, 0);
 188                             break;
 189                     }
 190                     if(param < 4)
 191                         break;
 192
 193                     svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4
 194                     svSatNum    = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number
 195                     svSatParam  = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite
 196
 197                     if(svSatNum > GPS_SV_MAXSATS)
 198                         break;
 199
 200                     switch(svSatParam) {
 201                         case 1:
 202                             // SV PRN number
 203                             gpsSol.svInfo[svSatNum - 1].chn = svSatNum;
 204                             gpsSol.svInfo[svSatNum - 1].svid = grab_fields(string, 0);
 205                             break;
 206                       /*case 2:
 207                             // Elevation, in degrees, 90 maximum
 208                             break;
 209                         case 3:
 210                             // Azimuth, degrees from True North, 000 through 359
 211                             break; */
 212                         case 4:
 213                             // SNR, 00 through 99 dB (null when not tracking)
 214                             gpsSol.svInfo[svSatNum - 1].cno = grab_fields(string, 0);
 215                             gpsSol.svInfo[svSatNum - 1].quality = 0; // only used by ublox
 216                             break;
 217                     }
 218                     break;
 219             }
 220
 221             param++;
 222             offset = 0;
 223             if (c == '*')
 224                 checksum_param = 1;
 225             else
 226                 parity ^= c;
 227             break;
 228         case '\r':
 229         case '\n':
 230             if (checksum_param) {   //parity checksum
 231                 uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
 232                 if (checksum == parity) {
 233                     gpsStats.packetCount++;
 234                     switch (gps_frame) {
 235                     case FRAME_GGA:
 236                         frameOK = 1;
 237                         gpsSol.numSat = gps_Msg.numSat;
 238                         if (gps_Msg.fix) {
 239                             gpsSol.flags.fix3D = 1;
 240                             gpsSol.llh.lat = gps_Msg.latitude;
 241                             gpsSol.llh.lon = gps_Msg.longitude;
 242                             gpsSol.llh.alt = gps_Msg.altitude;
 243
 244                             // EPH/EPV are unreliable for NMEA as they are not real accuracy
 245                             gpsSol.eph = gpsConstrainEPE(gps_Msg.hdop);
 246                             gpsSol.epv = gpsConstrainEPE(gps_Msg.hdop);
 247                             gpsSol.flags.validEPE = 0;
 248                         }
 249                         else {
 250                             gpsSol.flags.fix3D = 0;
 251                         }
 252
 253                         // NMEA does not report VELNED
 254                         gpsSol.flags.validVelNE = 0;
 255                         gpsSol.flags.validVelD = 0;
 256                         break;
 257                     case FRAME_RMC:
 258                         gpsSol.groundSpeed = gps_Msg.speed;
 259                         gpsSol.groundCourse = gps_Msg.ground_course;
 260                         break;
 261                     } // end switch
 262                 }
 263                 else {
 264                     gpsStats.errors++;
 265                 }
 266             }
 267             checksum_param = 0;
 268             break;
 269         default:
 270             if (offset < 15)
 271                 string[offset++] = c;
 272             if (!checksum_param)
 273                 parity ^= c;
 274     }
 275     return frameOK;
 276 }
 277
 278 static bool gpsReceiveData(void)
 279 {
 280     bool hasNewData = false;
 281
 282     if (gpsState.gpsPort) {
 283         while (serialRxBytesWaiting(gpsState.gpsPort)) {
 284             uint8_t newChar = serialRead(gpsState.gpsPort);
 285             if (gpsNewFrameNMEA(newChar)) {
 286                 gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
 287                 gpsSol.flags.validVelNE = 0;
 288                 gpsSol.flags.validVelD = 0;
 289                 hasNewData = true;
 290             }
 291         }
 292     }
 293
 294     return hasNewData;
 295 }
 296
 297 static bool gpsInitialize(void)
 298 {
 299     gpsSetState(GPS_CHANGE_BAUD);
 300     return false;
 301 }
 302
 303 static bool gpsChangeBaud(void)
 304 {
 305     gpsFinalizeChangeBaud();
 306     return false;
 307 }
 308
 309 bool gpsHandleNMEA(void)
 310 {
 311     // Receive data
 312     bool hasNewData = gpsReceiveData();
 313
 314     // Process state
 315     switch(gpsState.state) {
 316     default:
 317         return false;
 318
 319     case GPS_INITIALIZING:
 320         return gpsInitialize();
 321
 322     case GPS_CHANGE_BAUD:
 323         return gpsChangeBaud();
 324
 325
 326     case GPS_CHECK_VERSION:
 327     case GPS_CONFIGURE:
 328         // No autoconfig, switch straight to receiving data
 329         gpsSetState(GPS_RECEIVING_DATA);
 330         return false;
 331
 332     case GPS_RECEIVING_DATA:
 333         return hasNewData;
 334     }
 335 }
 336
 337 #endif