A GPS serial interrupt service routine that has an on the fly nmea parser. Works with a STM32F411RE and a Adafruit GPS logger.
Dependents: Bicycl_Computer_NUCLEO-F411RE Bicycl_Computer_NUCLEO-L476RG
Fork of GPS by
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 }
Generated on Tue Jul 12 2022 18:50:37 by 1.7.2