Update gps.c
correctly verifies checksum before modifying read NMEA gps data
This commit is contained in:
norem
Dominic Clifton
parent
7917e44293
commit
74c3e28a12
|
|
@ -1052,6 +1052,8 @@ static uint8_t hex_c(uint8_t n)
|
|||
/* This is a light implementation of a GPS frame decoding
|
||||
This should work with most of modern GPS devices configured to output NMEA frames.
|
||||
It assumes there are some NMEA GGA frames to decode on the serial bus
|
||||
Now verifies checksum correctly before applying data
|
||||
|
||||
Here we use only the following data :
|
||||
- latitude
|
||||
- longitude
|
||||
|
|
@ -1061,76 +1063,128 @@ static uint8_t hex_c(uint8_t n)
|
|||
- GPS altitude (for OSD displaying)
|
||||
- GPS speed (for OSD displaying)
|
||||
*/
|
||||
|
||||
#define NO_FRAME 0
|
||||
#define FRAME_GGA 1
|
||||
#define FRAME_RMC 2
|
||||
|
||||
static bool gpsNewFrameNMEA(char c)
|
||||
{
|
||||
typedef struct gpsdata_t {
|
||||
int32_t latitude;
|
||||
int32_t longitude;
|
||||
uint8_t numSat;
|
||||
uint16_t altitude;
|
||||
uint16_t speed;
|
||||
uint16_t ground_course;
|
||||
};
|
||||
|
||||
static gpsdata_t gps_Msg;
|
||||
|
||||
uint8_t frameOK = 0;
|
||||
static uint8_t param = 0, offset = 0, parity = 0;
|
||||
static char string[15];
|
||||
static uint8_t checksum_param, frame = 0;
|
||||
static uint8_t checksum_param, gps_frame = NO_FRAME;
|
||||
|
||||
if (c == '$') {
|
||||
switch (c) {
|
||||
case '$':
|
||||
param = 0;
|
||||
offset = 0;
|
||||
parity = 0;
|
||||
} else if (c == ',' || c == '*') {
|
||||
break;
|
||||
case ',':
|
||||
case '*':
|
||||
string[offset] = 0;
|
||||
if (param == 0) { //frame identification
|
||||
frame = 0;
|
||||
gps_frame = NO_FRAME;
|
||||
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
|
||||
frame = FRAME_GGA;
|
||||
gps_frame = FRAME_GGA;
|
||||
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
|
||||
frame = FRAME_RMC;
|
||||
} else if (frame == FRAME_GGA) {
|
||||
if (param == 2) {
|
||||
GPS_coord[LAT] = GPS_coord_to_degrees(string);
|
||||
} else if (param == 3 && string[0] == 'S')
|
||||
GPS_coord[LAT] = -GPS_coord[LAT];
|
||||
else if (param == 4) {
|
||||
GPS_coord[LON] = GPS_coord_to_degrees(string);
|
||||
} else if (param == 5 && string[0] == 'W')
|
||||
GPS_coord[LON] = -GPS_coord[LON];
|
||||
else if (param == 6) {
|
||||
gps_frame = FRAME_RMC;
|
||||
}
|
||||
|
||||
switch (gps_frame) {
|
||||
case FRAME_GGA: //************* GPGGA FRAME parsing
|
||||
switch (param) {
|
||||
// case 1: // Time information
|
||||
// break;
|
||||
case 2:
|
||||
gps_Msg.latitude = GPS_coord_to_degrees(string);
|
||||
break;
|
||||
case 3:
|
||||
if (string[0] == 'S')
|
||||
gps_Msg.latitude *= -1;
|
||||
break;
|
||||
case 4:
|
||||
gps_Msg.longitude = GPS_coord_to_degrees(string);
|
||||
break;
|
||||
case 5:
|
||||
if (string[0] == 'W')
|
||||
gps_Msg.longitude *= -1;
|
||||
break;
|
||||
case 6:
|
||||
f.GPS_FIX = string[0] > '0';
|
||||
} else if (param == 7) {
|
||||
GPS_numSat = grab_fields(string, 0);
|
||||
} else if (param == 9) {
|
||||
GPS_altitude = grab_fields(string, 0); // altitude in meters added by Mis
|
||||
break;
|
||||
case 7:
|
||||
gps_Msg.numSat = grab_fields(string, 0);
|
||||
break;
|
||||
case 9:
|
||||
gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis
|
||||
break;
|
||||
}
|
||||
} else if (frame == FRAME_RMC) {
|
||||
if (param == 7) {
|
||||
GPS_speed = (grab_fields(string, 1) * 5144L) / 1000L; // speed in cm/s added by Mis
|
||||
} else if (param == 8) {
|
||||
GPS_ground_course = grab_fields(string, 1); // ground course deg * 10
|
||||
break;
|
||||
case FRAME_RMC: //************* GPRMC FRAME parsing
|
||||
switch (param) {
|
||||
case 7:
|
||||
gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis
|
||||
break;
|
||||
case 8:
|
||||
gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
param++;
|
||||
offset = 0;
|
||||
if (c == '*')
|
||||
checksum_param = 1;
|
||||
else
|
||||
parity ^= c;
|
||||
} else if (c == '\r' || c == '\n') {
|
||||
if (checksum_param) { // parity checksum
|
||||
uint8_t checksum = hex_c(string[0]);
|
||||
checksum <<= 4;
|
||||
checksum += hex_c(string[1]);
|
||||
if (checksum == parity)
|
||||
break;
|
||||
case '\r':
|
||||
case '\n':
|
||||
if (checksum_param) { //parity checksum
|
||||
uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
|
||||
if (checksum == parity) {
|
||||
switch (gps_frame) {
|
||||
case FRAME_GGA:
|
||||
frameOK = 1;
|
||||
if (f.GPS_FIX) {
|
||||
GPS_coord[LAT] = gps_Msg.latitude;
|
||||
GPS_coord[LON] = gps_Msg.longitude;
|
||||
GPS_numSat = gps_Msg.numSat;
|
||||
GPS_altitude = gps_Msg.altitude;
|
||||
}
|
||||
break;
|
||||
case FRAME_RMC:
|
||||
GPS_speed = gps_Msg.speed;
|
||||
GPS_ground_course = gps_Msg.ground_course;
|
||||
break;
|
||||
} // end switch
|
||||
}
|
||||
}
|
||||
checksum_param = 0;
|
||||
} else {
|
||||
break;
|
||||
default:
|
||||
if (offset < 15)
|
||||
string[offset++] = c;
|
||||
if (!checksum_param)
|
||||
parity ^= c;
|
||||
}
|
||||
return frameOK && (frame == FRAME_GGA);
|
||||
return frameOK;
|
||||
}
|
||||
|
||||
|
||||
// UBX support
|
||||
typedef struct {
|
||||
uint8_t preamble1;
|
||||
|
|
|
|||
Loading…
Reference in New Issue