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
  26 #if defined(USE_GPS) && (defined(USE_GPS_PROTO_NMEA))
  27
  28 #include "build/build_config.h"
  29 #include "build/debug.h"
  30
  31 #include "common/axis.h"
  32 #include "common/gps_conversion.h"
  33 #include "common/maths.h"
  34 #include "common/utils.h"
  35
  36 #include "drivers/serial.h"
  37 #include "drivers/time.h"
  38
  39 #include "fc/config.h"
  40 #include "fc/runtime_config.h"
  41
  42 #include "io/serial.h"
  43 #include "io/gps.h"
  44 #include "io/gps_private.h"
  45
  46 #include "scheduler/protothreads.h"
  47
  48 /* This is a light implementation of a GPS frame decoding
  49    This should work with most of modern GPS devices configured to output 5 frames.
  50    It assumes there are some NMEA GGA frames to decode on the serial bus
  51    Now verifies checksum correctly before applying data
  52
  53    Here we use only the following data :
  54      - latitude
  55      - longitude
  56      - GPS fix is/is not ok
  57      - GPS num sat (4 is enough to be +/- reliable)
  58      // added by Mis
  59      - GPS altitude (for OSD displaying)
  60      - GPS speed (for OSD displaying)
  61 */
  62
  63 #define NO_FRAME   0
  64 #define FRAME_GGA  1
  65 #define FRAME_RMC  2
  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     int32_t altitude;
  94     uint16_t speed;
  95     uint16_t ground_course;
  96     uint16_t hdop;
  97     uint32_t time;
  98     uint32_t date;
  99 } gpsDataNmea_t;
 100
 101 #define NMEA_BUFFER_SIZE        16
 102
 103 static bool gpsNewFrameNMEA(char c)
 104 {
 105     static gpsDataNmea_t gps_Msg;
 106
 107     uint8_t frameOK = 0;
 108     static uint8_t param = 0, offset = 0, parity = 0;
 109     static char string[NMEA_BUFFER_SIZE];
 110     static uint8_t checksum_param, gps_frame = NO_FRAME;
 111
 112     switch (c) {
 113         case '$':
 114             param = 0;
 115             offset = 0;
 116             parity = 0;
 117             break;
 118         case ',':
 119         case '*':
 120             string[offset] = 0;
 121             if (param == 0) {       //frame identification
 122                 gps_frame = NO_FRAME;
 123                 if (strcmp(string, "GPGGA") == 0 || strcmp(string, "GNGGA") == 0) {
 124                     gps_frame = FRAME_GGA;
 125                 }
 126                 else if (strcmp(string, "GPRMC") == 0 || strcmp(string, "GNRMC") == 0) {
 127                     gps_frame = FRAME_RMC;
 128                 }
 129             }
 130
 131             switch (gps_frame) {
 132                 case FRAME_GGA:        //************* GPGGA FRAME parsing
 133                     switch (param) {
 134             //          case 1:             // Time information
 135             //              break;
 136                         case 2:
 137                             gps_Msg.latitude = GPS_coord_to_degrees(string);
 138                             break;
 139                         case 3:
 140                             if (string[0] == 'S')
 141                                 gps_Msg.latitude *= -1;
 142                             break;
 143                         case 4:
 144                             gps_Msg.longitude = GPS_coord_to_degrees(string);
 145                             break;
 146                         case 5:
 147                             if (string[0] == 'W')
 148                                 gps_Msg.longitude *= -1;
 149                             break;
 150                         case 6:
 151                             if (string[0] > '0') {
 152                                 gps_Msg.fix = true;
 153                             } else {
 154                                 gps_Msg.fix = false;
 155                             }
 156                             break;
 157                         case 7:
 158                             gps_Msg.numSat = grab_fields(string, 0);
 159                             break;
 160                         case 8:
 161                             gps_Msg.hdop = grab_fields(string, 1) * 10;          // hdop
 162                             break;
 163                         case 9:
 164                             gps_Msg.altitude = grab_fields(string, 1) * 10;     // altitude in cm
 165                             break;
 166                     }
 167                     break;
 168                 case FRAME_RMC:        //************* GPRMC FRAME parsing
 169                                        // $GNRMC,130059.00,V,,,,,,,110917,,,N*62
 170                     switch (param) {
 171                         case 1:
 172                             gps_Msg.time = grab_fields(string, 2);
 173                             break;
 174                         case 7:
 175                             gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
 176                             break;
 177                         case 8:
 178                             gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
 179                             break;
 180                         case 9:
 181                             gps_Msg.date = grab_fields(string, 0);
 182                             break;
 183                     }
 184                     break;
 185             }
 186
 187             param++;
 188             offset = 0;
 189             if (c == '*')
 190                 checksum_param = 1;
 191             else
 192                 parity ^= c;
 193             break;
 194         case '\r':
 195         case '\n':
 196             if (checksum_param) {   //parity checksum
 197                 uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
 198                 if (checksum == parity) {
 199                     gpsStats.packetCount++;
 200                     switch (gps_frame) {
 201                     case FRAME_GGA:
 202                         frameOK = 1;
 203                         gpsSol.numSat = gps_Msg.numSat;
 204                         if (gps_Msg.fix) {
 205                             gpsSol.fixType = GPS_FIX_3D;    // NMEA doesn't report fix type, assume 3D
 206
 207                             gpsSol.llh.lat = gps_Msg.latitude;
 208                             gpsSol.llh.lon = gps_Msg.longitude;
 209                             gpsSol.llh.alt = gps_Msg.altitude;
 210
 211                             // EPH/EPV are unreliable for NMEA as they are not real accuracy
 212                             gpsSol.hdop = gpsConstrainHDOP(gps_Msg.hdop);
 213                             gpsSol.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
 214                             gpsSol.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
 215                             gpsSol.flags.validEPE = false;
 216                         }
 217                         else {
 218                             gpsSol.fixType = GPS_NO_FIX;
 219                         }
 220
 221                         // NMEA does not report VELNED
 222                         gpsSol.flags.validVelNE = false;
 223                         gpsSol.flags.validVelD = false;
 224                         break;
 225                     case FRAME_RMC:
 226                         gpsSol.groundSpeed = gps_Msg.speed;
 227                         gpsSol.groundCourse = gps_Msg.ground_course;
 228
 229                         // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid
 230                         if (gps_Msg.date != 0 && gps_Msg.time != 0) {
 231                             gpsSol.time.year = (gps_Msg.date % 100) + 2000;
 232                             gpsSol.time.month = (gps_Msg.date / 100) % 100;
 233                             gpsSol.time.day = (gps_Msg.date / 10000) % 100;
 234                             gpsSol.time.hours = (gps_Msg.time / 1000000) % 100;
 235                             gpsSol.time.minutes = (gps_Msg.time / 10000) % 100;
 236                             gpsSol.time.seconds = (gps_Msg.time / 100) % 100;
 237                             gpsSol.time.millis = (gps_Msg.time & 100) * 10;
 238                             gpsSol.flags.validTime = true;
 239                         }
 240                         else {
 241                             gpsSol.flags.validTime = false;
 242                         }
 243
 244                         break;
 245                     } // end switch
 246                 }
 247                 else {
 248                     gpsStats.errors++;
 249                 }
 250             }
 251             checksum_param = 0;
 252             break;
 253         default:
 254             if (offset < (NMEA_BUFFER_SIZE-1)) {    // leave 1 byte to trailing zero
 255                 string[offset++] = c;
 256
 257                 // only checksum if character is recorded and used (will cause checksum failure on dropped characters)
 258                 if (!checksum_param)
 259                     parity ^= c;
 260             }
 261     }
 262     return frameOK;
 263 }
 264
 265 static ptSemaphore_t semNewDataReady;
 266
 267 STATIC_PROTOTHREAD(gpsProtocolReceiverThread)
 268 {
 269     ptBegin(gpsProtocolReceiverThread);
 270
 271     while (1) {
 272         // Wait until there are bytes to consume
 273         ptWait(serialRxBytesWaiting(gpsState.gpsPort));
 274
 275         // Consume bytes until buffer empty of until we have full message received
 276         while (serialRxBytesWaiting(gpsState.gpsPort)) {
 277             uint8_t newChar = serialRead(gpsState.gpsPort);
 278             if (gpsNewFrameNMEA(newChar)) {
 279                 gpsSol.flags.validVelNE = false;
 280                 gpsSol.flags.validVelD = false;
 281                 ptSemaphoreSignal(semNewDataReady);
 282                 break;
 283             }
 284         }
 285     }
 286
 287     ptEnd(0);
 288 }
 289
 290 STATIC_PROTOTHREAD(gpsProtocolStateThreadNMEA)
 291 {
 292     ptBegin(gpsProtocolStateThreadNMEA);
 293
 294     // Change baud rate
 295     ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
 296     if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) {
 297         // Cycle through available baud rates and hope that we will match GPS
 298         serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]);
 299         gpsState.autoBaudrateIndex = (gpsState.autoBaudrateIndex + 1) % GPS_BAUDRATE_COUNT;
 300         ptDelayMs(GPS_BAUD_CHANGE_DELAY);
 301     }
 302     else {
 303         // Set baud rate
 304         serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
 305     }
 306
 307     // No configuration is done for pure NMEA modules
 308
 309     // GPS setup done, reset timeout
 310     gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
 311
 312     // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore
 313     while (1) {
 314         ptSemaphoreWait(semNewDataReady);
 315         gpsProcessNewSolutionData();
 316     }
 317
 318     ptEnd(0);
 319 }
 320
 321 void gpsHandleNMEA(void)
 322 {
 323     // Run the protocol threads
 324     gpsProtocolReceiverThread();
 325     gpsProtocolStateThreadNMEA();
 326
 327     // If thread stopped - signal communication loss and restart
 328     if (ptIsStopped(ptGetHandle(gpsProtocolReceiverThread)) || ptIsStopped(ptGetHandle(gpsProtocolStateThreadNMEA))) {
 329         gpsSetState(GPS_LOST_COMMUNICATION);
 330     }
 331 }
 332
 333 void gpsRestartNMEA(void)
 334 {
 335     ptSemaphoreInit(semNewDataReady);
 336     ptRestart(ptGetHandle(gpsProtocolReceiverThread));
 337     ptRestart(ptGetHandle(gpsProtocolStateThreadNMEA));
 338 }
 339
 340 #endif