bool gpsPollI2CNAV(void)
{
static uint32_t lastPollTime = 0;
gpsDataI2CNAV_t gpsMsg;
// Check for poll rate timeout
if ((millis() - lastPollTime) < (1000 / GPS_I2C_POLL_RATE_HZ))
return false;
lastPollTime = millis();
i2cnavGPSModuleRead(&gpsMsg);
if (gpsMsg.flags.gpsOk) {
if (gpsMsg.flags.fix3D) {
// No fix type available - assume 3D
gpsSol.fixType = GPS_FIX_3D;
}
else {
gpsSol.fixType = GPS_NO_FIX;
}
// sat count
gpsSol.numSat = gpsMsg.numSat;
// Other data
if (gpsMsg.flags.newData) {
if (gpsMsg.flags.fix3D) {
gpsSol.hdop = gpsConstrainHDOP(gpsMsg.hdop);
gpsSol.eph = gpsConstrainEPE(gpsMsg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
gpsSol.epv = gpsConstrainEPE(gpsMsg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); // i2c-nav doesn't give vdop data, fake it using hdop
gpsSol.groundSpeed = gpsMsg.speed;
gpsSol.groundCourse = gpsMsg.ground_course;
gpsSol.llh.lat = gpsMsg.latitude;
gpsSol.llh.lon = gpsMsg.longitude;
gpsSol.llh.alt = gpsMsg.altitude;
gpsSol.flags.validVelNE = 0;
gpsSol.flags.validVelD = 0;
gpsSol.flags.validEPE = 0;
}
gpsStats.packetCount++;
gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
return true;
}
}
return false;
}
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;
}
