nmea.cpp

00001 /*
00002     File:       nmea.cpp
00003     Version:    0.1.0
00004     Date:       Feb. 23, 2013
00005     License:    GPL v2
00006     
00007     NMEA GPS content parser
00008     
00009     ****************************************************************************
00010     Copyright (C) 2013 Radu Motisan  <radu.motisan@gmail.com>
00011     
00012     http://www.pocketmagic.net
00013 
00014     This program is free software; you can redistribute it and/or modify
00015     it under the terms of the GNU General Public License as published by
00016     the Free Software Foundation; either version 2 of the License, or
00017     (at your option) any later version.
00018 
00019     This program is distributed in the hope that it will be useful,
00020     but WITHOUT ANY WARRANTY; without even the implied warranty of
00021     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022     GNU General Public License for more details.
00023 
00024     You should have received a copy of the GNU General Public License
00025     along with this program; if not, write to the Free Software
00026     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00027     ****************************************************************************
00028  */
00029 
00030 #include "nmea.h"
00031 #include <stdio.h>
00032 #include <stdlib.h>
00033 
00034 #include "mbed.h"
00035 #include <stdint.h>
00036 #include <math.h>
00037 #include <ctype.h>
00038 
00039 //#include "../uart/uart.h"
00040 //extern UART uart1;
00041 
00042 
00043 /*
00044  * The serial data is assembled on the fly, without using any redundant buffers. 
00045  * When a sentence is complete (one that starts with $, ending in EOL), all processing is done on 
00046  * this temporary buffer that we've built: checksum computation, extracting sentence "words" (the CSV values), 
00047  * and so on.
00048  * When a new sentence is fully assembled using the fusedata function, the code calls parsedata. 
00049  * This function in turn, splits the sentences and interprets the data. Here is part of the parser function, 
00050  * handling both the $GPRMC NMEA sentence:
00051  */
00052 int NMEA::fusedata(char c) {
00053     
00054     if (c == '$') {
00055         m_bFlagRead = true;
00056         // init parser vars
00057         m_bFlagComputedCks = false;
00058         m_nChecksum = 0;
00059         // after getting  * we start cuttings the received m_nChecksum
00060         m_bFlagReceivedCks = false;
00061         index_received_checksum = 0;
00062         // word cutting variables
00063         m_nWordIdx = 0; m_nPrevIdx = 0; m_nNowIdx = 0;
00064     }
00065     
00066     if (m_bFlagRead) {
00067         // check ending
00068         if (c == '\r' || c== '\n') {
00069             // catch last ending item too
00070             tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = 0;
00071             m_nWordIdx++;
00072             // cut received m_nChecksum
00073             tmp_szChecksum[index_received_checksum] = 0;
00074             // sentence complete, read done
00075             m_bFlagRead = false;
00076             // parse
00077             parsedata();
00078         } else {
00079             // computed m_nChecksum logic: count all chars between $ and * exclusively
00080             if (m_bFlagComputedCks && c == '*') m_bFlagComputedCks = false;
00081             if (m_bFlagComputedCks) m_nChecksum ^= c;
00082             if (c == '$') m_bFlagComputedCks = true;
00083             // received m_nChecksum
00084             if (m_bFlagReceivedCks)  {
00085                 tmp_szChecksum[index_received_checksum] = c;
00086                 index_received_checksum++;
00087             }
00088             if (c == '*') m_bFlagReceivedCks = true;
00089             // build a word
00090             tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = c;
00091             if (c == ',') {
00092                 tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = 0;
00093                 m_nWordIdx++;
00094                 m_nPrevIdx = m_nNowIdx;
00095             }
00096             else m_nNowIdx++;
00097         }               
00098     }
00099     return m_nWordIdx;
00100 }
00101 
00102 
00103 /*
00104  * parse internal tmp_ structures, fused by pushdata, and set the data flag when done
00105  */
00106 void NMEA::parsedata() {
00107     int received_cks = 16*digit2dec(tmp_szChecksum[0]) + digit2dec(tmp_szChecksum[1]);
00108     //uart1.Send("seq: [cc:%X][words:%d][rc:%s:%d]\r\n", m_nChecksum,m_nWordIdx, tmp_szChecksum, received_cks);
00109     // check checksum, and return if invalid!
00110     if (m_nChecksum != received_cks) {
00111         //m_bFlagDataReady = false;
00112         return;
00113     }
00114     /* $GPGGA
00115      * $GPGGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh
00116      * ex: $GPGGA,230600.501,4543.8895,N,02112.7238,E,1,03,3.3,96.7,M,39.0,M,,0000*6A,
00117      *
00118      * WORDS:
00119      *  1    = UTC of Position
00120      *  2    = Latitude
00121      *  3    = N or S
00122      *  4    = Longitude
00123      *  5    = E or W
00124      *  6    = GPS quality indicator (0=invalid; 1=GPS fix; 2=Diff. GPS fix)
00125      *  7    = Number of satellites in use [not those in view]
00126      *  8    = Horizontal dilution of position
00127      *  9    = Antenna altitude above/below mean sea level (geoid)
00128      *  10   = Meters  (Antenna height unit)
00129      *  11   = Geoidal separation (Diff. between WGS-84 earth ellipsoid and mean sea level.  
00130      *      -geoid is below WGS-84 ellipsoid)
00131      *  12   = Meters  (Units of geoidal separation)
00132      *  13   = Age in seconds since last update from diff. reference station
00133      *  14   = Diff. reference station ID#
00134      *  15   = Checksum
00135      */
00136     if (mstrcmp(tmp_words[0], "$GPGGA") == 0) {
00137         // Check GPS Fix: 0=no fix, 1=GPS fix, 2=Dif. GPS fix
00138         if (tmp_words[6][0] == '0') { 
00139             // clear data
00140             res_fLatitude = 0;
00141             res_fLongitude = 0;
00142             m_bFlagDataReady = false;
00143             return;
00144         }           
00145         // parse time
00146         res_nUTCHour = digit2dec(tmp_words[1][0]) * 10 + digit2dec(tmp_words[1][1]);
00147         res_nUTCMin = digit2dec(tmp_words[1][2]) * 10 + digit2dec(tmp_words[1][3]);
00148         res_nUTCSec = digit2dec(tmp_words[1][4]) * 10 + digit2dec(tmp_words[1][5]);
00149         // parse latitude and longitude in NMEA format
00150         res_fLatitude = string2float(tmp_words[2]);
00151         res_fLongitude = string2float(tmp_words[4]);
00152         // get decimal format
00153         if (tmp_words[3][0] == 'S') res_fLatitude  *= -1.0;
00154         if (tmp_words[5][0] == 'W') res_fLongitude *= -1.0;
00155         float degrees = trunc(res_fLatitude / 100.0f);
00156         float minutes = res_fLatitude - (degrees * 100.0f);
00157         res_fLatitude = degrees + minutes / 60.0f;
00158         degrees = trunc(res_fLongitude / 100.0f);
00159         minutes = res_fLongitude - (degrees * 100.0f);
00160         res_fLongitude = degrees + minutes / 60.0f;
00161         
00162         // parse number of satellites
00163         res_nSatellitesUsed = (int)string2float(tmp_words[7]);
00164         
00165         // parse altitude
00166         res_fAltitude = string2float(tmp_words[9]);
00167         
00168         // data ready
00169         m_bFlagDataReady = true;
00170     }
00171     
00172     /* $GPRMC
00173      * note: a siRF chipset will not support magnetic headers.
00174      * $GPRMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,ddmmyy,x.x,a*hh
00175      * ex: $GPRMC,230558.501,A,4543.8901,N,02112.7219,E,1.50,181.47,230213,,,A*66,
00176      *
00177      * WORDS:
00178      *  1    = UTC of position fix
00179      *  2    = Data status (V=navigation receiver warning)
00180      *  3    = Latitude of fix
00181      *  4    = N or S
00182      *  5    = Longitude of fix
00183      *  6    = E or W
00184      *  7    = Speed over ground in knots
00185      *  8    = Track made good in degrees True, Bearing This indicates the direction that the device is currently moving in, 
00186      *       from 0 to 360, measured in �azimuth�.
00187      *  9    = UT date
00188      *  10   = Magnetic variation degrees (Easterly var. subtracts from true course)
00189      *  11   = E or W
00190      *  12   = Checksum
00191      */
00192     if (mstrcmp(tmp_words[0], "$GPRMC") == 0) {
00193         // Check data status: A-ok, V-invalid
00194         if (tmp_words[2][0] == 'V') {
00195             // clear data
00196             res_fLatitude = 0;
00197             res_fLongitude = 0;
00198             m_bFlagDataReady = false;
00199             return;
00200         }
00201         // parse time
00202         res_nUTCHour = digit2dec(tmp_words[1][0]) * 10 + digit2dec(tmp_words[1][1]);
00203         res_nUTCMin = digit2dec(tmp_words[1][2]) * 10 + digit2dec(tmp_words[1][3]);
00204         res_nUTCSec = digit2dec(tmp_words[1][4]) * 10 + digit2dec(tmp_words[1][5]);
00205         // parse latitude and longitude in NMEA format
00206         res_fLatitude = string2float(tmp_words[3]);
00207         res_fLongitude = string2float(tmp_words[5]);
00208         // get decimal format
00209         if (tmp_words[4][0] == 'S') res_fLatitude  *= -1.0;
00210         res_clat = tmp_words[4][0];
00211         if (tmp_words[6][0] == 'W') res_fLongitude *= -1.0;
00212         res_clon = tmp_words[6][0];
00213         float degrees = trunc(res_fLatitude / 100.0f);
00214         float minutes = res_fLatitude - (degrees * 100.0f);
00215         res_fLatitude = degrees + minutes / 60.0f;
00216         degrees = trunc(res_fLongitude / 100.0f);
00217         minutes = res_fLongitude - (degrees * 100.0f);
00218         res_fLongitude = degrees + minutes / 60.0f;
00219         //parse speed
00220         // The knot (pronounced not) is a unit of speed equal to one nautical mile (1.852 km) per hour
00221         res_fSpeed = string2float(tmp_words[7]);
00222         res_fSpeed *= double(1.852); // convert to km/h  1knot = 1.852Km, Was res_fSpeed /= 1.852;
00223         // parse bearing
00224         res_fBearing = string2float(tmp_words[8]);
00225         // parse UTC date
00226         res_nUTCDay = digit2dec(tmp_words[9][0]) * 10 + digit2dec(tmp_words[9][1]);
00227         res_nUTCMonth = digit2dec(tmp_words[9][2]) * 10 + digit2dec(tmp_words[9][3]);
00228         res_nUTCYear = digit2dec(tmp_words[9][4]) * 10 + digit2dec(tmp_words[9][5]);
00229         
00230         // data ready
00231         m_bFlagDataReady = true;
00232     }       
00233 }
00234 /*
00235  * returns base-16 value of chars '0'-'9' and 'A'-'F';
00236  * does not trap invalid chars!
00237  */ 
00238 int NMEA::digit2dec(char digit) {
00239     if (int(digit) >= 65) 
00240         return int(digit) - 55;
00241     else 
00242         return int(digit) - 48;
00243 }
00244 
00245 /* returns base-10 value of zero-terminated string
00246  * that contains only chars '+','-','0'-'9','.';
00247  * does not trap invalid strings! 
00248  */
00249 float NMEA::string2float(char* s) {
00250     long  integer_part = 0;
00251     float decimal_part = 0.0;
00252     float decimal_pivot = 0.1;
00253     bool isdecimal = false, isnegative = false;
00254     
00255     char c;
00256     while ( ( c = *s++) )  { 
00257         // skip special/sign chars
00258         if (c == '-') { isnegative = true; continue; }
00259         if (c == '+') continue;
00260         if (c == '.') { isdecimal = true; continue; }
00261         
00262         if (!isdecimal) {
00263             integer_part = (10 * integer_part) + (c - 48);
00264         }
00265         else {
00266             decimal_part += decimal_pivot * (float)(c - 48);
00267             decimal_pivot /= 10.0;
00268         }
00269     }
00270     // add integer part
00271     decimal_part += (float)integer_part;
00272     
00273     // check negative
00274     if (isnegative)  decimal_part = - decimal_part;
00275 
00276     return decimal_part;
00277 }
00278 
00279 int NMEA::mstrcmp(const char *s1, const char *s2)
00280 {
00281     while((*s1 && *s2) && (*s1 == *s2))
00282     s1++,s2++;
00283     return *s1 - *s2;
00284 }
00285         
00286 bool NMEA::isdataready() {
00287     return m_bFlagDataReady;
00288 }
00289 
00290 int NMEA::getHour() {
00291     return res_nUTCHour;
00292 }   
00293 int NMEA::getMinute() {
00294     return res_nUTCMin;
00295 }
00296 int NMEA::getSecond() {
00297     return res_nUTCSec;
00298 }
00299 int NMEA::getDay() {
00300     return res_nUTCDay;
00301 }
00302 int NMEA::getMonth() {
00303     return res_nUTCMonth;
00304 }
00305 int NMEA::getYear() {
00306     return res_nUTCYear;
00307 }
00308 
00309 float NMEA::getLatitude() {
00310     return res_fLatitude;
00311 }
00312 
00313 float NMEA::getLongitude() {
00314     return res_fLongitude;
00315 }
00316 
00317 int NMEA::getSatellites() {
00318     return res_nSatellitesUsed;
00319 }
00320 
00321 float  NMEA::getAltitude() {
00322     return res_fAltitude;
00323 }
00324 
00325 float NMEA::getSpeed() {
00326     return res_fSpeed;
00327 }
00328 
00329 float NMEA::getBearing() {
00330     return res_fBearing;
00331 }
00332 
00333 char NMEA::getlatc() {
00334     return res_clat;
00335 }   
00336 
00337 char NMEA::getlonc() {
00338     return res_clon;
00339 }   
00340 
00341 float NMEA::trunc(float v) {
00342     if(v < 0.0) {
00343         v*= -1.0;
00344         v = floor(v);
00345         v*=-1.0;
00346     } else {
00347         v = floor(v);
00348     }
00349     return v;
00350 }