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