Blame |
Last modification |
View Log
| RSS feed
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//############################################################################
//# 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
;
}