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;
}
