From c226f6a4126001610cd6d96d44e307d8f0713223 Mon Sep 17 00:00:00 2001 From: tracernz Date: Sat, 31 Jan 2015 18:07:05 +1300 Subject: [PATCH] Tidy up NMEA Code A Little Still a LOT of tidying up needed in future, of the whole GPS module really. --- src/main/io/gps.c | 228 +++++++++++++++++++++++----------------------- 1 file changed, 115 insertions(+), 113 deletions(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index db0f726d3d..dfac152011 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -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; }