Subversion Repositories Projects

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//############################################################################

//# 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;

}