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   uint32_t date;
 138   uint32_t time;
 139 } gpsDataNmea_t;
 140
 141 bool gpsNewFrameNMEA(char c)
 142 {
 143   static gpsDataNmea_t gps_Msg;
 144
 145   uint8_t frameOK = 0;
 146   static uint8_t param = 0, offset = 0, parity = 0;
 147   static char string[15];
 148   static uint8_t checksum_param, gps_frame = NO_FRAME;
 149
 150   switch (c) {
 151     case '$':
 152       param = 0;
 153       offset = 0;
 154       parity = 0;
 155       break;
 156     case ',':
 157     case '*':
 158       string[offset] = 0;
 159       if (param == 0) {
 160         // Frame identification (accept all GPS talkers (GP: GPS, GL:Glonass, GN:combination, etc...))
 161         gps_frame = NO_FRAME;
 162         if (string[0] == 'G' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') {
 163           gps_frame = FRAME_GGA;
 164         }
 165         else if (string[0] == 'G' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') {
 166           gps_frame = FRAME_RMC;
 167         }
 168         else {
 169           // turn off this frame (do this only once a second)
 170           static gtime_t lastGpsCmdSent = 0;
 171           if (string[0] == 'G' && g_rtcTime != lastGpsCmdSent) {
 172             lastGpsCmdSent = g_rtcTime;
 173             char cmd[] = "$PUBX,40,GSV,0,0,0,0";
 174             cmd[9]  = string[2];
 175             cmd[10] = string[3];
 176             cmd[11] = string[4];
 177             gpsSendFrame(cmd);
 178           }
 179         }
 180       }
 181
 182       switch (gps_frame) {
 183         case FRAME_GGA:        //************* GPGGA FRAME parsing
 184           switch (param) {
 185             case 2:
 186               gps_Msg.latitude = GPS_coord_to_degrees(string);
 187               break;
 188             case 3:
 189               if (string[0] == 'S')
 190                 gps_Msg.latitude *= -1;
 191               break;
 192             case 4:
 193               gps_Msg.longitude = GPS_coord_to_degrees(string);
 194               break;
 195             case 5:
 196               if (string[0] == 'W')
 197                 gps_Msg.longitude *= -1;
 198               break;
 199             case 6:
 200               if (string[0] > '0') {
 201                 gps_Msg.fix = 1;
 202               }
 203               else {
 204                 gps_Msg.fix = 0;
 205               }
 206               break;
 207             case 7:
 208               gps_Msg.numSat = grab_fields(string, 0);
 209               break;
 210             case 9:
 211               gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
 212               break;
 213           }
 214           break;
 215         case FRAME_RMC:        //************* GPRMC FRAME parsing
 216           switch (param) {
 217             case 1:
 218               gps_Msg.time = grab_fields(string, 0);
 219               break;
 220             case 2:
 221               if (string[0] == 'A') {
 222                 gps_Msg.fix = 1;
 223               }
 224               else {
 225                 gps_Msg.fix = 0;
 226               }
 227               break;
 228             case 7:
 229               gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
 230               break;
 231             case 8:
 232               gps_Msg.groundCourse = (grab_fields(string, 1));      // ground course deg * 10
 233               break;
 234             case 9:
 235               gps_Msg.date = grab_fields(string, 0);
 236               break;
 237           }
 238           break;
 239
 240       }
 241
 242       param++;
 243       offset = 0;
 244       if (c == '*')
 245         checksum_param = 1;
 246       else
 247         parity ^= c;
 248       break;
 249     case '\r':
 250     case '\n':
 251       if (checksum_param) {   //parity checksum
 252         uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') +
 253                            ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
 254         if (checksum == parity) {
 255           gpsData.packetCount++;
 256           switch (gps_frame) {
 257             case FRAME_GGA:
 258               frameOK = 1;
 259               gpsData.fix = gps_Msg.fix;
 260               gpsData.numSat = gps_Msg.numSat;
 261               if (gps_Msg.fix) {
 262                 __disable_irq();    // do the atomic update of lat/lon
 263                 gpsData.latitude = gps_Msg.latitude;
 264                 gpsData.longitude = gps_Msg.longitude;
 265                 gpsData.altitude = gps_Msg.altitude;
 266                 __enable_irq();
 267               }
 268               break;
 269             case FRAME_RMC:
 270               gpsData.speed = gps_Msg.speed;
 271               gpsData.groundCourse = gps_Msg.groundCourse;
 272 #if defined(RTCLOCK)
 273               // set RTC clock if needed
 274               if (g_eeGeneral.adjustRTC && gps_Msg.fix) {
 275                 div_t qr = div(gps_Msg.date, 100);
 276                 uint8_t year = qr.rem;
 277                 qr = div(qr.quot, 100);
 278                 uint8_t mon = qr.rem;
 279                 uint8_t day = qr.quot;
 280                 qr = div(gps_Msg.time, 100);
 281                 uint8_t sec = qr.rem;
 282                 qr = div(qr.quot, 100);
 283                 uint8_t min = qr.rem;
 284                 uint8_t hour = qr.quot;
 285                 rtcAdjust(year+2000, mon, day, hour, min, sec);
 286               }
 287 #endif
 288           } // end switch
 289         }
 290         else {
 291           gpsData.errorCount++;
 292         }
 293       }
 294       checksum_param = 0;
 295       break;
 296     default:
 297       if (offset < 15)
 298         string[offset++] = c;
 299       if (!checksum_param)
 300         parity ^= c;
 301   }
 302   return frameOK;
 303 }
 304
 305 bool gpsNewFrame(uint8_t c)
 306 {
 307   return gpsNewFrameNMEA(c);
 308 }
 309
 310 void gpsNewData(uint8_t c)
 311 {
 312   if (!gpsNewFrame(c)) {
 313     return;
 314   }
 315 }
 316
 317 void gpsWakeup()
 318 {
 319   uint8_t byte;
 320   while (gpsGetByte(&byte)) {
 321     gpsNewData(byte);
 322   }
 323 }
 324
 325 char hex(uint8_t b) {
 326   return b > 9 ? b + 'A' - 10 : b + '0';
 327 }
 328
 329 void gpsSendFrame(const char * frame)
 330 {
 331   // send given frame, add checksum and CRLF
 332   uint8_t parity = 0;
 333   TRACE_NOCRLF("gps> %s", frame);
 334   while (*frame) {
 335     if (*frame != '$') parity ^= *frame;
 336     gpsSendByte(*frame);
 337     ++frame;
 338   }
 339   gpsSendByte('*');
 340   gpsSendByte(hex(parity >> 4));
 341   gpsSendByte(hex(parity & 0x0F));
 342   gpsSendByte('\r');
 343   gpsSendByte('\n');
 344   TRACE("*%02x", parity);
 345 }