0,0 → 1,525 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
//############################################################################ |
//# HISTORY nmea.c |
//# |
//# 08.08.2015 CB |
//# - add: Einführung eines neuen Interruptbasierten NMEA Parsers |
//# cpp-Code auf c geändert und an PKT angepasst |
//# |
//############################################################################ |
|
/* URSPRUNG: |
File: nmea.cpp |
Version: 0.1.0 |
Date: Feb. 23, 2013 |
License: GPL v2 |
|
NMEA GPS content parser |
|
**************************************************************************** |
Copyright (C) 2013 Radu Motisan <radu.motisan@gmail.com> |
|
http://www.pocketmagic.net |
|
This program is free software; you can redistribute it and/or modify |
it under the terms of the GNU General Public License as published by |
the Free Software Foundation; either version 2 of the License, or |
(at your option) any later version. |
|
This program is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
|
You should have received a copy of the GNU General Public License |
along with this program; if not, write to the Free Software |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
**************************************************************************** |
*/ |
|
#include "nmea.h" |
#include "../timer/timer.h" |
//#include <stdio.h> |
//#include <stdlib.h> |
//#include <util/delay.h> |
#include <stdbool.h> |
|
|
#define MAX_POWER 10 |
#define getPower(x) (int32_t)pgm_read_dword(&powers[x]) |
static const int32_t powers[MAX_POWER] PROGMEM = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000}; |
|
uint8_t m_bFlagRead, // flag used by the parser, when a valid sentence has begun |
m_bFlagDataReady = false; // valid GPS fix and data available, user can call reader functions |
volatile uint32_t res_nNMEAcounter = 0; // NMEA Datensatzzähler |
volatile uint32_t res_nCRCerror = 0; // zählt die CRC Errors beim dekodieren |
uint8_t receiveNMEA = false; |
char tmp_words[20][15], // hold parsed words for one given NMEA sentence |
tmp_szChecksum[15]; // hold the received checksum for one given NMEA sentence |
|
// will be set to true for characters between $ and * only |
bool m_bFlagComputedCks ; // used to compute checksum and indicate valid checksum interval (between $ and * in a given sentence) |
int m_nChecksum ; // numeric checksum, computed for a given sentence |
bool m_bFlagReceivedCks ; // after getting * we start cuttings the received checksum |
int index_received_checksum ; // used to parse received checksum |
|
// word cutting variables |
int m_nWordIdx , // the current word in a sentence |
m_nPrevIdx, // last character index where we did a cut |
m_nNowIdx ; // current character index |
|
// globals to store parser results |
int32_t res_fLongitude; // GPRMC and GPGGA |
int32_t res_fLatitude; // GPRMC and GPGGA |
unsigned char res_nUTCHour, res_nUTCMin, res_nUTCSec, // GPRMC and GPGGA |
res_nUTCDay, res_nUTCMonth, res_nUTCYear; // GPRMC |
char Time[9]; // GPRMC and GPGGA |
uint8_t res_nSatellitesUsed; // GPGGA |
uint8_t res_nGPSfix; // GPGGA |
int16_t res_nHDOP; // GPGGA |
int16_t res_fAltitude; // GPGGA |
float res_fSpeed; // GPRMC |
int16_t res_fBearing; // GPRMC |
|
NMEA_GPGGA_t NMEA; // Variable mit der Struktur für die NMEA Daten für PKT |
|
// the parser, currently handling GPRMC and GPGGA, but easy to add any new sentences |
// void parsedata(); |
// aux functions |
int digit2dec(char hexdigit); |
float string2float(char* s); |
int mstrcmp(const char *s1, const char *s2); |
|
|
|
|
/* |
* The serial data is assembled on the fly, without using any redundant buffers. |
* When a sentence is complete (one that starts with $, ending in EOL), all processing is done on |
* this temporary buffer that we've built: checksum computation, extracting sentence "words" (the CSV values), |
* and so on. |
* When a new sentence is fully assembled using the fusedata function, the code calls parsedata. |
* This function in turn, splits the sentences and interprets the data. Here is part of the parser function, |
* handling both the $GPRMC NMEA sentence: |
*/ |
uint8_t fusedata(char c) |
|
{ |
|
if (c == '$') { |
m_bFlagRead = true; |
// init parser vars |
m_bFlagComputedCks = false; |
m_nChecksum = 0; |
// after getting * we start cuttings the received m_nChecksum |
m_bFlagReceivedCks = false; |
index_received_checksum = 0; |
// word cutting variables |
m_nWordIdx = 0; m_nPrevIdx = 0; m_nNowIdx = 0; |
timer_nmea_timeout = NMEA_TIMEOUT; // Timeout Timer setzen |
} |
|
if (m_bFlagRead) { |
// check ending |
if (c == '\r' || c== '\n') { |
// catch last ending item too |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = 0; |
m_nWordIdx++; |
// cut received m_nChecksum |
tmp_szChecksum[index_received_checksum] = 0; |
// sentence complete, read done |
m_bFlagRead = false; |
// parse |
parsedata(); |
} else { |
// computed m_nChecksum logic: count all chars between $ and * exclusively |
if (m_bFlagComputedCks && c == '*') m_bFlagComputedCks = false; |
if (m_bFlagComputedCks) m_nChecksum ^= c; |
if (c == '$') m_bFlagComputedCks = true; |
// received m_nChecksum |
if (m_bFlagReceivedCks) { |
tmp_szChecksum[index_received_checksum] = c; |
index_received_checksum++; |
} |
if (c == '*') m_bFlagReceivedCks = true; |
// build a word |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = c; |
if (c == ',') { |
tmp_words[m_nWordIdx][m_nNowIdx - m_nPrevIdx] = 0; |
m_nWordIdx++; |
m_nPrevIdx = m_nNowIdx; |
} |
else m_nNowIdx++; |
} |
} |
return m_nWordIdx; |
} |
|
|
/* |
* parse internal tmp_ structures, fused by pushdata, and set the data flag when done |
*/ |
void parsedata(void) { |
int received_cks = 16*digit2dec(tmp_szChecksum[0]) + digit2dec(tmp_szChecksum[1]); |
//uart1.Send("seq: [cc:%X][words:%d][rc:%s:%d]\r\n", m_nChecksum,m_nWordIdx, tmp_szChecksum, received_cks); |
// check checksum, and return if invalid! |
if (m_nChecksum != received_cks) { |
m_bFlagDataReady = false; |
res_nCRCerror = res_nCRCerror+1; |
return; |
} |
/* $GPGGA |
* $GPGGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh |
* ex: $GPGGA,230600.501,4543.8895,N,02112.7238,E,1,03,3.3,96.7,M,39.0,M,,0000*6A, |
* |
* WORDS: |
* 1 = UTC of Position |
* 2 = Latitude |
* 3 = N or S |
* 4 = Longitude |
* 5 = E or W |
* 6 = GPS quality indicator (0=invalid; 1=GPS fix; 2=Diff. GPS fix) |
* 7 = Number of satellites in use [not those in view] |
* 8 = Horizontal dilution of position |
* 9 = Antenna altitude above/below mean sea level (geoid) |
* 10 = Meters (Antenna height unit) |
* 11 = Geoidal separation (Diff. between WGS-84 earth ellipsoid and mean sea level. |
* -geoid is below WGS-84 ellipsoid) |
* 12 = Meters (Units of geoidal separation) |
* 13 = Age in seconds since last update from diff. reference station |
* 14 = Diff. reference station ID# |
* 15 = Checksum |
*/ |
if (mstrcmp(tmp_words[0], "$GPGGA") == 0) { |
// Check GPS Fix: 0=no fix, 1=GPS fix, 2=Dif. GPS fix |
res_nGPSfix = atoi(&tmp_words[6][0]); |
NMEA.SatFix = res_nGPSfix; |
if (tmp_words[6][0] == '0') { |
// clear data |
res_fLatitude = 0; |
res_fLongitude = 0; |
// m_bFlagDataReady = false; |
|
// return; |
} |
// parse time |
res_nUTCHour = digit2dec(tmp_words[1][0]) * 10 + digit2dec(tmp_words[1][1]); |
res_nUTCMin = digit2dec(tmp_words[1][2]) * 10 + digit2dec(tmp_words[1][3]); |
res_nUTCSec = digit2dec(tmp_words[1][4]) * 10 + digit2dec(tmp_words[1][5]); |
NMEA_getTime (tmp_words[1]); |
|
// parse latitude and longitude in NMEA format |
// res_fLatitude = string2float(tmp_words[2]); |
// res_fLongitude = string2float(tmp_words[4]); |
|
// get decimal format |
// if (tmp_words[3][0] == 'S') res_fLatitude *= -1.0; |
// if (tmp_words[5][0] == 'W') res_fLongitude *= -1.0; |
// float degrees = trunc(res_fLatitude / 100.0f); |
// float minutes = res_fLatitude - (degrees * 100.0f); |
// res_fLatitude = degrees + minutes / 60.0f; |
// degrees = trunc(res_fLongitude / 100.0f); |
// minutes = res_fLongitude - (degrees * 100.0f); |
// res_fLongitude = degrees + minutes / 60.0f; |
|
res_fLatitude = NMEA_calcLatitude(tmp_words[2],tmp_words[3]); |
res_fLongitude = NMEA_calcLongitude(tmp_words[4],tmp_words[5]); |
NMEA.Latitude = res_fLatitude; |
NMEA.Longitude = res_fLongitude; |
// parse number of satellites |
res_nSatellitesUsed = (int)string2float(tmp_words[7]); |
NMEA.SatsInUse = atoi(tmp_words[7]); |
// parse HDOP |
res_nHDOP = NMEA_floatStrToInt(tmp_words[8],1); |
NMEA.HDOP = res_nHDOP; |
|
// parse altitude |
// res_fAltitude = string2float(tmp_words[9]); |
res_fAltitude = NMEA_floatStrToInt( tmp_words[9], 1); |
NMEA.Altitude = res_fAltitude; |
// data ready |
m_bFlagDataReady = true; |
res_nNMEAcounter = res_nNMEAcounter +1; |
NMEA.Counter = res_nNMEAcounter; |
|
} |
|
// 8.8.2015 CB wird zur Zeit nicht benötigt |
/* $GPRMC |
* note: a siRF chipset will not support magnetic headers. |
* $GPRMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,ddmmyy,x.x,a*hh |
* ex: $GPRMC,230558.501,A,4543.8901,N,02112.7219,E,1.50,181.47,230213,,,A*66, |
* |
* WORDS: |
* 1 = UTC of position fix |
* 2 = Data status (V=navigation receiver warning) |
* 3 = Latitude of fix |
* 4 = N or S |
* 5 = Longitude of fix |
* 6 = E or W |
* 7 = Speed over ground in knots |
* 8 = Track made good in degrees True, Bearing This indicates the direction that the device is currently moving in, |
* from 0 to 360, measured in azimuth. |
* 9 = UT date |
* 10 = Magnetic variation degrees (Easterly var. subtracts from true course) |
* 11 = E or W |
* 12 = Checksum |
*/ |
// if (mstrcmp(tmp_words[0], "$GPRMC") == 0) { |
// // Check data status: A-ok, V-invalid |
// if (tmp_words[2][0] == 'V') { |
// // clear data |
// res_fLatitude = 0; |
// res_fLongitude = 0; |
//// m_bFlagDataReady = false; |
// return; |
// } |
//// // parse time |
//// res_nUTCHour = digit2dec(tmp_words[1][0]) * 10 + digit2dec(tmp_words[1][1]); |
//// res_nUTCMin = digit2dec(tmp_words[1][2]) * 10 + digit2dec(tmp_words[1][3]); |
//// res_nUTCSec = digit2dec(tmp_words[1][4]) * 10 + digit2dec(tmp_words[1][5]); |
//// // parse latitude and longitude in NMEA format |
//// res_fLatitude = string2float(tmp_words[3]); |
//// res_fLongitude = string2float(tmp_words[5]); |
//// // get decimal format |
//// if (tmp_words[4][0] == 'S') res_fLatitude *= -1.0; |
//// if (tmp_words[6][0] == 'W') res_fLongitude *= -1.0; |
//// float degrees = trunc(res_fLatitude / 100.0f); |
//// float minutes = res_fLatitude - (degrees * 100.0f); |
//// res_fLatitude = degrees + minutes / 60.0f; |
//// degrees = trunc(res_fLongitude / 100.0f); |
//// minutes = res_fLongitude - (degrees * 100.0f); |
//// res_fLongitude = degrees + minutes / 60.0f; |
// //parse speed |
// // The knot (pronounced not) is a unit of speed equal to one nautical mile (1.852 km) per hour |
// res_fSpeed = NMEA_floatStrToInt(tmp_words[7],1); |
// res_fSpeed /= 1.852; // convert to km/h |
// // parse bearing |
// res_fBearing = NMEA_floatStrToInt(tmp_words[8],1); |
//// // parse UTC date |
//// res_nUTCDay = digit2dec(tmp_words[9][0]) * 10 + digit2dec(tmp_words[9][1]); |
//// res_nUTCMonth = digit2dec(tmp_words[9][2]) * 10 + digit2dec(tmp_words[9][3]); |
//// res_nUTCYear = digit2dec(tmp_words[9][4]) * 10 + digit2dec(tmp_words[9][5]); |
// |
// // data ready |
// res_nNMEAcounter = res_nNMEAcounter +1; |
// NMEA.Counter = res_nNMEAcounter; |
// m_bFlagDataReady = true; |
// } |
} |
|
|
|
//-------------------------------------------------------------- |
// NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps |
//-------------------------------------------------------------- |
int32_t NMEA_calcLatitude( const char *s, const char *NS) |
{ |
int32_t deg; |
int32_t min; |
|
//lcdx_puts_at(0,5,NS,0,0,0); |
deg = (s[0] - '0') * 10 + s[1] - '0'; // First 2 chars are full degrees |
min = NMEA_floatStrToInt( &s[2], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
|
deg = deg * 10000000 + min; |
|
if( *NS == 'S' ) |
{ |
deg = -deg; |
} |
|
return deg; |
} |
|
|
|
//-------------------------------------------------------------- |
// NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps |
//-------------------------------------------------------------- |
int32_t NMEA_calcLongitude( const char *s, const char *WE) |
{ |
int32_t deg; |
int32_t min; |
|
//lcdx_puts_at(10,5,WE,0,0,0); |
deg = ((s[0] - '0') * 10 + s[1] - '0') * 10 + s[2] - '0'; // First 3 chars are full degrees |
min = NMEA_floatStrToInt( &s[3], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
|
deg = deg * 10000000 + min; |
|
if( *WE == 'W' ) |
{ |
deg = -deg; |
} |
|
return deg; |
} |
|
//-------------------------------------------------------------- |
void NMEA_getTime( const char *s) |
{ |
uint8_t sem = 0; |
uint8_t i; |
|
for( i=0; i<6; i++ ) |
{ |
NMEA.Time[sem++] = s[i]; |
|
if( i==1 || i==3) |
NMEA.Time[sem++] = ':'; |
|
} |
NMEA.Time[sem] = '\0'; |
} |
|
//-------------------------------------------------------------- |
/* |
* returns base-16 value of chars '0'-'9' and 'A'-'F'; |
* does not trap invalid chars! |
*/ |
int digit2dec(char digit) |
{ |
if ((int)digit >= 65) |
return ((int)digit - 55); |
else |
return ((int)digit - 48); |
} |
|
//-------------------------------------------------------------- |
/* returns base-10 value of zero-terminated string |
* that contains only chars '+','-','0'-'9','.'; |
* does not trap invalid strings! |
*/ |
float string2float(char* s) { |
long integer_part = 0; |
float decimal_part = 0.0; |
float decimal_pivot = 0.1; |
bool isdecimal = false, isnegative = false; |
|
char c; |
while ( ( c = *s++) ) { |
// skip special/sign chars |
if (c == '-') { isnegative = true; continue; } |
if (c == '+') continue; |
if (c == '.') { isdecimal = true; continue; } |
|
if (!isdecimal) { |
integer_part = (10 * integer_part) + (c - 48); |
} |
else { |
decimal_part += decimal_pivot * (float)(c - 48); |
decimal_pivot /= 10.0; |
} |
} |
// add integer part |
decimal_part += (float)integer_part; |
|
// check negative |
if (isnegative) decimal_part = - decimal_part; |
|
return decimal_part; |
} |
|
|
//-------------------------------------------------------------- |
// Trying to avoid floating point maths here. Converts a floating point string to an integer with a smaller unit |
// i.e. floatStrToInt("4.5", 2) = 4.5 * 1E2 = 450 |
//-------------------------------------------------------------- |
int32_t NMEA_floatStrToInt( const char *s, int32_t power1 ) |
{ |
char *endPtr; |
int32_t v; |
|
v = strtol(s, &endPtr, 10); |
|
if( *endPtr == '.' ) |
{ |
for (s = endPtr + 1; *s && power1; s++) |
{ |
v = v * 10 + (*s - '0'); |
--power1; |
} |
} |
|
if( power1 ) |
{ |
// Table to avoid multiple multiplications |
v = v * getPower(power1); |
} |
|
return v; |
} |
|
|
//-------------------------------------------------------------- |
int mstrcmp(const char *s1, const char *s2) |
{ |
while((*s1 && *s2) && (*s1 == *s2)) |
s1++,s2++; |
return *s1 - *s2; |
} |
|
//-------------------------------------------------------------- |
bool NMEA_isdataready() { |
return m_bFlagDataReady; |
} |
|
int NMEA_getHour() { |
return res_nUTCHour; |
} |
int NMEA_getMinute() { |
return res_nUTCMin; |
} |
int NMEA_getSecond() { |
return res_nUTCSec; |
} |
int NMEA_getDay() { |
return res_nUTCDay; |
} |
int NMEA_getMonth() { |
return res_nUTCMonth; |
} |
int NMEA_getYear() { |
return res_nUTCYear; |
} |
|
int32_t NMEA_getLatitude() { |
return res_fLatitude; |
} |
|
int32_t NMEA_getLongitude() { |
return res_fLongitude; |
} |
|
uint8_t NMEA_getSatellites() { |
return res_nSatellitesUsed; |
} |
|
uint8_t NMEA_getGPSfix() { |
return res_nGPSfix; |
} |
int16_t NMEA_getHDOP() { |
return res_nHDOP; |
} |
|
int16_t NMEA_getAltitude() { |
return res_fAltitude; |
} |
|
float NMEA_getSpeed() { |
return res_fSpeed; |
} |
|
int16_t NMEA_getBearing() { |
return res_fBearing; |
} |
uint32_t NMEA_getNMEAcounter() { |
return res_nNMEAcounter; |
} |
|
uint32_t NMEA_getCRCerror() { |
return res_nCRCerror; |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |