bool GPS_NMEA_newFrame(char c) { uint8_t frameOK = 0; static uint8_t param = 0, offset = 0, parity = 0; static char string[15]; static uint8_t checksum_param, frame = 0; if (c == '$') { param = 0; offset = 0; parity = 0; } else if (c == ',' || c == '*') { string[offset] = 0; if (param == 0) { //frame identification frame = 0; if (string[0] == 'G' && string[1] == 'N' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') frame = FRAME_GGA; if (string[0] == 'G' && string[1] == 'N' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') frame = FRAME_RMC; } else if (frame == FRAME_GGA) { if (param == 2) { GPS_Info.GPS_coord[LAT] = GPS_coord_to_degrees(string); } else if (param == 3 && string[0] == 'S') GPS_Info.GPS_coord[LAT] = -GPS_Info.GPS_coord[LAT]; else if (param == 4) {GPS_Info.GPS_coord[LON] = GPS_coord_to_degrees(string);} else if (param == 5 && string[0] == 'W') GPS_Info.GPS_coord[LON] = -GPS_Info.GPS_coord[LON]; else if (param == 6) { fgps.GPS_FIX = (string[0] > '0'); GPS_Info.GPS_Fixed = fgps.GPS_FIX; } else if (param == 7) { GPS_Info.GPS_numSat = grab_fields(string,0); } else if (param == 9) { GPS_Info.GPS_altitude = grab_fields(string,0); } // altitude in meters added by Mis } else if (frame == FRAME_RMC) { if (param == 7) { GPS_Info.GPS_speed = ((uint32_t)grab_fields(string,1)*5144L)/1000L;} //gps speed in cm/s will be used for navigation else if (param == 8) { GPS_Info.GPS_ground_course = grab_fields(string,1); } //ground course deg*10 } 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) frameOK = 1; } checksum_param=0; } else { if (offset < 15) string[offset++] = c; if (!checksum_param) parity ^= c; } if (frame) GPS_Info.GPS_Present = 1; return frameOK && (frame==FRAME_GGA); }
static bool GPS_NMEA_newFrame(char c) { uint8_t frameOK = 0; static uint8_t param = 0, offset = 0, parity = 0; static char string[15]; static uint8_t checksum_param, frame = 0; if (c == '$') { param = 0; offset = 0; parity = 0; } else if (c == ',' || c == '*') { string[offset] = 0; if (param == 0) { //frame identification frame = 0; if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') 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) { if (string[0] > '0') flagSet(FLAG_GPS_FIX); else flagClear(FLAG_GPS_FIX); } 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 } } 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 } } 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) frameOK = 1; } checksum_param = 0; } else { if (offset < 15) string[offset++] = c; if (!checksum_param) parity ^= c; } return frameOK && (frame == FRAME_GGA); }
static bool gpsNewFrameNMEA(char c) { static gpsDataNmea_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, gps_frame = NO_FRAME; static uint8_t svMessageNum = 0; uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0; 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; if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'S' && string[4] == 'V') gps_frame = FRAME_GSV; } 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: //************* 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; 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; } GPS_svInfoReceivedCount++; 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; } 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; }
static int gpsNewFrameNMEA(char c) { uint8_t frameOK = 0; static uint8_t param = 0, offset = 0, parity = 0; static char string[15]; static uint8_t checksum_param, frame = 0; if (c == '$') { param = 0; offset = 0; parity = 0; } else if (c == ',' || c == '*') { string[offset] = 0; if (param == 0) { // frame identification frame = 0; if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') 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) { gpsData.lat = GPS_coord_to_degrees(string); } else if (param == 3 && string[0] == 'S') gpsData.lat = -gpsData.lat; else if (param == 4) { gpsData.lon = GPS_coord_to_degrees(string); } else if (param == 5 && string[0] == 'W') gpsData.lon = -gpsData.lon; else if (param == 6) { gpsData.fix = string[0] > '0'; } else if (param == 7) { gpsData.numSatellites = grab_fields(string, 0); } else if (param == 9) { gpsData.height = stringToFloat(string); } } else if (frame == FRAME_RMC) { if (param == 7) { gpsData.speed = stringToFloat(string) * 51.44444f; // speed in cm/s } } 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) frameOK = 1; } checksum_param = 0; } else { if (offset < 15) string[offset++] = c; if (!checksum_param) parity ^= c; } if (frame) gpsData.present = 1; return frameOK && (frame == FRAME_GGA); }
static bool GPS_newFrame(char c) { uint8_t frameOK = 0; static uint8_t param = 0, offset = 0, parity = 0; static char string[15]; static uint8_t checksum_param, frame = 0; if (c == '$') { param = 0; offset = 0; parity = 0; } else if (c == ',' || c == '*') { string[offset] = 0; if (param == 0) { //frame identification frame = 0; if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') 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_latitude = GPS_coord_to_degrees(string); } else if (param == 3 && string[0] == 'S') GPS_latitude = -GPS_latitude; else if (param == 4) { GPS_longitude = GPS_coord_to_degrees(string); } else if (param == 5 && string[0] == 'W') GPS_longitude = -GPS_longitude; else if (param == 6) { 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 } else if (frame == FRAME_RMC) { if (param == 7) { GPS_speed = ((uint32_t) grab_fields(string, 1) * 514444L) / 100000L; } // speed in cm/s added by Mis if (param == 8) { GPS_heading = grab_fields(string, 0); } // heading } 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) frameOK = 1; } checksum_param = 0; } else { if (offset < 15) string[offset++] = c; if (!checksum_param) parity ^= c; } return frameOK && (frame == FRAME_GGA); }
static bool gpsNewFrameNMEA(char c) { static gpsDataNmea_t gps_Msg; uint8_t frameOK = 0; static uint8_t param = 0, offset = 0, parity = 0; static char string[NMEA_BUFFER_SIZE]; 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 (strcmp(string, "GPGGA") == 0 || strcmp(string, "GNGGA") == 0) { gps_frame = FRAME_GGA; } else if (strcmp(string, "GPRMC") == 0 || strcmp(string, "GNRMC") == 0) { 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') { gps_Msg.fix = true; } else { gps_Msg.fix = false; } break; case 7: gps_Msg.numSat = grab_fields(string, 0); break; case 8: gps_Msg.hdop = grab_fields(string, 1) * 10; // hdop break; case 9: gps_Msg.altitude = grab_fields(string, 1) * 10; // altitude in cm break; } 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; 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) { gpsStats.packetCount++; switch (gps_frame) { case FRAME_GGA: frameOK = 1; gpsSol.numSat = gps_Msg.numSat; if (gps_Msg.fix) { gpsSol.fixType = GPS_FIX_3D; // NMEA doesn't report fix type, assume 3D gpsSol.llh.lat = gps_Msg.latitude; gpsSol.llh.lon = gps_Msg.longitude; gpsSol.llh.alt = gps_Msg.altitude; // EPH/EPV are unreliable for NMEA as they are not real accuracy gpsSol.hdop = gpsConstrainHDOP(gps_Msg.hdop); gpsSol.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); gpsSol.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); gpsSol.flags.validEPE = 0; } else { gpsSol.fixType = GPS_NO_FIX; } // NMEA does not report VELNED gpsSol.flags.validVelNE = 0; gpsSol.flags.validVelD = 0; break; case FRAME_RMC: gpsSol.groundSpeed = gps_Msg.speed; gpsSol.groundCourse = gps_Msg.ground_course; break; } // end switch } else { gpsStats.errors++; } } checksum_param = 0; break; default: if (offset < (NMEA_BUFFER_SIZE-1)) { // leave 1 byte to trailing zero string[offset++] = c; // only checksum if character is recorded and used (will cause checksum failure on dropped characters) if (!checksum_param) parity ^= c; } } return frameOK; }
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; uint8_t frameOK = 0; static uint8_t param = 0, offset = 0, parity = 0; static char string[15]; 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; } 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; 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 (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: GPS_speed = gps_Msg.speed; GPS_ground_course = gps_Msg.ground_course; break; } // end switch } } checksum_param = 0; break; default: if (offset < 15) string[offset++] = c; if (!checksum_param) parity ^= c; } return frameOK; }