Here’s how I fixed it. I took the AP_GPS_NMEA code and stripped out all the class stuff and went through all the related files looking for variable definitions. It seems to work now. I’m getting position and some other GPS related stuff.
///< true if we have a position fix
unsigned int imu_health = 65012;
void update(void)
{
char c;
int numc;
int i;
numc = Serial.available();
if (numc > 0){
for (i = 0; i < numc; i++){
c = Serial.read();
if (c == '$'){ // NMEA Start
bufferidx = 0;
buffer[bufferidx++] = c;
GPS_checksum = 0;
GPS_checksum_calc = true;
continue;
}
if (c == '\r'){ // NMEA End
buffer[bufferidx++] = 0;
parse_nmea_gps();
Serial.print("!!!VER:");
Serial.print(SOFTWARE_VER); //output the software version
Serial.print(",");
Serial.print("LAT:");
Serial.print((float)current_loc.latitude);
Serial.print(",LON:");
Serial.print((float)current_loc.longitude);
Serial.print(",ALT:");
Serial.print((float)current_loc.altitude/1000); // meters
Serial.print(",COG:");
Serial.print((float)ground_course/1000); // degrees
Serial.print(",SOG:");
Serial.print((float)ground_speed/100);
Serial.print(",FIX:");
Serial.print((int)fix); // 1 = good fix
Serial.print(",SAT:");
Serial.print(num_sats);
Serial.print (",");
Serial.print(",IMUH:");
Serial.print((imu_health>>8)&0xff);
Serial.print(",");
Serial.print("TOW:");
Serial.print(time);
Serial.println("***");
} else {
if (bufferidx < (GPS_BUFFERSIZE - 1)){
if (c == '*')
GPS_checksum_calc = false; // Checksum calculation end
buffer[bufferidx++] = c;
if (GPS_checksum_calc){
GPS_checksum ^= c; // XOR
}
} else {
bufferidx = 0; // Buffer overflow : restart
}
}
}
}
}
void parse_nmea_gps(void)
{
uint8_t NMEA_check;
long aux_deg;
long aux_min;
char *parseptr;
static unsigned long GPS_timer = 0; //used to turn off the LED if no data is received.
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
//Serial.println("buffer");
//_setTime();
GPS_timer = DIYmillis();
valid_read = true;
new_data = true; // New GPS Data
parseptr = strchr(buffer, ',')+1;
//parseptr = strchr(parseptr, ',')+1;
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
parseptr = strchr(parseptr, ',')+1;
aux_deg = parsedecimal(parseptr, 2); // degrees
aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal
//latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5)
current_loc.latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5)
parseptr = strchr(parseptr, ',')+1;
if ( * parseptr == 'S')
latitude = -1 * latitude; // South latitudes are negative
parseptr = strchr(parseptr, ',')+1;
// W longitudes are Negative
aux_deg = parsedecimal(parseptr, 3); // degrees
aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal)
//longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000)
current_loc.longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000)
//longitude = -1*longitude; // This Assumes that we are in W longitudes...
parseptr = strchr(parseptr, ',')+1;
if ( * parseptr == 'W')
longitude = -1 * longitude; // West longitudes are negative
parseptr = strchr(parseptr, ',')+1;
fix = parsedecimal(parseptr, 1);
parseptr = strchr(parseptr, ',')+1;
num_sats = parsedecimal(parseptr, 2);
parseptr = strchr(parseptr, ',')+1;
HDOP = parsenumber(parseptr, 1); // HDOP * 10
parseptr = strchr(parseptr, ',')+1;
//altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters
current_loc.altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters
if (fix < 1)
quality = 0; // No FIX
else if(num_sats < 5)
quality = 1; // Bad (Num sats < 5)
else if(HDOP > 30)
quality = 2; // Poor (HDOP > 30)
else if(HDOP > 25)
quality = 3; // Medium (HDOP > 25)
else
quality = 4; // Good (HDOP < 25)
} else {
//_error("GPSERR: Checksum error!!\n");
}
}
} else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
//Serial.println(buffer);
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
//_setTime();
GPS_timer = DIYmillis();
valid_read = true;
new_data = true; // New GPS Data
parseptr = strchr(buffer, ',')+1;
ground_course = parsenumber(parseptr, 1) * 10; // Ground course in degrees * 100
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
ground_speed = parsenumber(parseptr, 1) * 100 / 36; // Convert Km / h to m / s ( * 100)
//GPS_line = true;
} else {
//_error("GPSERR: Checksum error!!\n");
}
}
} else {
bufferidx = 0;
//_error("GPSERR: Bad sentence!!\n");
GPS_timer = DIYmillis(); //Restarting timer...
}
}
// Parse hexadecimal numbers
uint8_t parseHex(char c) {
if (c < '0')
return (0);
if (c <= '9')
return (c - '0');
if (c < 'A')
return (0);
if (c <= 'F')
return ((c - 'A')+10);
}
// Decimal number parser
long parsedecimal(char *str, uint8_t num_car) {
long d = 0;
uint8_t i;
i = num_car;
while ((str[0] != 0) && (i > 0)) {
if ((str[0] > '9') || (str[0] < '0'))
return d;
d *= 10;
d += str[0] - '0';
str++;
i--;
}
return d;
}
// Function to parse fixed point numbers (numdec=number of decimals)
long parsenumber(char *str, uint8_t numdec) {
long d = 0;
uint8_t ndec = 0;
while (str[0] != 0) {
if (str[0] == '.'){
ndec = 1;
} else {
if ((str[0] > '9') || (str[0] < '0'))
return d;
d *= 10;
d += str[0] - '0';
if (ndec > 0)
ndec++;
if (ndec > numdec) // we reach the number of decimals...
return d;
}
str++;
}
return d;
}