From c226f6a4126001610cd6d96d44e307d8f0713223 Mon Sep 17 00:00:00 2001 From: tracernz Date: Sat, 31 Jan 2015 18:07:05 +1300 Subject: [PATCH 1/2] 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 db0f726d3..dfac15201 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; } From cda2f3169e2974376b40ada5813324315c21e613 Mon Sep 17 00:00:00 2001 From: tracernz Date: Sat, 31 Jan 2015 18:08:36 +1300 Subject: [PATCH 2/2] Add NMEA $GPGSV Decoding for Sat Data --- src/main/io/gps.c | 48 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index dfac15201..c2896d87d 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -452,6 +452,7 @@ bool gpsNewFrame(uint8_t c) #define NO_FRAME 0 #define FRAME_GGA 1 #define FRAME_RMC 2 +#define FRAME_GSV 3 // This code is used for parsing NMEA data @@ -533,6 +534,8 @@ static bool gpsNewFrameNMEA(char c) static uint8_t param = 0, offset = 0, parity = 0; static char string[15]; static uint8_t checksum_param, gps_frame = NO_FRAME; + static uint8_t svMessageNum = 0; + uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0; switch (c) { case '$': @@ -549,6 +552,8 @@ static bool gpsNewFrameNMEA(char c) gps_frame = FRAME_GGA; if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') gps_frame = FRAME_RMC; + if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'S' && string[4] == 'V') + gps_frame = FRAME_GSV; } switch (gps_frame) { @@ -595,6 +600,49 @@ static bool gpsNewFrameNMEA(char c) break; } break; + case FRAME_GSV: + switch(param) { + /*case 1: + // Total number of messages of this type in this cycle + break; */ + case 2: + // Message number + svMessageNum = grab_fields(string, 0); + break; + case 3: + // Total number of SVs visible + GPS_numCh = grab_fields(string, 0); + break; + } + if(param < 4) + break; + + svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4 + svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number + svSatParam = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite + + if(svSatNum > GPS_SV_MAXSATS) + break; + + switch(svSatParam) { + case 1: + // SV PRN number + GPS_svinfo_chn[svSatNum - 1] = svSatNum; + GPS_svinfo_svid[svSatNum - 1] = grab_fields(string, 0); + break; + /*case 2: + // Elevation, in degrees, 90 maximum + break; + case 3: + // Azimuth, degrees from True North, 000 through 359 + break; */ + case 4: + // SNR, 00 through 99 dB (null when not tracking) + GPS_svinfo_cno[svSatNum - 1] = grab_fields(string, 0); + GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox + break; + } + break; } param++;