You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 

118 lines
2.0 KiB

#include <stdio.h>
#include <string.h>
#include "nmea_parser.h"
static char get_checksum(const char* data, int len)
{
const char* c;
char checksum;
checksum = 0x00;
c = data;
while (*c != '*' && len-- > 0) {
checksum ^= *c;
c++;
}
return checksum;
}
static const char* get_pos(const char* c, int* pos)
{
int p1;
int p2;
char d;
if (*c != ',') {
sscanf(c, "%d.%d", &p1, &p2);
*pos = p1*10000 + p2;
c = strchr(c, ',');
c++;
sscanf(c, "%c", &d);
if (d == 'S' || d == 'E') {
*pos = -*pos;
}
} else {
*pos = 0;
c++;
}
c = strchr(c, ',');
c++;
return c;
}
static void parse_GGA(const char* data, nmeadata_t* nmea)
{
const char* c;
c = data;
c = strchr(c, ',');
c++;
sscanf(c, "%d", &nmea->data.fix.time);
c = strchr(c, ',');
c++;
c = get_pos(c, &nmea->data.fix.latitude);
c = get_pos(c, &nmea->data.fix.longitude);
sscanf(c, "%d", &nmea->data.fix.fix);
c = strchr(c, ',');
c++;
sscanf(c, "%d", &nmea->data.fix.satelites);
c = strchr(c, ',');
c++;
nmea->type = nmeatype_fix;
}
void nmea_parser_parse(const char* data, int len, nmeadata_t* nmea)
{
char checksum;
char* checksum_pos;
char checksum_line[3] = {0};
int res;
int checksum_file;
if (nmea == NULL) {
return;
}
nmea->type = nmeatype_unknown;
if (data[0] != '$') {
return;
}
checksum = get_checksum(&data[1], len);
checksum_pos = strchr(data, '*');
if (checksum_pos == NULL) {
return;
}
checksum_pos++;
checksum_line[0] = *checksum_pos;
checksum_pos++;
checksum_line[1] = *checksum_pos;
res = sscanf(checksum_line, "%X", &checksum_file);
if (res != 1) {
return;
}
if (checksum != checksum_file) {
return;
}
if (strncmp(&data[1], "GP", 2) != 0) {
return;
}
if (strncmp(&data[3], "GGA", 3) != 0) {
parse_GGA(data, nmea);
}
}