radio/src/gps.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 /*
  22  * This file is based on code from Cleanflight project
  23  * https://github.com/cleanflight/cleanflight
  24  *
  25  * Cleanflight is free software: you can redistribute it and/or modify
  26  * it under the terms of the GNU General Public License as published by
  27  * the Free Software Foundation, either version 3 of the License, or
  28  * (at your option) any later version.
  29  *
  30  * Cleanflight is distributed in the hope that it will be useful,
  31  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  32  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  33  * GNU General Public License for more details.
  34  *
  35  * You should have received a copy of the GNU General Public License
  36  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  37  */
  38
  39 #include "opentx.h"
  40 #include <ctype.h>
  41
  42 gpsdata_t gpsData;
  43
  44 /* This is a light implementation of a GPS frame decoding
  45    This should work with most of modern GPS devices configured to output 5 frames.
  46    It assumes there are some NMEA GGA frames to decode on the serial bus
  47    Now verifies checksum correctly before applying data
  48
  49    Here we use only the following data :
  50      - latitude
  51      - longitude
  52      - GPS fix is/is not ok
  53      - GPS num sat (4 is enough to be +/- reliable)
  54      // added by Mis
  55      - GPS altitude (for OSD displaying)
  56      - GPS speed (for OSD displaying)
  57 */
  58
  59 #define NO_FRAME   0
  60 #define FRAME_GGA  1
  61 #define FRAME_RMC  2
  62
  63 #define DIGIT_TO_VAL(_x)    (_x - '0')
  64
  65 uint32_t GPS_coord_to_degrees(const char * coordinateString)
  66 {
  67   const char * fieldSeparator, * remainingString;
  68   uint8_t degrees = 0, minutes = 0;
  69   uint16_t fractionalMinutes = 0;
  70   uint8_t digitIndex;
  71
  72   // scan for decimal point or end of field
  73   for (fieldSeparator = coordinateString; isdigit((unsigned char) *fieldSeparator); fieldSeparator++) {
  74     if (fieldSeparator >= coordinateString + 15)
  75       return 0; // stop potential fail
  76   }
  77   remainingString = coordinateString;
  78
  79   // convert degrees
  80   while ((fieldSeparator - remainingString) > 2) {
  81     if (degrees)
  82       degrees *= 10;
  83     degrees += DIGIT_TO_VAL(*remainingString++);
  84   }
  85   // convert minutes
  86   while (fieldSeparator > remainingString) {
  87     if (minutes)
  88       minutes *= 10;
  89     minutes += DIGIT_TO_VAL(*remainingString++);
  90   }
  91   // convert fractional minutes
  92   // expect up to four digits, result is in
  93   // ten-thousandths of a minute
  94   if (*fieldSeparator == '.') {
  95     remainingString = fieldSeparator + 1;
  96     for (digitIndex = 0; digitIndex < 4; digitIndex++) {
  97       fractionalMinutes *= 10;
  98       if (isdigit((unsigned char) *remainingString))
  99         fractionalMinutes += *remainingString++ - '0';
 100     }
 101   }
 102   // TODO return degrees * 10000000UL + (minutes * 1000000UL + fractionalMinutes * 100UL) / 6;
 103   return degrees * 1000000UL + (minutes * 100000UL + fractionalMinutes * 10UL) / 6;
 104 }
 105
 106 // helper functions
 107 uint32_t grab_fields(char * src, uint8_t mult)
 108 {
 109   uint32_t i;
 110   uint32_t tmp = 0;
 111   for (i = 0; src[i] != 0; i++) {
 112     if (src[i] == '.') {
 113       i++;
 114       if (mult == 0)
 115         break;
 116       else
 117         src[i + mult] = 0;
 118     }
 119     tmp *= 10;
 120     if (src[i] >= '0' && src[i] <= '9')
 121       tmp += src[i] - '0';
 122     if (i >= 15)
 123       return 0; // out of bounds
 124   }
 125   return tmp;
 126 }
 127
 128 typedef struct gpsDataNmea_s
 129 {
 130   uint8_t fix;
 131   int32_t latitude;
 132   int32_t longitude;
 133   uint8_t numSat;
 134   uint16_t altitude;
 135   uint16_t speed;
 136   uint16_t groundCourse;
 137   uint16_t hdop;
 138   uint32_t date;
 139   uint32_t time;
 140 } gpsDataNmea_t;
 141
 142 bool gpsNewFrameNMEA(char c)
 143 {
 144   static gpsDataNmea_t gps_Msg;
 145
 146   uint8_t frameOK = 0;
 147   static uint8_t param = 0, offset = 0, parity = 0;
 148   static char string[15];
 149   static uint8_t checksum_param, gps_frame = NO_FRAME;
 150
 151   switch (c) {
 152     case '$':
 153       param = 0;
 154       offset = 0;
 155       parity = 0;
 156       break;
 157     case ',':
 158     case '*':
 159       string[offset] = 0;
 160       if (param == 0) {
 161         // Frame identification (accept all GPS talkers (GP: GPS, GL:Glonass, GN:combination, etc...))
 162         gps_frame = NO_FRAME;
 163         if (string[0] == 'G' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') {
 164           gps_frame = FRAME_GGA;
 165         }
 166         else if (string[0] == 'G' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') {
 167           gps_frame = FRAME_RMC;
 168         }
 169         else {
 170           // turn off this frame (do this only once a second)
 171           static gtime_t lastGpsCmdSent = 0;
 172           if (string[0] == 'G' && g_rtcTime != lastGpsCmdSent) {
 173             lastGpsCmdSent = g_rtcTime;
 174             char cmd[] = "$PUBX,40,GSV,0,0,0,0";
 175             cmd[9]  = string[2];
 176             cmd[10] = string[3];
 177             cmd[11] = string[4];
 178             gpsSendFrame(cmd);
 179           }
 180         }
 181       }
 182
 183       switch (gps_frame) {
 184         case FRAME_GGA:        //************* GPGGA FRAME parsing
 185           switch (param) {
 186             case 2:
 187               gps_Msg.latitude = GPS_coord_to_degrees(string);
 188               break;
 189             case 3:
 190               if (string[0] == 'S')
 191                 gps_Msg.latitude *= -1;
 192               break;
 193             case 4:
 194               gps_Msg.longitude = GPS_coord_to_degrees(string);
 195               break;
 196             case 5:
 197               if (string[0] == 'W')
 198                 gps_Msg.longitude *= -1;
 199               break;
 200             case 6:
 201               if (string[0] > '0') {
 202                 gps_Msg.fix = 1;
 203               }
 204               else {
 205                 gps_Msg.fix = 0;
 206               }
 207               break;
 208             case 7:
 209               gps_Msg.numSat = grab_fields(string, 0);
 210               break;
 211             case 8:
 212               gps_Msg.hdop = grab_fields(string, 1) * 10;
 213               break;
 214             case 9:
 215               gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
 216               break;
 217           }
 218           break;
 219         case FRAME_RMC:        //************* GPRMC FRAME parsing
 220           switch (param) {
 221             case 1:
 222               gps_Msg.time = grab_fields(string, 0);
 223               break;
 224             case 2:
 225               if (string[0] == 'A') {
 226                 gps_Msg.fix = 1;
 227               }
 228               else {
 229                 gps_Msg.fix = 0;
 230               }
 231               break;
 232             case 7:
 233               gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
 234               break;
 235             case 8:
 236               gps_Msg.groundCourse = (grab_fields(string, 1));      // ground course deg * 10
 237               break;
 238             case 9:
 239               gps_Msg.date = grab_fields(string, 0);
 240               break;
 241           }
 242           break;
 243
 244       }
 245
 246       param++;
 247       offset = 0;
 248       if (c == '*')
 249         checksum_param = 1;
 250       else
 251         parity ^= c;
 252       break;
 253     case '\r':
 254     case '\n':
 255       if (checksum_param) {   //parity checksum
 256         uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') +
 257                            ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
 258         if (checksum == parity) {
 259           gpsData.packetCount++;
 260           switch (gps_frame) {
 261             case FRAME_GGA:
 262               frameOK = 1;
 263               gpsData.fix = gps_Msg.fix;
 264               gpsData.numSat = gps_Msg.numSat;
 265               gpsData.hdop = gps_Msg.hdop;
 266               if (gps_Msg.fix) {
 267                 __disable_irq();    // do the atomic update of lat/lon
 268                 gpsData.latitude = gps_Msg.latitude;
 269                 gpsData.longitude = gps_Msg.longitude;
 270                 gpsData.altitude = gps_Msg.altitude;
 271                 __enable_irq();
 272               }
 273               break;
 274             case FRAME_RMC:
 275               gpsData.speed = gps_Msg.speed;
 276               gpsData.groundCourse = gps_Msg.groundCourse;
 277 #if defined(RTCLOCK)
 278               // set RTC clock if needed
 279               if (g_eeGeneral.adjustRTC && gps_Msg.fix) {
 280                 div_t qr = div(gps_Msg.date, 100);
 281                 uint8_t year = qr.rem;
 282                 qr = div(qr.quot, 100);
 283                 uint8_t mon = qr.rem;
 284                 uint8_t day = qr.quot;
 285                 qr = div(gps_Msg.time, 100);
 286                 uint8_t sec = qr.rem;
 287                 qr = div(qr.quot, 100);
 288                 uint8_t min = qr.rem;
 289                 uint8_t hour = qr.quot;
 290                 rtcAdjust(year+2000, mon, day, hour, min, sec);
 291               }
 292 #endif
 293           } // end switch
 294         }
 295         else {
 296           gpsData.errorCount++;
 297         }
 298       }
 299       checksum_param = 0;
 300       break;
 301     default:
 302       if (offset < 15)
 303         string[offset++] = c;
 304       if (!checksum_param)
 305         parity ^= c;
 306   }
 307   return frameOK;
 308 }
 309
 310 bool gpsNewFrame(uint8_t c)
 311 {
 312   return gpsNewFrameNMEA(c);
 313 }
 314
 315 void gpsNewData(uint8_t c)
 316 {
 317   if (!gpsNewFrame(c)) {
 318     return;
 319   }
 320 }
 321
 322 void gpsWakeup()
 323 {
 324   uint8_t byte;
 325   while (gpsGetByte(&byte)) {
 326     gpsNewData(byte);
 327   }
 328 }
 329
 330 char hex(uint8_t b) {
 331   return b > 9 ? b + 'A' - 10 : b + '0';
 332 }
 333
 334 void gpsSendFrame(const char * frame)
 335 {
 336   // send given frame, add checksum and CRLF
 337   uint8_t parity = 0;
 338   TRACE_NOCRLF("gps> %s", frame);
 339   while (*frame) {
 340     if (*frame != '$') parity ^= *frame;
 341     gpsSendByte(*frame);
 342     ++frame;
 343   }
 344   gpsSendByte('*');
 345   gpsSendByte(hex(parity >> 4));
 346   gpsSendByte(hex(parity & 0x0F));
 347   gpsSendByte('\r');
 348   gpsSendByte('\n');
 349   TRACE("*%02x", parity);
 350 }