mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
5244559010
Most of AP_Progmem is already gone so we can stop including it in most of the places. The only places that need it are the ones using pgm_read_*() APIs. In some cases the header needed to be added in the .cpp since it was removed from the .h to reduce scope. In those cases the headers were also reordered.
464 lines
14 KiB
C++
464 lines
14 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program 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 General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
//
|
|
// 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.
|
|
//
|
|
|
|
/// @file AP_GPS_NMEA.cpp
|
|
/// @brief NMEA protocol parser
|
|
///
|
|
/// This is a lightweight NMEA parser, derived originally from the
|
|
/// TinyGPS parser by Mikal Hart.
|
|
///
|
|
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#include <ctype.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
|
|
#include "AP_GPS_NMEA.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
// optionally log all NMEA data for debug purposes
|
|
// #define NMEA_LOG_PATH "nmea.log"
|
|
|
|
#ifdef NMEA_LOG_PATH
|
|
#include <stdio.h>
|
|
#endif
|
|
|
|
// 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.
|
|
//
|
|
#define SIRF_INIT_MSG \
|
|
"$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.
|
|
//
|
|
#define MTK_INIT_MSG \
|
|
"$PMTK314,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n" /* RMC, GGA & VTG once every fix */ \
|
|
"$PMTK330,0*2E\r\n" /* 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.
|
|
//
|
|
#define UBLOX_INIT_MSG \
|
|
"$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?) */
|
|
|
|
const char AP_GPS_NMEA::_initialisation_blob[] = SIRF_INIT_MSG MTK_INIT_MSG UBLOX_INIT_MSG;
|
|
|
|
// NMEA message identifiers ////////////////////////////////////////////////////
|
|
//
|
|
const char AP_GPS_NMEA::_gprmc_string[] = "GPRMC";
|
|
const char AP_GPS_NMEA::_gpgga_string[] = "GPGGA";
|
|
const char AP_GPS_NMEA::_gpvtg_string[] = "GPVTG";
|
|
|
|
// Convenience macros //////////////////////////////////////////////////////////
|
|
//
|
|
#define DIGIT_TO_VAL(_x) (_x - '0')
|
|
#define hexdigit(x) ((x)>9?'A'+(x):'0'+(x))
|
|
|
|
AP_GPS_NMEA::AP_GPS_NMEA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
|
|
AP_GPS_Backend(_gps, _state, _port),
|
|
_parity(0),
|
|
_is_checksum_term(false),
|
|
_sentence_type(0),
|
|
_term_number(0),
|
|
_term_offset(0),
|
|
_gps_data_good(false)
|
|
{
|
|
gps.send_blob_start(state.instance, _initialisation_blob, sizeof(_initialisation_blob));
|
|
// this guarantees that _term is always nul terminated
|
|
memset(_term, 0, sizeof(_term));
|
|
}
|
|
|
|
bool AP_GPS_NMEA::read(void)
|
|
{
|
|
int16_t numc;
|
|
bool parsed = false;
|
|
|
|
numc = port->available();
|
|
while (numc--) {
|
|
char c = port->read();
|
|
#ifdef NMEA_LOG_PATH
|
|
static FILE *logf = NULL;
|
|
if (logf == NULL) {
|
|
logf = fopen(NMEA_LOG_PATH, "wb");
|
|
}
|
|
if (logf != NULL) {
|
|
::fwrite(&c, 1, 1, logf);
|
|
}
|
|
#endif
|
|
if (_decode(c)) {
|
|
parsed = true;
|
|
}
|
|
}
|
|
return parsed;
|
|
}
|
|
|
|
bool AP_GPS_NMEA::_decode(char c)
|
|
{
|
|
bool valid_sentence = false;
|
|
|
|
switch (c) {
|
|
case ',': // term terminators
|
|
_parity ^= c;
|
|
/* no break */
|
|
case '\r':
|
|
case '\n':
|
|
case '*':
|
|
if (_term_offset < sizeof(_term)) {
|
|
_term[_term_offset] = 0;
|
|
valid_sentence = _term_complete();
|
|
}
|
|
++_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
|
|
//
|
|
int16_t 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';
|
|
}
|
|
|
|
uint32_t AP_GPS_NMEA::_parse_decimal_100()
|
|
{
|
|
char *p = _term;
|
|
uint32_t ret = 100UL * atol(p);
|
|
while (isdigit(*p))
|
|
++p;
|
|
if (*p == '.') {
|
|
if (isdigit(p[1])) {
|
|
ret += 10 * DIGIT_TO_VAL(p[1]);
|
|
if (isdigit(p[2]))
|
|
ret += DIGIT_TO_VAL(p[2]);
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
parse a NMEA latitude/longitude degree value. The result is in degrees*1e7
|
|
*/
|
|
uint32_t AP_GPS_NMEA::_parse_degrees()
|
|
{
|
|
char *p, *q;
|
|
uint8_t deg = 0, min = 0;
|
|
float frac_min = 0;
|
|
int32_t ret = 0;
|
|
|
|
// scan for decimal point or end of field
|
|
for (p = _term; *p && isdigit(*p); p++)
|
|
;
|
|
q = _term;
|
|
|
|
// convert degrees
|
|
while ((p - q) > 2 && *q) {
|
|
if (deg)
|
|
deg *= 10;
|
|
deg += DIGIT_TO_VAL(*q++);
|
|
}
|
|
|
|
// convert minutes
|
|
while (p > q && *q) {
|
|
if (min)
|
|
min *= 10;
|
|
min += DIGIT_TO_VAL(*q++);
|
|
}
|
|
|
|
// convert fractional minutes
|
|
if (*p == '.') {
|
|
q = p + 1;
|
|
float frac_scale = 0.1f;
|
|
while (*q && isdigit(*q)) {
|
|
frac_min += DIGIT_TO_VAL(*q) * frac_scale;
|
|
q++;
|
|
frac_scale *= 0.1f;
|
|
}
|
|
}
|
|
ret = (deg * (int32_t)10000000UL);
|
|
ret += (min * (int32_t)10000000UL / 60);
|
|
ret += (int32_t) (frac_min * (1.0e7f / 60.0f));
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
see if we have a new set of NMEA messages
|
|
*/
|
|
bool AP_GPS_NMEA::_have_new_message()
|
|
{
|
|
if (_last_GPRMC_ms == 0 ||
|
|
_last_GPGGA_ms == 0) {
|
|
return false;
|
|
}
|
|
uint32_t now = hal.scheduler->millis();
|
|
if (now - _last_GPRMC_ms > 150 ||
|
|
now - _last_GPGGA_ms > 150) {
|
|
return false;
|
|
}
|
|
if (_last_GPVTG_ms != 0 &&
|
|
now - _last_GPVTG_ms > 150) {
|
|
return false;
|
|
}
|
|
// prevent these messages being used again
|
|
if (_last_GPVTG_ms != 0) {
|
|
_last_GPVTG_ms = 1;
|
|
}
|
|
_last_GPGGA_ms = 1;
|
|
_last_GPRMC_ms = 1;
|
|
return true;
|
|
}
|
|
|
|
// 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;
|
|
state.location.lat = _new_latitude;
|
|
state.location.lng = _new_longitude;
|
|
state.ground_speed = _new_speed*0.01f;
|
|
state.ground_course_cd = wrap_360_cd(_new_course);
|
|
make_gps_time(_new_date, _new_time * 10);
|
|
state.last_gps_time_ms = hal.scheduler->millis();
|
|
// To-Do: add support for proper reporting of 2D and 3D fix
|
|
state.status = AP_GPS::GPS_OK_FIX_3D;
|
|
fill_3d_velocity();
|
|
break;
|
|
case _GPS_SENTENCE_GPGGA:
|
|
state.location.alt = _new_altitude;
|
|
state.location.lat = _new_latitude;
|
|
state.location.lng = _new_longitude;
|
|
state.num_sats = _new_satellite_count;
|
|
state.hdop = _new_hdop;
|
|
// To-Do: add support for proper reporting of 2D and 3D fix
|
|
state.status = AP_GPS::GPS_OK_FIX_3D;
|
|
break;
|
|
case _GPS_SENTENCE_GPVTG:
|
|
state.ground_speed = _new_speed*0.01f;
|
|
state.ground_course_cd = wrap_360_cd(_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.
|
|
state.status = AP_GPS::NO_FIX;
|
|
}
|
|
}
|
|
// see if we got a good message
|
|
return _have_new_message();
|
|
}
|
|
// we got a bad message, ignore it
|
|
return false;
|
|
}
|
|
|
|
// the first term determines the sentence type
|
|
if (_term_number == 0) {
|
|
if (!strcmp(_term, _gprmc_string)) {
|
|
_sentence_type = _GPS_SENTENCE_GPRMC;
|
|
_last_GPRMC_ms = hal.scheduler->millis();
|
|
} else if (!strcmp(_term, _gpgga_string)) {
|
|
_sentence_type = _GPS_SENTENCE_GPGGA;
|
|
_last_GPGGA_ms = hal.scheduler->millis();
|
|
} else if (!strcmp(_term, _gpvtg_string)) {
|
|
_sentence_type = _GPS_SENTENCE_GPVTG;
|
|
// VTG may not contain a data qualifier, presume the solution is good
|
|
// unless it tells us otherwise.
|
|
_last_GPVTG_ms = hal.scheduler->millis();
|
|
_gps_data_good = true;
|
|
} else {
|
|
_sentence_type = _GPS_SENTENCE_OTHER;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// 32 = RMC, 64 = GGA, 96 = VTG
|
|
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
|
|
switch (_sentence_type + _term_number) {
|
|
// operational status
|
|
//
|
|
case _GPS_SENTENCE_GPRMC + 2: // validity (RMC)
|
|
_gps_data_good = _term[0] == 'A';
|
|
break;
|
|
case _GPS_SENTENCE_GPGGA + 6: // Fix data (GGA)
|
|
_gps_data_good = _term[0] > '0';
|
|
break;
|
|
case _GPS_SENTENCE_GPVTG + 9: // validity (VTG) (we may not see this field)
|
|
_gps_data_good = _term[0] != 'N';
|
|
break;
|
|
case _GPS_SENTENCE_GPGGA + 7: // satellite count (GGA)
|
|
_new_satellite_count = atol(_term);
|
|
break;
|
|
case _GPS_SENTENCE_GPGGA + 8: // HDOP (GGA)
|
|
_new_hdop = _parse_decimal_100();
|
|
break;
|
|
|
|
// time and date
|
|
//
|
|
case _GPS_SENTENCE_GPRMC + 1: // Time (RMC)
|
|
case _GPS_SENTENCE_GPGGA + 1: // Time (GGA)
|
|
_new_time = _parse_decimal_100();
|
|
break;
|
|
case _GPS_SENTENCE_GPRMC + 9: // Date (GPRMC)
|
|
_new_date = atol(_term);
|
|
break;
|
|
|
|
// location
|
|
//
|
|
case _GPS_SENTENCE_GPRMC + 3: // Latitude
|
|
case _GPS_SENTENCE_GPGGA + 2:
|
|
_new_latitude = _parse_degrees();
|
|
break;
|
|
case _GPS_SENTENCE_GPRMC + 4: // N/S
|
|
case _GPS_SENTENCE_GPGGA + 3:
|
|
if (_term[0] == 'S')
|
|
_new_latitude = -_new_latitude;
|
|
break;
|
|
case _GPS_SENTENCE_GPRMC + 5: // Longitude
|
|
case _GPS_SENTENCE_GPGGA + 4:
|
|
_new_longitude = _parse_degrees();
|
|
break;
|
|
case _GPS_SENTENCE_GPRMC + 6: // E/W
|
|
case _GPS_SENTENCE_GPGGA + 5:
|
|
if (_term[0] == 'W')
|
|
_new_longitude = -_new_longitude;
|
|
break;
|
|
case _GPS_SENTENCE_GPGGA + 9: // Altitude (GPGGA)
|
|
_new_altitude = _parse_decimal_100();
|
|
break;
|
|
|
|
// course and speed
|
|
//
|
|
case _GPS_SENTENCE_GPRMC + 7: // Speed (GPRMC)
|
|
case _GPS_SENTENCE_GPVTG + 5: // Speed (VTG)
|
|
_new_speed = (_parse_decimal_100() * 514) / 1000; // knots-> m/sec, approximiates * 0.514
|
|
break;
|
|
case _GPS_SENTENCE_GPRMC + 8: // Course (GPRMC)
|
|
case _GPS_SENTENCE_GPVTG + 1: // Course (VTG)
|
|
_new_course = _parse_decimal_100();
|
|
break;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
detect a NMEA GPS. Adds one byte, and returns true if the stream
|
|
matches a NMEA string
|
|
*/
|
|
bool
|
|
AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data)
|
|
{
|
|
switch (state.step) {
|
|
case 0:
|
|
state.ck = 0;
|
|
if ('$' == data) {
|
|
state.step++;
|
|
}
|
|
break;
|
|
case 1:
|
|
if ('*' == data) {
|
|
state.step++;
|
|
} else {
|
|
state.ck ^= data;
|
|
}
|
|
break;
|
|
case 2:
|
|
if (hexdigit(state.ck>>4) == data) {
|
|
state.step++;
|
|
} else {
|
|
state.step = 0;
|
|
}
|
|
break;
|
|
case 3:
|
|
if (hexdigit(state.ck&0xF) == data) {
|
|
return true;
|
|
}
|
|
state.step = 0;
|
|
break;
|
|
}
|
|
return false;
|
|
}
|