mirror of https://github.com/ArduPilot/ardupilot
Implement a completely new NMEA parser based on the TinyGPS parser by Mikal Hart.
Major new features: - No RAM buffer for the NMEA message being parsed; saves 120 bytes of RAM. - More robust parser, a little less likely to be confused by bad messages. - Added configuration strings for SiRF, ublox and MediaTek GPS in NMEA mode to select just the messages of interest. Note that the issue that TinyGPS has with co-ordinates with "different" numbers of decimal digits is addressed in this code. Size is comparable to the old parser. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1468 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com
parent
3ac2193d48
commit
e27af48391
|
|
@ -1,244 +1,380 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
/*
|
||||
GPS_NMEA.cpp - Generic NMEA GPS library for Arduino
|
||||
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 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.
|
||||
//
|
||||
// NMEA parser, adapted by Michael Smith from TinyGPS v9:
|
||||
//
|
||||
// TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
|
||||
// Copyright (C) 2008-9 Mikal Hart
|
||||
// All rights reserved.
|
||||
//
|
||||
// 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.
|
||||
//
|
||||
// This library is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
// Lesser General Public License for more details.
|
||||
//
|
||||
|
||||
GPS configuration : NMEA protocol
|
||||
Baud rate : 38400
|
||||
NMEA Sentences :
|
||||
$GPGGA : Global Positioning System fix Data
|
||||
$GPVTG : Ttack and Ground Speed
|
||||
/// @file AP_GPS_NMEA.cpp
|
||||
/// @brief NMEA protocol parser
|
||||
///
|
||||
/// This is a lightweight NMEA parser, derived originally from the
|
||||
/// TinyGPS parser by Mikal Hart.
|
||||
///
|
||||
|
||||
Methods:
|
||||
init() : GPS Initialization
|
||||
read() : Call this funcion as often as you want to ensure you read the incomming gps data
|
||||
#include <FastSerial.h>
|
||||
#include <AP_Common.h>
|
||||
|
||||
Properties:
|
||||
latitude : latitude * 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)
|
||||
new_data : 1 when a new data is received.
|
||||
You need to write a 0 to new_data 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
|
||||
#include <avr/pgmspace.h>
|
||||
#include <ctype.h>
|
||||
#include <stdint.h>
|
||||
|
||||
NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset)
|
||||
*/
|
||||
#include "AP_GPS_NMEA.h"
|
||||
|
||||
// SiRF init messages //////////////////////////////////////////////////////////
|
||||
//
|
||||
// Note that we will only see a SiRF in NMEA mode if we are explicitly configured
|
||||
// for NMEA. GPS_AUTO will try to set any SiRF unit to binary mode as part of
|
||||
// the autodetection process.
|
||||
//
|
||||
const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM =
|
||||
"$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz
|
||||
"$PSRF103,1,0,0,1*25\r\n" // GLL off
|
||||
"$PSRF103,2,0,0,1*26\r\n" // GSA off
|
||||
"$PSRF103,3,0,0,1*27\r\n" // GSV off
|
||||
"$PSRF103,4,0,1,1*20\r\n" // RMC off
|
||||
"$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz
|
||||
"$PSRF103,6,0,0,1*22\r\n" // MSS off
|
||||
"$PSRF103,8,0,0,1*2C\r\n" // ZDA off
|
||||
"$PSRF151,1*3F\r\n" // WAAS on (not always supported)
|
||||
"$PSRF106,21*0F\r\n" // datum = WGS84
|
||||
"";
|
||||
|
||||
// MediaTek init messages //////////////////////////////////////////////////////
|
||||
//
|
||||
// Note that we may see a MediaTek in NMEA mode if we are connected to a non-DIYDrones
|
||||
// MediaTek-based GPS.
|
||||
//
|
||||
const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM =
|
||||
"$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" // GGA & VTG once every fix
|
||||
"$PMTK330,0*2E" // datum = WGS84
|
||||
"$PMTK313,1*2E\r\n" // SBAS on
|
||||
"$PMTK301,2*2E\r\n" // use SBAS data for DGPS
|
||||
"";
|
||||
|
||||
// ublox init messages /////////////////////////////////////////////////////////
|
||||
//
|
||||
// Note that we will only see a ublox in NMEA mode if we are explicitly configured
|
||||
// for NMEA. GPS_AUTO will try to set any ublox unit to binary mode as part of
|
||||
// the autodetection process.
|
||||
//
|
||||
// We don't attempt to send $PUBX,41 as the unit must already be talking NMEA
|
||||
// and we don't know the baudrate.
|
||||
//
|
||||
const prog_char AP_GPS_NMEA::_ublox_init_string[] PROGMEM =
|
||||
"$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" // GGA on at one per fix
|
||||
"$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" // VTG on at one per fix
|
||||
"$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" // RMC off (XXX suppress other message types?)
|
||||
"";
|
||||
|
||||
// NMEA message identifiers ////////////////////////////////////////////////////
|
||||
//
|
||||
const char AP_GPS_NMEA::_gprmc_string[] PROGMEM = "GPRMC";
|
||||
const char AP_GPS_NMEA::_gpgga_string[] PROGMEM = "GPGGA";
|
||||
const char AP_GPS_NMEA::_gpvtg_string[] PROGMEM = "GPVTG";
|
||||
|
||||
// Convenience macros //////////////////////////////////////////////////////////
|
||||
//
|
||||
#define DIGIT_TO_VAL(_x) (_x - '0')
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s)
|
||||
AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) :
|
||||
GPS(s)
|
||||
{
|
||||
FastSerial *fs = (FastSerial *)_port;
|
||||
|
||||
// Re-open the port with enough receive buffering for the messages we expect
|
||||
// and very little tx buffering, since we don't care about sending.
|
||||
// Leave the port speed alone as we don't actually know at what rate we're running...
|
||||
//
|
||||
fs->begin(0, 200, 16);
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void
|
||||
AP_GPS_NMEA::init(void)
|
||||
void AP_GPS_NMEA::init(void)
|
||||
{
|
||||
//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);
|
||||
BetterStream *bs = (BetterStream *)_port;
|
||||
|
||||
// send the SiRF init strings
|
||||
bs->print_P(_SiRF_init_string);
|
||||
|
||||
// send the MediaTek init strings
|
||||
bs->print_P(_MTK_init_string);
|
||||
|
||||
// send the ublox init strings
|
||||
bs->print_P(_ublox_init_string);
|
||||
}
|
||||
|
||||
// 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.
|
||||
bool
|
||||
AP_GPS_NMEA::read(void)
|
||||
bool AP_GPS_NMEA::read(void)
|
||||
{
|
||||
char c;
|
||||
char data;
|
||||
int numc;
|
||||
int i;
|
||||
bool parsed = false;
|
||||
|
||||
numc = _port->available();
|
||||
|
||||
if (numc > 0){
|
||||
for (i = 0; i < numc; i++){
|
||||
c = _port->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;
|
||||
parsed = 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
|
||||
}
|
||||
}
|
||||
while (numc--) {
|
||||
if (_decode(_port->read())) {
|
||||
parsed = true;
|
||||
}
|
||||
}
|
||||
return parsed;
|
||||
}
|
||||
|
||||
/****************************************************************
|
||||
*
|
||||
* * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/
|
||||
// Private Methods //////////////////////////////////////////////////////////////
|
||||
bool
|
||||
AP_GPS_NMEA::parse_nmea_gps(void)
|
||||
bool AP_GPS_NMEA::_decode(char c)
|
||||
{
|
||||
uint8_t NMEA_check;
|
||||
long aux_deg;
|
||||
long aux_min;
|
||||
char *parseptr;
|
||||
bool valid_sentence = false;
|
||||
|
||||
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");
|
||||
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)
|
||||
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)
|
||||
//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
|
||||
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");
|
||||
return false;
|
||||
}
|
||||
switch (c) {
|
||||
case ',': // term terminators
|
||||
_parity ^= c;
|
||||
case '\r':
|
||||
case '\n':
|
||||
case '*':
|
||||
if (_term_offset < sizeof(_term)) {
|
||||
_term[_term_offset] = 0;
|
||||
valid_sentence = _term_complete();
|
||||
}
|
||||
} 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, 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");
|
||||
return false;
|
||||
}
|
||||
++_term_number;
|
||||
_term_offset = 0;
|
||||
_is_checksum_term = c == '*';
|
||||
return valid_sentence;
|
||||
|
||||
case '$': // sentence begin
|
||||
_term_number = _term_offset = 0;
|
||||
_parity = 0;
|
||||
_sentence_type = _GPS_SENTENCE_OTHER;
|
||||
_is_checksum_term = false;
|
||||
_gps_data_good = false;
|
||||
return valid_sentence;
|
||||
}
|
||||
|
||||
// ordinary characters
|
||||
if (_term_offset < sizeof(_term) - 1)
|
||||
_term[_term_offset++] = c;
|
||||
if (!_is_checksum_term)
|
||||
_parity ^= c;
|
||||
|
||||
return valid_sentence;
|
||||
}
|
||||
|
||||
//
|
||||
// internal utilities
|
||||
//
|
||||
int AP_GPS_NMEA::_from_hex(char a)
|
||||
{
|
||||
if (a >= 'A' && a <= 'F')
|
||||
return a - 'A' + 10;
|
||||
else if (a >= 'a' && a <= 'f')
|
||||
return a - 'a' + 10;
|
||||
else
|
||||
return a - '0';
|
||||
}
|
||||
|
||||
unsigned long AP_GPS_NMEA::_parse_decimal()
|
||||
{
|
||||
char *p = _term;
|
||||
unsigned long ret = 100UL * atol(p);
|
||||
while (isdigit(*p))
|
||||
++p;
|
||||
if (*p == '.') {
|
||||
if (isdigit(p[1])) {
|
||||
ret += 10 * (p[1] - '0');
|
||||
if (isdigit(p[2]))
|
||||
ret += p[2] - '0';
|
||||
}
|
||||
} else {
|
||||
bufferidx = 0;
|
||||
_error("GPSERR: Bad sentence!!\n");
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
unsigned long AP_GPS_NMEA::_parse_degrees()
|
||||
{
|
||||
char *p, *q;
|
||||
uint8_t deg = 0, min = 0;
|
||||
unsigned int frac_min = 0;
|
||||
unsigned long result;
|
||||
|
||||
// scan for decimal point or end of field
|
||||
for (p = _term; isdigit(*p); p++)
|
||||
;
|
||||
q = _term;
|
||||
|
||||
// convert degrees
|
||||
while ((p - q) > 2) {
|
||||
if (deg)
|
||||
deg *= 10;
|
||||
deg += DIGIT_TO_VAL(*q++);
|
||||
}
|
||||
|
||||
// convert minutes
|
||||
while (p > q) {
|
||||
if (min)
|
||||
min *= 10;
|
||||
min += DIGIT_TO_VAL(*q++);
|
||||
}
|
||||
|
||||
// convert fractional minutes
|
||||
// expect up to four digits, result is in
|
||||
// ten-thousandths of a minute
|
||||
if (*p == '.') {
|
||||
q = p + 1;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
frac_min *= 10;
|
||||
if (isdigit(*q))
|
||||
frac_min += *q++ - '0';
|
||||
}
|
||||
}
|
||||
return deg * 100000UL + (min * 10000UL + frac_min) / 6;
|
||||
}
|
||||
|
||||
// Processes a just-completed term
|
||||
// Returns true if new sentence has just passed checksum test and is validated
|
||||
bool AP_GPS_NMEA::_term_complete()
|
||||
{
|
||||
// handle the last term in a message
|
||||
if (_is_checksum_term) {
|
||||
uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]);
|
||||
if (checksum == _parity) {
|
||||
if (_gps_data_good) {
|
||||
switch (_sentence_type) {
|
||||
case _GPS_SENTENCE_GPRMC:
|
||||
time = _new_time;
|
||||
date = _new_date;
|
||||
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
|
||||
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
|
||||
ground_speed = _new_speed;
|
||||
ground_course = _new_course;
|
||||
num_sats = _new_satellite_count;
|
||||
hdop = _new_hdop;
|
||||
fix = true;
|
||||
break;
|
||||
case _GPS_SENTENCE_GPGGA:
|
||||
altitude = _new_altitude;
|
||||
time = _new_time;
|
||||
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
|
||||
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
|
||||
fix = true;
|
||||
break;
|
||||
case _GPS_SENTENCE_VTG:
|
||||
ground_speed = _new_speed;
|
||||
ground_course = _new_course;
|
||||
// VTG has no fix indicator, can't change fix status
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
switch (_sentence_type) {
|
||||
case _GPS_SENTENCE_GPRMC:
|
||||
case _GPS_SENTENCE_GPGGA:
|
||||
// Only these sentences give us information about
|
||||
// fix status.
|
||||
fix = false;
|
||||
}
|
||||
}
|
||||
// we got a good message
|
||||
return true;
|
||||
}
|
||||
// we got a bad message, ignore it
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
// Parse hexadecimal numbers
|
||||
uint8_t
|
||||
AP_GPS_NMEA::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
|
||||
AP_GPS_NMEA::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
|
||||
AP_GPS_NMEA::parsenumber(char *str, uint8_t numdec) {
|
||||
long d = 0;
|
||||
uint8_t ndec = 0;
|
||||
|
||||
while (str[0] != 0) {
|
||||
if (str[0] == '.'){
|
||||
ndec = 1;
|
||||
// the first term determines the sentence type
|
||||
if (_term_number == 0) {
|
||||
if (!strcmp_P(_term, _gprmc_string)) {
|
||||
_sentence_type = _GPS_SENTENCE_GPRMC;
|
||||
} else if (!strcmp_P(_term, _gpgga_string)) {
|
||||
_sentence_type = _GPS_SENTENCE_GPGGA;
|
||||
} else if (!strcmp_P(_term, _gpvtg_string)) {
|
||||
_sentence_type = _GPS_SENTENCE_VTG;
|
||||
// VTG may not contain a data qualifier, presume the solution is good
|
||||
// unless it tells us otherwise.
|
||||
_gps_data_good = true;
|
||||
} 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;
|
||||
_sentence_type = _GPS_SENTENCE_OTHER;
|
||||
}
|
||||
str++;
|
||||
return false;
|
||||
}
|
||||
return d;
|
||||
|
||||
// 10 = RMC, 20 = GGA, 30 = VTG
|
||||
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
|
||||
switch (_sentence_type + _term_number) {
|
||||
// operational status
|
||||
//
|
||||
case 12: // validity (RMC)
|
||||
_gps_data_good = _term[0] == 'A';
|
||||
break;
|
||||
case 26: // Fix data (GGA)
|
||||
_gps_data_good = _term[0] > '0';
|
||||
break;
|
||||
case 39: // validity (VTG) (we may not see this field)
|
||||
_gps_data_good = _term[0] != 'N';
|
||||
break;
|
||||
case 27: // satellite count (GGA)
|
||||
_new_satellite_count = atol(_term);
|
||||
break;
|
||||
case 28: // HDOP (GGA)
|
||||
_new_hdop = _parse_decimal();
|
||||
break;
|
||||
|
||||
// time and date
|
||||
//
|
||||
case 11: // Time (RMC)
|
||||
case 21: // Time (GGA)
|
||||
_new_time = _parse_decimal();
|
||||
break;
|
||||
case 19: // Date (GPRMC)
|
||||
_new_date = atol(_term);
|
||||
break;
|
||||
|
||||
// location
|
||||
//
|
||||
case 13: // Latitude
|
||||
case 22:
|
||||
_new_latitude = _parse_degrees();
|
||||
break;
|
||||
case 14: // N/S
|
||||
case 23:
|
||||
if (_term[0] == 'S')
|
||||
_new_latitude = -_new_latitude;
|
||||
break;
|
||||
case 15: // Longitude
|
||||
case 24:
|
||||
_new_longitude = _parse_degrees();
|
||||
break;
|
||||
case 16: // E/W
|
||||
case 25:
|
||||
if (_term[0] == 'W')
|
||||
_new_longitude = -_new_longitude;
|
||||
break;
|
||||
case 29: // Altitude (GPGGA)
|
||||
_new_altitude = _parse_decimal();
|
||||
break;
|
||||
|
||||
// course and speed
|
||||
//
|
||||
case 17: // Speed (GPRMC)
|
||||
_new_speed = (_parse_decimal() * 514) / 100; // knots-> m/sec, approximiates * 0.514
|
||||
break;
|
||||
case 37: // Speed (VTG)
|
||||
_new_speed = _parse_decimal();
|
||||
break;
|
||||
case 18: // Course (GPRMC)
|
||||
case 31: // Course (VTG)
|
||||
_new_course = _parse_decimal();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,59 +1,160 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
//
|
||||
// NMEA parser, adapted by Michael Smith from TinyGPS v9:
|
||||
//
|
||||
// TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
|
||||
// Copyright (C) 2008-9 Mikal Hart
|
||||
// All rights reserved.
|
||||
//
|
||||
// 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.
|
||||
//
|
||||
// This library is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
// Lesser General Public License for more details.
|
||||
//
|
||||
|
||||
/// @file AP_GPS_NMEA.h
|
||||
/// @brief NMEA protocol parser
|
||||
///
|
||||
/// This is a lightweight NMEA parser, derived originally from the
|
||||
/// TinyGPS parser by Mikal Hart. It is frugal in its use of memory
|
||||
/// and tries to avoid unnecessary arithmetic.
|
||||
///
|
||||
/// The parser handles GPGGA, GPRMC and GPVTG messages, and attempts to be
|
||||
/// robust in the face of occasional corruption in the input stream. It
|
||||
/// makes a basic effort to configure GPS' that are likely to be connected in
|
||||
/// NMEA mode (SiRF, MediaTek and ublox) to emit the correct message
|
||||
/// stream, but does not validate that the correct stream is being received.
|
||||
/// In particular, a unit emitting just GPRMC will show as having a fix
|
||||
/// even though no altitude data is being received.
|
||||
///
|
||||
/// GPVTG data is parsed, but as the message may not contain the the
|
||||
/// qualifier field (this is common with e.g. older SiRF units) it is
|
||||
/// not considered a source of fix-valid information.
|
||||
///
|
||||
|
||||
|
||||
#ifndef AP_GPS_NMEA_h
|
||||
#define AP_GPS_NMEA_h
|
||||
|
||||
#include <GPS.h>
|
||||
#define GPS_BUFFERSIZE 120
|
||||
#include <avr/pgmspace.h>
|
||||
|
||||
#define NMEA_OUTPUT_SENTENCES "$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GPGGA and GPVTG
|
||||
|
||||
#define NMEA_BAUD_RATE_4800 "$PSRF100,1,4800,8,1,0*0E\r\n"
|
||||
#define NMEA_BAUD_RATE_9600 "$PSRF100,1,9600,8,1,0*0D\r\n"
|
||||
#define NMEA_BAUD_RATE_19200 "$PSRF100,1,19200,8,1,0*38\r\n"
|
||||
#define NMEA_BAUD_RATE_38400 "$PSRF100,1,38400,8,1,0*3D\r\n"
|
||||
#define NMEA_BAUD_RATE_57600 "$PSRF100,1,57600,8,1,0*36\r\n"
|
||||
|
||||
#define NMEA_OUTPUT_1HZ "$PMTK220,1000*1F\r\n"
|
||||
#define NMEA_OUTPUT_2HZ "$PMTK220,500*2B\r\n"
|
||||
#define NMEA_OUTPUT_4HZ "$PMTK220,250*29\r\n"
|
||||
#define NMEA_OTUPUT_5HZ "$PMTK220,200*2C\r\n"
|
||||
#define NMEA_OUTPUT_10HZ "$PMTK220,100*2F\r\n"
|
||||
|
||||
#define SBAS_ON "$PMTK313,1*2E\r\n"
|
||||
#define SBAS_OFF "$PMTK313,0*2F\r\n"
|
||||
|
||||
#define WAAS_ON "$PSRF151,1*3F\r\n"
|
||||
#define WAAS_OFF "$PSRF151,0*3E\r\n"
|
||||
|
||||
#define DGPS_OFF "$PMTK301,0*2C\r\n"
|
||||
#define DGPS_RTCM "$PMTK301,1*2D\r\n"
|
||||
#define DGPS_SBAS "$PMTK301,2*2E\r\n"
|
||||
|
||||
#define DATUM_GOOGLE "$PMTK330,0*2E\r\n"
|
||||
|
||||
/// NMEA parser
|
||||
///
|
||||
class AP_GPS_NMEA : public GPS
|
||||
{
|
||||
public:
|
||||
// Methods
|
||||
public:
|
||||
/// Constructor
|
||||
///
|
||||
AP_GPS_NMEA(Stream *s);
|
||||
virtual void init();
|
||||
virtual bool read();
|
||||
|
||||
// Properties
|
||||
uint8_t quality; // GPS Signal quality
|
||||
/// Perform a (re)initialisation of the GPS; sends the
|
||||
/// protocol configuration messages.
|
||||
///
|
||||
virtual void init();
|
||||
|
||||
private:
|
||||
// Internal variables
|
||||
uint8_t GPS_checksum;
|
||||
uint8_t GPS_checksum_calc;
|
||||
char buffer[GPS_BUFFERSIZE];
|
||||
int bufferidx;
|
||||
/// Checks the serial receive buffer for characters,
|
||||
/// attempts to parse NMEA data and updates internal state
|
||||
/// accordingly.
|
||||
///
|
||||
virtual bool read();
|
||||
|
||||
bool parse_nmea_gps(void);
|
||||
uint8_t parseHex(char c);
|
||||
long parsedecimal(char *str,uint8_t num_car);
|
||||
long parsenumber(char *str,uint8_t numdec);
|
||||
private:
|
||||
/// Coding for the GPS sentences that the parser handles
|
||||
enum _sentence_types {
|
||||
_GPS_SENTENCE_GPRMC = 10,
|
||||
_GPS_SENTENCE_GPGGA = 20,
|
||||
_GPS_SENTENCE_VTG = 30,
|
||||
_GPS_SENTENCE_OTHER = 0
|
||||
};
|
||||
|
||||
/// Update the decode state machine with a new character
|
||||
///
|
||||
/// @param c The next character in the NMEA input stream
|
||||
/// @returns True if processing the character has resulted in
|
||||
/// an update to the GPS state
|
||||
///
|
||||
bool _decode(char c);
|
||||
|
||||
/// Return the numeric value of an ascii hex character
|
||||
///
|
||||
/// @param a The character to be converted
|
||||
/// @returns The value of the character as a hex digit
|
||||
///
|
||||
int _from_hex(char a);
|
||||
|
||||
/// Parses the current term as a NMEA-style decimal number with
|
||||
/// up to two decimal digits.
|
||||
///
|
||||
/// @returns The value expressed by the string in _term,
|
||||
/// multiplied by 100.
|
||||
///
|
||||
unsigned long _parse_decimal();
|
||||
|
||||
/// Parses the current term as a NMEA-style degrees + minutes
|
||||
/// value with up to four decimal digits.
|
||||
///
|
||||
/// This gives a theoretical resolution limit of around 18cm.
|
||||
///
|
||||
/// @returns The value expressed by the string in _term,
|
||||
/// multiplied by 10000.
|
||||
///
|
||||
unsigned long _parse_degrees();
|
||||
|
||||
/// Processes the current term when it has been deemed to be
|
||||
/// complete.
|
||||
///
|
||||
/// Each GPS message is broken up into terms separated by commas.
|
||||
/// Each term is then processed by this function as it is received.
|
||||
///
|
||||
/// @returns True if completing the term has resulted in
|
||||
/// an update to the GPS state.
|
||||
bool _term_complete();
|
||||
|
||||
uint8_t _parity; ///< NMEA message checksum accumulator
|
||||
bool _is_checksum_term; ///< current term is the checksum
|
||||
char _term[15]; ///< buffer for the current term within the current sentence
|
||||
uint8_t _sentence_type; ///< the sentence type currently being processed
|
||||
uint8_t _term_number; ///< term index within the current sentence
|
||||
uint8_t _term_offset; ///< character offset with the term being received
|
||||
bool _gps_data_good; ///< set when the sentence indicates data is good
|
||||
|
||||
// The result of parsing terms within a message is stored temporarily until
|
||||
// the message is completely processed and the checksum validated.
|
||||
// This avoids the need to buffer the entire message.
|
||||
long _new_time; ///< time parsed from a term
|
||||
long _new_date; ///< date parsed from a term
|
||||
long _new_latitude; ///< latitude parsed from a term
|
||||
long _new_longitude; ///< longitude parsed from a term
|
||||
long _new_altitude; ///< altitude parsed from a term
|
||||
long _new_speed; ///< speed parsed from a term
|
||||
long _new_course; ///< course parsed from a term
|
||||
int _new_hdop; ///< HDOP parsed from a term
|
||||
uint8_t _new_satellite_count; ///< satellite count parsed from a term
|
||||
|
||||
/// @name Init strings
|
||||
/// In ::init, an attempt is made to configure the GPS
|
||||
/// unit to send just the messages that we are interested
|
||||
/// in using these strings
|
||||
//@{
|
||||
static const prog_char _SiRF_init_string[]; ///< init string for SiRF units
|
||||
static const prog_char _MTK_init_string[]; ///< init string for MediaTek units
|
||||
static const prog_char _ublox_init_string[]; ///< init string for ublox units
|
||||
//@}
|
||||
|
||||
/// @name GPS message identifier strings
|
||||
//@{
|
||||
static const prog_char _gprmc_string[];
|
||||
static const prog_char _gpgga_string[];
|
||||
static const prog_char _gpvtg_string[];
|
||||
//@}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue