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;
}