Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Fixed issue with comma parsing (space after comma was causing routine to misread data)

Browse Source 
Added init functions (PMTK)

git-svn-id: https://arducopter.googlecode.com/svn/trunk@663 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
paulbmather@gmail.com 2010-10-16 16:55:56 +00:00
parent 778c68d5ef
commit d7e71b602c
1 changed files with 24 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -2,6 +2,7 @@
/* /*
GPS_NMEA.cpp - Generic NMEA GPS library for Arduino GPS_NMEA.cpp - Generic NMEA GPS library for Arduino
Code by Jordi Muñoz and Jose Julio. DIYDrones.com Code by Jordi Muñoz and Jose Julio. DIYDrones.com
Edits by HappyKillmore
This code works with boards based on ATMega168 / 328 and ATMega1280 (Serial port 1) This code works with boards based on ATMega168 / 328 and ATMega1280 (Serial port 1)
This library is free software; you can redistribute it and / or This library is free software; you can redistribute it and / or
@ -50,6 +51,12 @@ void
AP_GPS_NMEA::init(void) AP_GPS_NMEA::init(void)
{ {
//Type = 2; //Type = 2;
// initialize serial port for binary protocol use
_port->print(NMEA_OUTPUT_SENTENCES);
_port->print(NMEA_OUTPUT_4HZ);
_port->print(SBAS_ON);
_port->print(DGPS_SBAS);
_port->print(DATUM_GOOGLE);
} }
// This code don´t wait for data, only proccess the data available on serial port // This code don´t wait for data, only proccess the data available on serial port
@ -79,7 +86,7 @@ AP_GPS_NMEA::update(void)
parse_nmea_gps(); parse_nmea_gps();
} else { } else {
if (bufferidx < (GPS_BUFFERSIZE - 1)){ if (bufferidx < (GPS_BUFFERSIZE - 1)){
if (c == ' * ') if (c == '*')
GPS_checksum_calc = false; // Checksum calculation end GPS_checksum_calc = false; // Checksum calculation end
buffer[bufferidx++] = c; buffer[bufferidx++] = c;
if (GPS_checksum_calc){ if (GPS_checksum_calc){
@ -112,33 +119,33 @@ AP_GPS_NMEA::parse_nmea_gps(void)
if (GPS_checksum == NMEA_check){ // Checksum validation if (GPS_checksum == NMEA_check){ // Checksum validation
//Serial.println("buffer"); //Serial.println("buffer");
new_data = 1; // New GPS Data new_data = 1; // New GPS Data
parseptr = strchr(buffer, ', ')+1; parseptr = strchr(buffer, ',')+1;
//parseptr = strchr(parseptr, ',')+1; //parseptr = strchr(parseptr, ',')+1;
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ',')+1;
// //
aux_deg = parsedecimal(parseptr, 2); // degrees aux_deg = parsedecimal(parseptr, 2); // degrees
aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal 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) latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5)
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ',')+1;
if ( * parseptr == 'S') if ( * parseptr == 'S')
latitude = -1 * latitude; // South latitudes are negative latitude = -1 * latitude; // South latitudes are negative
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ',')+1;
// W longitudes are Negative // W longitudes are Negative
aux_deg = parsedecimal(parseptr, 3); // degrees aux_deg = parsedecimal(parseptr, 3); // degrees
aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal) aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal)
longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000)
//longitude = -1*longitude; // This Assumes that we are in W longitudes... //longitude = -1*longitude; // This Assumes that we are in W longitudes...
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ',')+1;
if ( * parseptr == 'W') if ( * parseptr == 'W')
longitude = -1 * longitude; // West longitudes are negative longitude = -1 * longitude; // West longitudes are negative
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ',')+1;
fix = parsedecimal(parseptr, 1); fix = parsedecimal(parseptr, 1);
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ',')+1;
num_sats = parsedecimal(parseptr, 2); num_sats = parsedecimal(parseptr, 2);
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ',')+1;
HDOP = parsenumber(parseptr, 1); // HDOP * 10 HDOP = parsenumber(parseptr, 1); // HDOP * 10
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ',')+1;
altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters
if (fix < 1) if (fix < 1)
quality = 0; // No FIX quality = 0; // No FIX
@ -159,14 +166,14 @@ AP_GPS_NMEA::parse_nmea_gps(void)
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation if (GPS_checksum == NMEA_check){ // Checksum validation
parseptr = strchr(buffer, ', ')+1; parseptr = strchr(buffer, ',')+1;
ground_course = parsenumber(parseptr, 2); // Ground course in degrees * 100 ground_course = parsenumber(parseptr, 2); // 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;
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, 2) * 10 / 36; // Convert Km / h to m / s ( * 100) ground_speed = parsenumber(parseptr, 2) * 10 / 36; // Convert Km / h to m / s ( * 100)
//GPS_line = true; //GPS_line = true;
} else { } else {