/Designs/GPSnavigator/SW/GPS/nmea_scan.c
0,0 → 1,272
//************************************************************************
// 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++);
}