Tidy up NMEA Code A Little
Still a LOT of tidying up needed in future, of the whole GPS module really.
This commit is contained in:
tracernz
parent
b8b248827c
commit
c226f6a412
|
|
@ -69,6 +69,8 @@ extern int16_t debug[4];
|
|||
#define LOG_UBLOX_POSLLH 'P'
|
||||
#define LOG_UBLOX_VELNED 'V'
|
||||
|
||||
#define GPS_SV_MAXSATS 16
|
||||
|
||||
char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
|
||||
static char *gpsPacketLogChar = gpsPacketLog;
|
||||
// **********************
|
||||
|
|
@ -85,11 +87,11 @@ uint16_t GPS_altitude; // altitude in 0.1m
|
|||
uint16_t GPS_speed; // speed in 0.1m/s
|
||||
uint16_t GPS_ground_course = 0; // degrees * 10
|
||||
|
||||
uint8_t GPS_numCh; // Number of channels
|
||||
uint8_t GPS_svinfo_chn[16]; // Channel number
|
||||
uint8_t GPS_svinfo_svid[16]; // Satellite ID
|
||||
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
|
||||
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
|
||||
uint8_t GPS_numCh; // Number of channels
|
||||
uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number
|
||||
uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID
|
||||
uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
|
||||
uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS]; // Carrier to Noise Ratio (Signal Strength)
|
||||
|
||||
static gpsConfig_t *gpsConfig;
|
||||
|
||||
|
|
@ -514,18 +516,18 @@ static uint32_t grab_fields(char *src, uint8_t mult)
|
|||
return tmp;
|
||||
}
|
||||
|
||||
typedef struct gpsDataNmea_s {
|
||||
int32_t latitude;
|
||||
int32_t longitude;
|
||||
uint8_t numSat;
|
||||
uint16_t altitude;
|
||||
uint16_t speed;
|
||||
uint16_t ground_course;
|
||||
} gpsDataNmea_t;
|
||||
|
||||
static bool gpsNewFrameNMEA(char c)
|
||||
{
|
||||
typedef struct gpsdata_s {
|
||||
int32_t latitude;
|
||||
int32_t longitude;
|
||||
uint8_t numSat;
|
||||
uint16_t altitude;
|
||||
uint16_t speed;
|
||||
uint16_t ground_course;
|
||||
} gpsdata_t;
|
||||
|
||||
static gpsdata_t gps_Msg;
|
||||
static gpsDataNmea_t gps_Msg;
|
||||
|
||||
uint8_t frameOK = 0;
|
||||
static uint8_t param = 0, offset = 0, parity = 0;
|
||||
|
|
@ -533,111 +535,111 @@ static bool gpsNewFrameNMEA(char c)
|
|||
static uint8_t checksum_param, gps_frame = NO_FRAME;
|
||||
|
||||
switch (c) {
|
||||
case '$':
|
||||
param = 0;
|
||||
offset = 0;
|
||||
parity = 0;
|
||||
break;
|
||||
case ',':
|
||||
case '*':
|
||||
string[offset] = 0;
|
||||
if (param == 0) { //frame identification
|
||||
gps_frame = NO_FRAME;
|
||||
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
|
||||
gps_frame = FRAME_GGA;
|
||||
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
|
||||
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:
|
||||
if (string[0] > '0') {
|
||||
ENABLE_STATE(GPS_FIX);
|
||||
} else {
|
||||
DISABLE_STATE(GPS_FIX);
|
||||
}
|
||||
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;
|
||||
}
|
||||
case '$':
|
||||
param = 0;
|
||||
offset = 0;
|
||||
parity = 0;
|
||||
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;
|
||||
case ',':
|
||||
case '*':
|
||||
string[offset] = 0;
|
||||
if (param == 0) { //frame identification
|
||||
gps_frame = NO_FRAME;
|
||||
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
|
||||
gps_frame = FRAME_GGA;
|
||||
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
|
||||
gps_frame = FRAME_RMC;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
param++;
|
||||
offset = 0;
|
||||
if (c == '*')
|
||||
checksum_param = 1;
|
||||
else
|
||||
parity ^= c;
|
||||
break;
|
||||
case '\r':
|
||||
case '\n':
|
||||
if (checksum_param) { //parity checksum
|
||||
shiftPacketLog();
|
||||
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) {
|
||||
*gpsPacketLogChar = LOG_IGNORED;
|
||||
GPS_packetCount++;
|
||||
switch (gps_frame) {
|
||||
case FRAME_GGA:
|
||||
*gpsPacketLogChar = LOG_NMEA_GGA;
|
||||
frameOK = 1;
|
||||
if (STATE(GPS_FIX)) {
|
||||
GPS_coord[LAT] = gps_Msg.latitude;
|
||||
GPS_coord[LON] = gps_Msg.longitude;
|
||||
GPS_numSat = gps_Msg.numSat;
|
||||
GPS_altitude = gps_Msg.altitude;
|
||||
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:
|
||||
if (string[0] > '0') {
|
||||
ENABLE_STATE(GPS_FIX);
|
||||
} else {
|
||||
DISABLE_STATE(GPS_FIX);
|
||||
}
|
||||
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;
|
||||
}
|
||||
break;
|
||||
case FRAME_RMC:
|
||||
*gpsPacketLogChar = LOG_NMEA_RMC;
|
||||
GPS_speed = gps_Msg.speed;
|
||||
GPS_ground_course = gps_Msg.ground_course;
|
||||
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;
|
||||
} // end switch
|
||||
} else {
|
||||
*gpsPacketLogChar = LOG_ERROR;
|
||||
}
|
||||
}
|
||||
checksum_param = 0;
|
||||
break;
|
||||
default:
|
||||
if (offset < 15)
|
||||
string[offset++] = c;
|
||||
if (!checksum_param)
|
||||
parity ^= c;
|
||||
|
||||
param++;
|
||||
offset = 0;
|
||||
if (c == '*')
|
||||
checksum_param = 1;
|
||||
else
|
||||
parity ^= c;
|
||||
break;
|
||||
case '\r':
|
||||
case '\n':
|
||||
if (checksum_param) { //parity checksum
|
||||
shiftPacketLog();
|
||||
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) {
|
||||
*gpsPacketLogChar = LOG_IGNORED;
|
||||
GPS_packetCount++;
|
||||
switch (gps_frame) {
|
||||
case FRAME_GGA:
|
||||
*gpsPacketLogChar = LOG_NMEA_GGA;
|
||||
frameOK = 1;
|
||||
if (STATE(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:
|
||||
*gpsPacketLogChar = LOG_NMEA_RMC;
|
||||
GPS_speed = gps_Msg.speed;
|
||||
GPS_ground_course = gps_Msg.ground_course;
|
||||
break;
|
||||
} // end switch
|
||||
} else {
|
||||
*gpsPacketLogChar = LOG_ERROR;
|
||||
}
|
||||
}
|
||||
checksum_param = 0;
|
||||
break;
|
||||
default:
|
||||
if (offset < 15)
|
||||
string[offset++] = c;
|
||||
if (!checksum_param)
|
||||
parity ^= c;
|
||||
}
|
||||
return frameOK;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue