Subversion Repositories – MLAB

(root)//Designs/GPSnavigator/SW/GPS/nmea_scan.c @ 4914

Rev 1213

//************************************************************************
// NMEA LOAD FROM rx GPS

#include <stdint.h>
#include <math.h>
#include <stdlib.h>
#include "nmea_scan.h"
#include "gps.h"

enum {ID_RX_GGA,ID_RX_GSA,ID_RX_GSV,ID_RX_RMC,ID_RX_VTG,ID_RX_LOAD,ID_RX_FIND}; 

uint8_t nmea_start_load(char data)
{
        enum {NM_G,NM_P,NM_GP,NM_GPG,NM_GPR,NM_GPV,NM_GPGG,NM_GPGS,NM_GPRM,NM_GPVT};
        static uint8_t ptr = NM_G;
        
        switch (ptr)
        {
                case NM_G: if (data == 'G') ptr = NM_P;                                 //G
                                else { ptr = NM_G; return ID_RX_FIND;}
                                break; 
                case NM_P: if (data == 'P') ptr = NM_GP;                                //P
                                else { ptr = NM_G; return ID_RX_FIND;}
                                break; 
                case NM_GP: switch (data)                                                               //GPG || GPR || GPV
                                {       
                                        case 'G': ptr=NM_GPG;break;
                                        case 'R': ptr=NM_GPR;break;
                                        case 'V': ptr=NM_GPV;break;
                                        default : ptr = NM_G; return ID_RX_FIND;
                                }
                                break;
                case NM_GPG: switch(data)                                                               //GPGG || GPGS
                                {       
                                        case 'G': ptr=NM_GPGG;break;
                                        case 'S': ptr=NM_GPGS;break;
                                        default : ptr = NM_G; return ID_RX_FIND;
                                }
                                break;
                case NM_GPR: if (data == 'M') ptr = NM_GPRM;                    //GPRM
                                else { ptr = NM_G; return ID_RX_FIND;}
                                break;                  
                case NM_GPV: if (data == 'T') ptr = NM_GPVT;                    //GPVT
                                else { ptr = NM_G; return ID_RX_FIND;}
                                break;
                case NM_GPGG: ptr = NM_G;                                                               //GPGGA
                                if (data == 'A') return ID_RX_GGA;                      
                                return ID_RX_FIND;
                case NM_GPGS: ptr = NM_G;
                                switch(data)                                                            //GPGG || GPGS
                                {       
                                        case 'A': return ID_RX_GSA;
                                        case 'V': return ID_RX_GSV;
                                }
                                return ID_RX_FIND;
                case NM_GPRM: ptr = NM_G;                                                               //GPRMC
                                if (data == 'C') return ID_RX_RMC;                      
                                return ID_RX_FIND;
                case NM_GPVT: ptr = NM_G;                                                               //GPVTG
                                if (data == 'G') return ID_RX_VTG;                      
                                return ID_RX_FIND;
                default: ptr = NM_G; return ID_RX_FIND;
                                
        }
        return ID_RX_LOAD;
}

uint8_t  load_nmea(uint8_t rx_shift, char *rx_buf,char *scan_buf)
{
        static uint8_t id_rx_msg = ID_RX_FIND;
        static uint8_t rx_now = 0;
        static uint8_t ptr = 0;
        char data;

        while (1)
        {
        if (rx_shift == rx_now) return RETURN_RX;
        if(++rx_now >= MAX_RX_BUF) rx_now = 0;  
        data = *(rx_buf + rx_now);

        switch (id_rx_msg)
        {
                case ID_RX_GGA:         *(scan_buf+ptr++) = data;
                                                        if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_GGA;}
                                                        if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
                                                        break;
                case ID_RX_GSA:         *(scan_buf+ptr++) = data;
                                                        if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_GSA;}
                                                        if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
                                                        break;
                case ID_RX_GSV:         *(scan_buf+ptr++) = data;
                                                        if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_GSV;}
                                                        if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
                                                        break;  
                case ID_RX_RMC:         *(scan_buf+ptr++) = data;
                                                        if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_RMC;}
                                                        if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
                                                        break;  
                case ID_RX_VTG:         *(scan_buf+ptr++) = data;
                                                        if (data == '*') {id_rx_msg = ID_RX_FIND;return RETURN_VTG;}
                                                        if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
                                                        break;
                case ID_RX_LOAD:        id_rx_msg = nmea_start_load(data);      break;  
                case ID_RX_FIND:
                default:                        ptr=0;if (data == '$') id_rx_msg = ID_RX_LOAD;
                        
        }
        }
        return 0 ;
}

void nmea_gga(char *buf,DATA_GPS *pgps)
{
        uint8_t a;

        if (*(buf++) != ',') return;                                                            // ,
        for (a=0;a<5;a++) while (*(buf++) != ',');                                      // UTC,lat,N,lot,W,
        pgps->fix_position = *(buf++) - 0x30;                                           // fix_position
        if (*(buf++) != ',') return;                                                            // ,
        pgps->satelites_used = 10 * (*(buf++) - 0x30) ;                         // satelites
        pgps->satelites_used += *(buf++)-0x30;
        if (*(buf++) != ',') return;                                                            // ,
        while (*(buf++) != ',');                                                                        // HDOP,
        pgps->altitude = atof(buf++);                                                           // atlitude,
        for (a=0;a<2;a++) while (*(buf++) != ',');                                      // ,M,
        pgps->geoid = atof(buf++);                                                                      // geoid
        for (a=0;a<2;a++) while (*(buf++) != ',');                                      //,M,
        if (*buf != ',')
        {
                pgps->age_diff_corr = atol(buf);                                                // age_dif_corr,
                while (*(buf++) != ',');
        }
        else buf++;
        if (*buf != ',') pgps->diff_id = atol(buf);                                     // ID_diff_station
}

void nmea_gsa(char *buf,DATA_GPS *pgps)
{
        uint8_t a;

        if (*(buf++) != ',') return;                                                            // ,
        pgps->mode1 = *(buf++);                                                                         // mode1
        if (*(buf++) != ',') return;                                                            // ,
        pgps->mode2 = *(buf++);                                                                         // mode2
        if (*(buf++) != ',') return;                                                            // ,
        for (a=0;a<12;a++)
        {
                if (*buf != ',')
                {
                        //pgps->satelite_id[a] = 10*(*(buf++) - 0x30);
                        //pgps->satelite_id[a] += *(buf++)-0x30;
                        //buf++;
                        pgps->satelite_id[a] = atol(buf);
                        while (*(buf++) != ',');
                }
                else
                {
                        pgps->satelite_id[a] = 0;
                        buf++;
                 }
        }
        pgps->PDOP = atof(buf++);
        while (*(buf++) != ',');
        pgps->HDOP = atof(buf++);
        while (*(buf++) != ',');
        pgps->VDOP = atof(buf++);
        while (*(buf++) != ',');
}

void nmea_gsv(char *buf,DATA_GPS *pgps)
{
        uint8_t a,i,b;

        if (*(buf++) != ',') return;                                                                    // ,
        pgps->gsv_num_msg = (*(buf++) - 0x30);                                                  // num of msg
        if (*(buf++) != ',') return;                                                                    //,
        pgps->gsv_msg = (*(buf++) - 0x30);                                                              // num msg
        if (*(buf++) != ',') return;                                                                    //,
        pgps->gsv_satelites_view = 10*(*(buf++) - 0x30);                                // satelites view
        pgps->gsv_satelites_view +=(*(buf++) - 0x30);
        if (*(buf++) != ',') return;                                                                    // ,
        a = 4 * (pgps->gsv_msg - 1);
        for (i= 0; i<4; i++)
        {
                /*pgps->satelit_detail[a].id = 10*(*(buf++) - 0x30);                    
                 pgps->satelit_detail[a].id += *(buf++) - 0x30;                         
                buf++;
                pgps->satelit_detail[a].elevation = 10*(*(buf++) - 0x30);
                 pgps->satelit_detail[a].elevation += *(buf++) - 0x30;
                buf++;
                pgps->satelit_detail[a].azimut = 100*(*(buf++) - 0x30);
                 pgps->satelit_detail[a].azimut += 10*(*(buf++) - 0x30);
                 pgps->satelit_detail[a].azimut += *(buf++) - 0x30;
                buf++;*/
                pgps->satelit_detail[a].id = atol(buf);
                 while (*(buf++) != ',');
                pgps->satelit_detail[a].elevation = atol(buf);
                 while (*(buf++) != ',');
                pgps->satelit_detail[a].azimut = atol(buf);
                 while (*(buf++) != ',');

                if ((*buf) != ',')
                {
                        if (*(buf) == '*')
                        {
                                pgps->satelit_detail[a].SNR =0;
                                return;
                        }
                        //pgps->satelit_detail[a].SNR = 10*(*(buf++) - 0x30);
                         //pgps->satelit_detail[a].SNR += *(buf++) - 0x30;
                         pgps->satelit_detail[a].SNR = atol(buf);
                          b= *(buf++);
                          while ((b != ',') && (b != '*') ) b=*(buf++);
                         if (b == '*') return;
                }
                else 
                {
                        pgps->satelit_detail[a].SNR =0;
                        buf++;
                }
                if ( ++a >= pgps->gsv_satelites_view ) return;
                
        }                                                               
}

//$GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598, ,*10
void nmea_rmc(char *buf,DATA_GPS *pgps)
{
        uint8_t a;      

        if (*(buf++) != ',') return;                                                                    // ,
        pgps->hour = (10* (*(buf++) - 0x30));                                                   //hour
         pgps->hour += (*(buf++) - 0x30);
        pgps->minute = 10* (*(buf++) - 0x30);                                                   //minute
         pgps->minute += (*(buf++) - 0x30);
        pgps->second = 10* (*(buf++) - 0x30);                                                   //second
         pgps->second += (*(buf++) - 0x30);
        while (*(buf++) != ',');                                                                                //.xxx  
        pgps->status = *(buf++);
        if (*(buf++) != ',') return;                                                                    //A,                                    
        pgps->latitude =10.0 * (*(buf++) - 0x30);
         pgps->latitude += 1.0 * (*(buf++) - 0x30);
         pgps->latitude += atof(buf)/60.0;
         while (*(buf++) != ',');                                                                               //latitude,
        pgps->ns_indicator =  *(buf++);                                                                 // N/S
        if (*(buf++) != ',') return;                                                                    //,
        pgps->longitude = 100.0 * (*(buf++) - 0x30);
         pgps->longitude += 10.0 * (*(buf++) - 0x30);
         pgps->longitude += 1.0 * (*(buf++) - 0x30);
         pgps->longitude += atof(buf)/60.0;
        while (*(buf++) != ',');                                                                                //longitude,
        pgps->we_indicator =  *(buf++);                                                                 // E/W
        for (a=0;a<3;a++) while (*(buf++) != ',');                                              // ,speed,course,
        pgps->day = 10* (*(buf++) - 0x30);      
         pgps->day += *(buf++) - 0x30;                                                                  // day
        pgps->month = 10* (*(buf++) - 0x30);    
         pgps->month += *(buf++) - 0x30;                                                                // month
        pgps->year = 10* (*(buf++) - 0x30);
         pgps->year += *(buf) - 0x30;                                                                   // year
                
}

void nmea_vtg(char *buf,DATA_GPS *pgps)
{
        uint8_t a;

        if (*(buf++) != ',') return;                                                    // ,
        pgps->course = atof(buf++);                                                             // course
        for (a=0;a<6;a++) while (*(buf++) != ',');                              // ,T,course,M,speed,N,
        pgps->speed = atof(buf++);
}