You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
272 lines
8.5 KiB
272 lines
8.5 KiB
/* |
|
GPS_NMEA.cpp - Generic NMEA GPS library for Arduino |
|
Code by Jordi Muñoz and Jose Julio. DIYDrones.com |
|
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 |
|
modify it under the terms of the GNU Lesser General Public |
|
License as published by the Free Software Foundation; either |
|
version 2.1 of the License, or (at your option) any later version. |
|
|
|
GPS configuration : NMEA protocol |
|
Baud rate : 38400 |
|
NMEA Sentences : |
|
$GPGGA : Global Positioning System Fix Data |
|
$GPVTG : Ttack and Ground Speed |
|
|
|
Methods: |
|
Init() : GPS Initialization |
|
Read() : Call this funcion as often as you want to ensure you read the incomming gps data |
|
|
|
Properties: |
|
Lattitude : Lattitude * 10000000 (long value) |
|
Longitude : Longitude * 10000000 (long value) |
|
Altitude : Altitude * 1000 (milimeters) (long value) |
|
Ground_speed : Speed (m/s) * 100 (long value) |
|
Ground_course : Course (degrees) * 100 (long value) |
|
Type : 2 (This indicate that we are using the Generic NMEA library) |
|
NewData : 1 when a new data is received. |
|
You need to write a 0 to NewData when you read the data |
|
Fix : >=1: GPS FIX, 0: No Fix (normal logic) |
|
Quality : 0 = No Fix |
|
1 = Bad (Num sats < 5) |
|
2 = Poor |
|
3 = Medium |
|
4 = Good |
|
|
|
NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset) |
|
*/ |
|
|
|
#include "GPS_NMEA.h" |
|
|
|
#include <avr/interrupt.h> |
|
#include "WProgram.h" |
|
|
|
|
|
// Constructors //////////////////////////////////////////////////////////////// |
|
GPS_NMEA_Class::GPS_NMEA_Class() |
|
{ |
|
} |
|
|
|
// Public Methods ////////////////////////////////////////////////////////////// |
|
void GPS_NMEA_Class::Init(void) |
|
{ |
|
Type = 2; |
|
GPS_checksum_calc = false; |
|
bufferidx = 0; |
|
NewData=0; |
|
Fix=0; |
|
Quality=0; |
|
PrintErrors=0; |
|
// Initialize serial port |
|
#if defined(__AVR_ATmega1280__) |
|
Serial1.begin(38400); // Serial port 1 on ATMega1280 |
|
#else |
|
Serial.begin(38400); |
|
#endif |
|
} |
|
|
|
// This code don´t wait for data, only proccess the data available on serial port |
|
// We can call this function on the main loop (50Hz loop) |
|
// If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info. |
|
void GPS_NMEA_Class::Read(void) |
|
{ |
|
char c; |
|
int numc; |
|
int i; |
|
|
|
|
|
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... |
|
numc = Serial1.available(); |
|
#else |
|
numc = Serial.available(); |
|
#endif |
|
if (numc > 0) |
|
for (i=0;i<numc;i++){ |
|
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... |
|
c = Serial1.read(); |
|
#else |
|
c = Serial.read(); |
|
#endif |
|
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(); |
|
} |
|
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 |
|
} |
|
} |
|
} |
|
|
|
/**************************************************************** |
|
* |
|
****************************************************************/ |
|
// Private Methods ////////////////////////////////////////////////////////////// |
|
void GPS_NMEA_Class::parse_nmea_gps(void) |
|
{ |
|
byte NMEA_check; |
|
long aux_deg; |
|
long aux_min; |
|
char *parseptr; |
|
|
|
|
|
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"); |
|
NewData = 1; // 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 |
|
Lattitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000) (0.6 = 3/5) |
|
parseptr = strchr(parseptr, ',')+1; |
|
// |
|
if (*parseptr=='S') |
|
Lattitude = -1*Lattitude; // South Lattitudes 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) |
|
//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; |
|
NumSats = 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 |
|
if (Fix < 1) |
|
Quality = 0; // No FIX |
|
else if(NumSats<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 |
|
{ |
|
if (PrintErrors) |
|
Serial.println("GPSERR: Checksum error!!"); |
|
} |
|
} |
|
} |
|
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 |
|
parseptr = strchr(buffer, ',')+1; |
|
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; |
|
Ground_Speed = parsenumber(parseptr,2)*10/36; // Convert Km/h to m/s (*100) |
|
//GPS_line = true; |
|
} |
|
else |
|
{ |
|
if (PrintErrors) |
|
Serial.println("GPSERR: Checksum error!!"); |
|
} |
|
} |
|
} |
|
else |
|
{ |
|
bufferidx = 0; |
|
if (PrintErrors) |
|
Serial.println("GPSERR: Bad sentence!!"); |
|
} |
|
} |
|
|
|
|
|
/**************************************************************** |
|
* |
|
****************************************************************/ |
|
// Parse hexadecimal numbers |
|
byte GPS_NMEA_Class::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 GPS_NMEA_Class::parsedecimal(char *str,byte num_car) { |
|
long d = 0; |
|
byte 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 GPS_NMEA_Class::parsenumber(char *str,byte numdec) { |
|
long d = 0; |
|
byte 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; |
|
} |
|
|
|
GPS_NMEA_Class GPS;
|