Added non-final versions of iris and tower firmware.

This commit is contained in:
2018-02-20 15:45:21 -08:00
parent 31a07e82f8
commit 6bb3539488
171 changed files with 36168 additions and 13 deletions

View File

@@ -0,0 +1,34 @@
#ifndef COSACOMPAT_H
#define COSACOMPAT_H
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#ifdef __AVR__
#include <avr/pgmspace.h>
#else
#define PGM_P const char *
#endif
typedef PGM_P str_P;
#define __PROGMEM PROGMEM
#endif

View File

@@ -0,0 +1,121 @@
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "DMS.h"
#include <Print.h>
//----------------------------------------------------------------
// Note that no division is used, and shifts are on byte boundaries. Fast!
void DMS_t::From( int32_t deg_1E7 )
{
const uint32_t E7 = 10000000UL;
if (deg_1E7 < 0) {
deg_1E7 = -deg_1E7;
hemisphere = SOUTH_H; // or WEST_H
} else
hemisphere = NORTH_H; // or EAST_H
const uint32_t div_E32 = 429; // 1e-07 * 2^32
degrees = ((deg_1E7 >> 16) * div_E32) >> 16;
uint32_t remainder = deg_1E7 - degrees * E7;
remainder *= 60; // to minutes * E7
minutes = ((remainder >> 16) * div_E32) >> 16;
remainder -= minutes * E7;
remainder *= 60; // to seconds * E7
uint32_t secs = ((remainder >> 16) * div_E32) >> 16;
remainder -= secs * E7;
const uint32_t div_1E4_E24 = 1677; // 0.00001 * 2^24
seconds_frac = (((remainder >> 8) * div_1E4_E24) >> 16); // thousandths
seconds_whole = secs;
// Carry if thousandths too big
if (seconds_frac >= 1000) {
seconds_frac -= 1000;
seconds_whole++;
if (seconds_whole >= 60) {
seconds_whole -= 60;
minutes++;
if (minutes >= 60) {
minutes -= 60;
degrees++;
}
}
}
} // From
//----------------------------------------------------------------
Print & operator << ( Print & outs, const DMS_t & dms )
{
if (dms.degrees < 10)
outs.write( '0' );
outs.print( dms.degrees );
outs.write( ' ' );
if (dms.minutes < 10)
outs.write( '0' );
outs.print( dms.minutes );
outs.print( F("\' ") );
if (dms.seconds_whole < 10)
outs.write( '0' );
outs.print( dms.seconds_whole );
outs.write( '.' );
if (dms.seconds_frac < 100)
outs.write( '0' );
if (dms.seconds_frac < 10)
outs.write( '0' );
outs.print( dms.seconds_frac );
outs.print( F("\" ") );
return outs;
} // operator <<
//----------------------------------------------------------------
void DMS_t::printDDDMMmmmm( Print & outs ) const
{
outs.print( degrees );
if (minutes < 10)
outs.print( '0' );
outs.print( minutes );
outs.print( '.' );
// Calculate the fractional minutes from the seconds,
// *without* using floating-point numbers.
uint16_t mmmm = seconds_whole * 166; // same as 10000/60, less .66666...
mmmm += (seconds_whole * 2 + seconds_frac/2 ) / 3; // ... plus the remaining .66666
// ... plus the seconds_frac, scaled by 10000/(60*1000) = 1/6, which
// is implemented above as 1/2 * 1/3
// print leading zeroes, if necessary
if (mmmm < 1000)
outs.print( '0' );
if (mmmm < 100)
outs.print( '0' );
if (mmmm < 10)
outs.print( '0' );
outs.print( mmmm );
}

View File

@@ -0,0 +1,55 @@
#ifndef DMS_H
#define DMS_H
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "NeoGPS_cfg.h"
#include <stdint.h>
class Print;
enum Hemisphere_t { NORTH_H = 0, SOUTH_H = 1, EAST_H = 0, WEST_H = 1 };
class DMS_t
{
public:
uint8_t degrees;
uint8_t minutes ;//NEOGPS_BF(6);
Hemisphere_t hemisphere ;//NEOGPS_BF(2); compiler bug!
uint8_t seconds_whole NEOGPS_BF(6);
uint16_t seconds_frac NEOGPS_BF(10); // 1000ths
void init() { degrees = minutes = seconds_whole = seconds_frac = 0;
hemisphere = NORTH_H; }
float secondsF() const { return seconds_whole + 0.001 * seconds_frac; };
char NS () const { return (hemisphere == SOUTH_H) ? 'S' : 'N'; };
char EW () const { return (hemisphere == WEST_H) ? 'W' : 'E'; };
//.............................................................................
// A utility function to convert from integer 'lat' or 'lon', scaled by 10^7
void From( int32_t deg_1E7 );
// Print DMS as the funky NMEA DDDMM.mmmm format
void printDDDMMmmmm( Print & outs ) const;
} NEOGPS_PACKED;
extern Print & operator << ( Print & outs, const DMS_t & );
#endif

View File

@@ -0,0 +1,21 @@
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "GPSTime.h"
uint8_t GPSTime::leap_seconds = 0;
NeoGPS::clock_t GPSTime::s_start_of_week = 0;

View File

@@ -0,0 +1,85 @@
#ifndef GPSTIME_H
#define GPSTIME_H
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "NeoTime.h"
class GPSTime
{
GPSTime();
static NeoGPS::clock_t s_start_of_week;
public:
/**
* GPS time is offset from UTC by a number of leap seconds. To convert a GPS
* time to UTC time, the current number of leap seconds must be known.
* See http://en.wikipedia.org/wiki/Global_Positioning_System#Leap_seconds
*/
static uint8_t leap_seconds;
/**
* Some receivers report time WRT start of the current week, defined as
* Sunday 00:00:00. To save fairly expensive date/time calculations,
* the UTC start of week is cached
*/
static void start_of_week( NeoGPS::time_t & now )
{
now.set_day();
s_start_of_week =
(NeoGPS::clock_t) now -
(NeoGPS::clock_t) ((((now.day-1 ) * 24L +
now.hours ) * 60L +
now.minutes) * 60L +
now.seconds);
}
static NeoGPS::clock_t start_of_week()
{
return s_start_of_week;
}
/*
* Convert a GPS time-of-week to UTC.
* Requires /leap_seconds/ and /start_of_week/.
*/
static NeoGPS::clock_t TOW_to_UTC( uint32_t time_of_week )
{ return (NeoGPS::clock_t)
(start_of_week() + time_of_week - leap_seconds); }
/**
* Set /fix/ timestamp from a GPS time-of-week in milliseconds.
* Requires /leap_seconds/ and /start_of_week/.
**/
static bool from_TOWms
( uint32_t time_of_week_ms, NeoGPS::time_t &dt, uint16_t &ms )
{
//trace << PSTR("from_TOWms(") << time_of_week_ms << PSTR("), sow = ") << start_of_week() << PSTR(", leap = ") << leap_seconds << endl;
bool ok = (start_of_week() != 0) && (leap_seconds != 0);
if (ok) {
NeoGPS::clock_t tow_s = time_of_week_ms/1000UL;
dt = TOW_to_UTC( tow_s );
ms = (uint16_t)(time_of_week_ms - tow_s*1000UL);
}
return ok;
}
};
#endif

View File

@@ -0,0 +1,501 @@
#ifndef GPSFIX_H
#define GPSFIX_H
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "NeoGPS_cfg.h"
#include "GPSfix_cfg.h"
#if defined( GPS_FIX_DATE ) | defined( GPS_FIX_TIME )
#include "NeoTime.h"
#endif
#ifdef GPS_FIX_LOCATION_DMS
#include "DMS.h"
#endif
#ifdef GPS_FIX_LOCATION
#include "Location.h"
#endif
/**
* A structure for holding a GPS fix: time, position, velocity, etc.
*
* Because GPS devices report various subsets of a coherent fix,
* this class tracks which members of the fix are being reported:
* each part has its own validity flag. Also, operator |= implements
* merging multiple reports into one consolidated report.
*
* @section Limitations
* Reports are not really fused with an algorithm; if present in
* the source, they are simply replaced in the destination.
*
*/
class gps_fix
{
public:
// Default Constructor
gps_fix() { init(); };
//------------------------------------------------------------------
// 'whole_frac' is a utility structure that holds the two parts
// of a floating-point number.
//
// This is used for Altitude, Heading and Speed, which require more
// significant digits than a 16-bit number.
//
// When parsing floating-point fields, the decimal point is used as
// a separator for these two parts. This is much more efficient
// than calling 'long' or 'floating-point' math subroutines.
//
// This requires knowing the exponent on the fraction when a simple type
// (e.g., float or int) is needed. For example, 'altitude()' knows that
// the whole part was stored as integer meters, and the fractional part
// was stored as integer centimeters.
//
// Unless you want the speed and precision of the two integer parts, you
// shouldn't have to use 'whole_frac'. Instead, use the
// accessor functions for each of the specific fields for
// Altitude, Heading and Speed.
struct whole_frac {
int16_t whole;
int16_t frac;
void init() { whole = 0; frac = 0; };
int32_t int32_00() const { return ((int32_t)whole) * 100L + frac; };
int16_t int16_00() const { return whole * 100 + frac; };
int32_t int32_000() const { return whole * 1000L + frac; };
float float_00() const { return ((float)whole) + ((float)frac)*0.01; };
float float_000() const { return ((float)whole) + ((float)frac)*0.001; };
} NEOGPS_PACKED;
//--------------------------------------------------------------
// Members of a GPS fix
//
// Each member is separately enabled or disabled by the CFG file
// by #ifdef/#endif wrappers.
// Each member has a storage declaration that depends on the
// precision and type of data available from GPS devices.
// Some members have a separate accessor function that converts the
// internal storage type to a more common or convenient type
// for use by an application.
// For example, latitude data is internally stored as a 32-bit integer,
// where the reported degrees have been multiplied by 10^7. Many
// applications expect a floating-point value, so a floating-point
// accessor is provided: 'latitude()'. This function converts the
// internal 32-bit integer to a floating-point value, and then
// divides it by 10^7. The returned value is now a floating-point
// degrees value.
#ifdef GPS_FIX_LOCATION
NeoGPS::Location_t location;
int32_t latitudeL() const { return location.lat (); };
float latitude () const { return location.latF(); }; // accuracy loss
int32_t longitudeL() const { return location.lon (); };
float longitude () const { return location.lonF(); }; // accuracy loss
#endif
#ifdef GPS_FIX_LOCATION_DMS
DMS_t latitudeDMS;
DMS_t longitudeDMS;
#endif
#ifdef GPS_FIX_ALTITUDE
whole_frac alt; // .01 meters
int32_t altitude_cm() const { return alt.int32_00(); };
float altitude () const { return alt.float_00(); };
float altitude_ft() const { return altitude() * 3.28084; };
#endif
#ifdef GPS_FIX_SPEED
whole_frac spd; // .001 nautical miles per hour
uint32_t speed_mkn () const { return spd.int32_000(); };
float speed () const { return spd.float_000(); };
// Utilities for speed in other units
CONST_CLASS_DATA float KM_PER_NMI = 1.852;
float speed_kph () const { return speed() * KM_PER_NMI; };
CONST_CLASS_DATA uint16_t M_PER_NMI = 1852;
uint32_t speed_metersph() const { return (spd.whole * M_PER_NMI) + (spd.frac * M_PER_NMI)/1000; };
CONST_CLASS_DATA float MI_PER_NMI = 1.150779;
float speed_mph() const { return speed() * MI_PER_NMI; };
#endif
#ifdef GPS_FIX_HEADING
whole_frac hdg; // .01 degrees
uint16_t heading_cd() const { return hdg.int16_00(); };
float heading () const { return hdg.float_00(); };
#endif
//--------------------------------------------------------
// Dilution of Precision is a measure of the current satellite
// constellation geometry WRT how 'good' it is for determining a
// position. This is _independent_ of signal strength and many
// other factors that may be internal to the receiver.
// It _cannot_ be used to determine position accuracy in meters.
// Instead, use the LAT/LON/ALT error in cm members, which are
// populated by GST sentences.
#ifdef GPS_FIX_HDOP
uint16_t hdop; // Horizontal Dilution of Precision x 1000
#endif
#ifdef GPS_FIX_VDOP
uint16_t vdop; // Vertical Dilution of Precision x 1000
#endif
#ifdef GPS_FIX_PDOP
uint16_t pdop; // Position Dilution of Precision x 1000
#endif
//--------------------------------------------------------
// Error estimates for latitude, longitude and altitude, in centimeters.
#ifdef GPS_FIX_LAT_ERR
uint16_t lat_err_cm;
float lat_err() const { return lat_err_cm / 100.0; }
#endif
#ifdef GPS_FIX_LON_ERR
uint16_t lon_err_cm;
float lon_err() const { return lon_err_cm / 100.0; }
#endif
#ifdef GPS_FIX_ALT_ERR
uint16_t alt_err_cm;
float alt_err() const { return alt_err_cm / 100.0; }
#endif
//--------------------------------------------------------
// Height of the geoid above the WGS84 ellipsoid
#ifdef GPS_FIX_GEOID_HEIGHT
whole_frac geoidHt; // .01 meters
int32_t geoidHeight_cm() const { return geoidHt.int32_00(); };
float geoidHeight () const { return geoidHt.float_00(); };
#endif
//--------------------------------------------------------
// Number of satellites used to calculate a fix.
#ifdef GPS_FIX_SATELLITES
uint8_t satellites;
#endif
//--------------------------------------------------------
// Date and Time for the fix
#if defined(GPS_FIX_DATE) | defined(GPS_FIX_TIME)
NeoGPS::time_t dateTime ; // Date and Time in one structure
#endif
#if defined(GPS_FIX_TIME)
uint8_t dateTime_cs; // The fix's UTC hundredths of a second
uint32_t dateTime_us() const // The fix's UTC microseconds
{ return dateTime_cs * 10000UL; };
uint16_t dateTime_ms() const // The fix's UTC millseconds
{ return dateTime_cs * 10; };
#endif
//--------------------------------------------------------
// The current fix status or mode of the GPS device.
//
// Unfortunately, the NMEA sentences are a little inconsistent
// in their use of "status" and "mode". Both fields are mapped
// onto this enumerated type. Be aware that different
// manufacturers interpret them differently. This can cause
// problems in sentences which include both types (e.g., GPGLL).
//
// Note: Sorted by increasing accuracy. See also /operator |=/.
enum status_t {
STATUS_NONE,
STATUS_EST,
STATUS_TIME_ONLY,
STATUS_STD,
STATUS_DGPS,
STATUS_RTK_FLOAT,
STATUS_RTK_FIXED,
STATUS_PPS // Precise Position System, *NOT* Pulse-per-second
};
status_t status NEOGPS_BF(8);
//--------------------------------------------------------
// Flags to indicate which members of this fix are valid.
//
// Because different sentences contain different pieces of a fix,
// each piece can be valid or NOT valid. If the GPS device does not
// have good reception, some fields may not contain any value.
// Those empty fields will be marked as NOT valid.
struct valid_t {
bool status NEOGPS_BF(1);
#if defined(GPS_FIX_DATE)
bool date NEOGPS_BF(1);
#endif
#if defined(GPS_FIX_TIME)
bool time NEOGPS_BF(1);
#endif
#if defined( GPS_FIX_LOCATION ) | defined( GPS_FIX_LOCATION_DMS )
bool location NEOGPS_BF(1);
#endif
#ifdef GPS_FIX_ALTITUDE
bool altitude NEOGPS_BF(1);
#endif
#ifdef GPS_FIX_SPEED
bool speed NEOGPS_BF(1);
#endif
#ifdef GPS_FIX_HEADING
bool heading NEOGPS_BF(1);
#endif
#ifdef GPS_FIX_SATELLITES
bool satellites NEOGPS_BF(1);
#endif
#ifdef GPS_FIX_HDOP
bool hdop NEOGPS_BF(1);
#endif
#ifdef GPS_FIX_VDOP
bool vdop NEOGPS_BF(1);
#endif
#ifdef GPS_FIX_PDOP
bool pdop NEOGPS_BF(1);
#endif
#ifdef GPS_FIX_LAT_ERR
bool lat_err NEOGPS_BF(1);
#endif
#ifdef GPS_FIX_LON_ERR
bool lon_err NEOGPS_BF(1);
#endif
#ifdef GPS_FIX_ALT_ERR
bool alt_err NEOGPS_BF(1);
#endif
#ifdef GPS_FIX_GEOID_HEIGHT
bool geoidHeight NEOGPS_BF(1);
#endif
// Initialize all flags to false
void init()
{
uint8_t *all = (uint8_t *) this;
for (uint8_t i=0; i<sizeof(*this); i++)
*all++ = 0;
}
// Merge these valid flags with another set of valid flags
void operator |=( const valid_t & r )
{
uint8_t *all = (uint8_t *) this;
const uint8_t *r_all = (const uint8_t *) &r;
for (uint8_t i=0; i<sizeof(*this); i++)
*all++ |= *r_all++;
}
} NEOGPS_PACKED
valid; // This is the name of the collection of valid flags
//--------------------------------------------------------
// Initialize a fix. All configured members are set to zero.
void init()
{
#ifdef GPS_FIX_LOCATION
location.init();
#endif
#ifdef GPS_FIX_LOCATION_DMS
latitudeDMS.init();
longitudeDMS.init();
#endif
#ifdef GPS_FIX_ALTITUDE
alt.init();
#endif
#ifdef GPS_FIX_SPEED
spd.init();
#endif
#ifdef GPS_FIX_HEADING
hdg.init();
#endif
#ifdef GPS_FIX_HDOP
hdop = 0;
#endif
#ifdef GPS_FIX_VDOP
vdop = 0;
#endif
#ifdef GPS_FIX_PDOP
pdop = 0;
#endif
#ifdef GPS_FIX_LAT_ERR
lat_err_cm = 0;
#endif
#ifdef GPS_FIX_LON_ERR
lon_err_cm = 0;
#endif
#ifdef GPS_FIX_ALT_ERR
alt_err_cm = 0;
#endif
#ifdef GPS_FIX_GEOID_HEIGHT
geoidHt.init();
#endif
#ifdef GPS_FIX_SATELLITES
satellites = 0;
#endif
#if defined(GPS_FIX_DATE) | defined(GPS_FIX_TIME)
dateTime.init();
#endif
#if defined(GPS_FIX_TIME)
dateTime_cs = 0;
#endif
status = STATUS_NONE;
valid.init();
} // init
//-------------------------------------------------------------
// Merge valid fields from the right fix into a "fused" fix
// on the left (i.e., /this/).
//
// Usage: gps_fix left, right;
// left |= right; // explicit merge
gps_fix & operator |=( const gps_fix & r )
{
// Replace /status/ only if the right is more "accurate".
if (r.valid.status && (!valid.status || (status < r.status)))
status = r.status;
#ifdef GPS_FIX_DATE
if (r.valid.date) {
dateTime.date = r.dateTime.date;
dateTime.month = r.dateTime.month;
dateTime.year = r.dateTime.year;
}
#endif
#ifdef GPS_FIX_TIME
if (r.valid.time) {
dateTime.hours = r.dateTime.hours;
dateTime.minutes = r.dateTime.minutes;
dateTime.seconds = r.dateTime.seconds;
dateTime_cs = r.dateTime_cs;
}
#endif
#ifdef GPS_FIX_LOCATION
if (r.valid.location) {
location = r.location;
}
#endif
#ifdef GPS_FIX_LOCATION_DMS
if (r.valid.location) {
latitudeDMS = r.latitudeDMS;
longitudeDMS = r.longitudeDMS;
}
#endif
#ifdef GPS_FIX_ALTITUDE
if (r.valid.altitude)
alt = r.alt;
#endif
#ifdef GPS_FIX_HEADING
if (r.valid.heading)
hdg = r.hdg;
#endif
#ifdef GPS_FIX_SPEED
if (r.valid.speed)
spd = r.spd;
#endif
#ifdef GPS_FIX_SATELLITES
if (r.valid.satellites)
satellites = r.satellites;
#endif
#ifdef GPS_FIX_HDOP
if (r.valid.hdop)
hdop = r.hdop;
#endif
#ifdef GPS_FIX_VDOP
if (r.valid.vdop)
vdop = r.vdop;
#endif
#ifdef GPS_FIX_PDOP
if (r.valid.pdop)
pdop = r.pdop;
#endif
#ifdef GPS_FIX_LAT_ERR
if (r.valid.lat_err)
lat_err_cm = r.lat_err_cm;
#endif
#ifdef GPS_FIX_LON_ERR
if (r.valid.lon_err)
lon_err_cm = r.lon_err_cm;
#endif
#ifdef GPS_FIX_ALT_ERR
if (r.valid.alt_err)
alt_err_cm = r.alt_err_cm;
#endif
#ifdef GPS_FIX_GEOID_HEIGHT
if (r.valid.geoidHeight)
geoidHt = r.geoidHt;
#endif
// Update all the valid flags
valid |= r.valid;
return *this;
} // operator |=
} NEOGPS_PACKED;
#endif

View File

@@ -0,0 +1,52 @@
#ifndef GPS_FIX_CFG
#define GPS_FIX_CFG
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
/**
* Enable/disable the storage for the members of a fix.
*
* Disabling a member prevents it from being parsed from a received message.
* The disabled member cannot be accessed or stored, and its validity flag
* would not be available. It will not be declared, and code that uses that
* member will not compile.
*
* DATE and TIME are somewhat coupled in that they share a single `time_t`,
* but they have separate validity flags.
*
* See also note regarding the DOP members, below.
*
*/
#define GPS_FIX_DATE
#define GPS_FIX_TIME
#define GPS_FIX_LOCATION
//#define GPS_FIX_LOCATION_DMS
#define GPS_FIX_ALTITUDE
#define GPS_FIX_SPEED
#define GPS_FIX_HEADING
#define GPS_FIX_SATELLITES
//#define GPS_FIX_HDOP
//#define GPS_FIX_VDOP
//#define GPS_FIX_PDOP
//#define GPS_FIX_LAT_ERR
//#define GPS_FIX_LON_ERR
//#define GPS_FIX_ALT_ERR
//#define GPS_FIX_GEOID_HEIGHT
#endif

View File

@@ -0,0 +1,271 @@
#ifndef GPSport_h
#define GPSport_h
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
//-----------------------------------------------------------
// There are 2 ways this file can be used:
//
// I. AS IS, which tries to *guess* which port a beginner should use.
// If you include <SoftwareSerial.h>, <NeoSWSerial.h>, <AltSoftSerial.h>,
// <NeoICSerial.h> *OR* <NeoHWSerial.h> before this file (in the INO)
// or on line 152 below, one of those ports will be used for the GPS.
// This file cannot always guess what you want, so you may need to use it
// in the 2nd way...
//
// II. *REPLACE EVERYTHING* in this file to *specify* which port and
// which library you want to use for that port. Just declare the
// port here. You must declare three things:
//
// 1) the "gpsPort" variable (used by all example programs),
// 2) the string for the GPS_PORT_NAME (displayed by all example programs), and
// 3) the DEBUG_PORT to use for Serial Monitor print messages (usually Serial).
//
// The examples will display the device you have selected during setup():
//
// DEBUG_PORT.println( F("Looking for GPS device on " GPS_PORT_NAME) );
//
// Choosing the serial port connection for your GPS device is probably
// the most important design decision, because it has a tremendous impact
// on the processor load. Here are the possible configurations,
// from BEST to WORST:
//
// --- The BEST choice is to use a HardwareSerial port. You could use Serial
// on any board, but you will have to disconnect the Arduino RX pin 0
// from the GPS TX pin to upload a new sketch over USB. This is a very
// reliable connection:
//
// #define gpsPort Serial
// #define GPS_PORT_NAME "Serial"
// #define DEBUG_PORT Serial
//
// If you have a Mega, Leo or Due board, you could use Serial1,
// Serial2 or Serial3:
//
// #define gpsPort Serial1
// #define GPS_PORT_NAME "Serial1"
// #define DEBUG_PORT Serial
//
// These extra ports do not have to be disconnected to upload new
// sketches over USB.
//
// Use NeoHWSerial if you want to handle GPS characters
// in an Interrupt Service Routine (see NMEA_isr.INO).
// Also uncomment NMEAGPS_INTERRUPT_PROCESSING in NMEAGPS_cfg.h.
//
// #include <NeoHWSerial.h>
// #define gpsPort NeoSerial
// #define GPS_PORT_NAME "NeoSerial"
// #define DEBUG_PORT NeoSerial
//
// Note that the DEBUG_PORT must be NeoSerial, not Serial
// (see NeoHWSerial documentation for additional requirements).
//
// --- The SECOND BEST choice is AltSoftSerial. It uses 10x to 500x
// more processor time than a HardwareSerial port, but it is the BEST
// *software* serial port library. It only works on two specific pins
// (see the AltSoftSerial documentation).
//
// #include <AltSoftSerial.h>
// AltSoftSerial gpsPort; // 8 & 9 for an UNO
// #define GPS_PORT_NAME "AltSoftSerial"
// #define DEBUG_PORT Serial
//
// Use NeoICSerial if you want to handle GPS characters
// in an Interrupt Service Routine (see NMEA_isr.INO).
// Also uncomment NMEAGPS_INTERRUPT_PROCESSING in NMEAGPS_cfg.h.
//
// #include <NeoICSerial.h>
// NeoICSerial gpsPort; // 8 & 9 for an UNO
// #define GPS_PORT_NAME "NeoICSerial"
// #define DEBUG_PORT Serial
//
// --- The THIRD BEST choice is NeoSWSerial. It is almost as efficient as
// AltSoftSerial, but it only supports baud rates 9600, 19200 or 38400.
//
// #include <NeoSWSerial.h>
// NeoSWSerial gpsPort(3, 2);
// #define GPS_PORT_NAME "NeoSWSerial(3,2)"
// #define DEBUG_PORT Serial
//
// NeoSWSerial supports handling GPS characters in an Interrupt Service
// Routine (see NMEA_isr.INO). If you want to do that, also uncomment
// NMEAGPS_INTERRUPT_PROCESSING in NMEAGPS_cfg.h.
//
// --- The WORST choice is SoftwareSerial. IT IS NOT RECOMMENDED because
// it disables interrupts for the entire time a character is sent OR
// received. This interferes with other parts of your sketch or with
// other libraries. It prevents receiving or sending data on any other
// port. It cannot transmit and receive at the same time. 95% of the
// CPU time will be spent twiddling its thumbs. It may support 57600,
// but 115200 is rarely reliable.
//
// #include <SoftwareSerial.h>
// SoftwareSerial gpsPort(2,3);
// #define GPS_PORT_NAME "SoftwareSerial(8,9)"
// #define DEBUG_PORT Serial
//
// You should *seriously* consider using other pins or lower baud rates,
// so that you could use one of the other recommended ports.
//-----------------------------------------------------------
// DEFAULT file contents:
// *GUESS* which port should be used. If you know what port you want to use,
// *DELETE* the rest of this file and declare your own GPS port variable,
// GPS port name string, and debug print port variable (see above).
#if defined(SERIAL_PORT_HARDWARE_OPEN)
#if SERIAL_PORT_HARDWARE_OPEN == Serial1
// This Arduino has more than one hardware serial port,
// use Serial1 (or NeoSerial1).
#define NEOGPS_USE_SERIAL1
#endif
#endif
#ifdef NEOGPS_USE_SERIAL1
#if defined( NMEAGPS_INTERRUPT_PROCESSING )
#include <NeoHWSerial.h>
#define gpsPort NeoSerial1
#define GPS_PORT_NAME "NeoSerial1"
#else
#define gpsPort Serial1
#define GPS_PORT_NAME "Serial1"
#endif
#else
// Uncomment *zero* or *one* of the serial port library includes.
// If none of these includes are uncommented, Serial will be used.
//
//#include <NeoHWSerial.h> // NeoSerial or NeoSerial1 Interrupt-style processing
#include <AltSoftSerial.h> // <-- DEFAULT. Two specific pins required
//#include <NeoICSerial.h> // AltSoftSerial with Interrupt-style processing (see docs)
//#include <NeoSWSerial.h> // Any pins, only @ 9600, 19200 or 38400 baud
//#include <SoftwareSerial.h> // NOT RECOMMENDED!
#if defined( SoftwareSerial_h )
#define SS_TYPE SoftwareSerial
#warning SoftwareSerial is NOT RECOMMENDED. Use AltSoftSerial or NeoSWSerial instead.
#if defined( NMEAGPS_INTERRUPT_PROCESSING )
#error You cannot use SoftwareSerial with INTERRUPT_PROCESSING. Use NeoSWSerial or NeoICSerial instead.
#endif
#elif defined( NeoSWSerial_h )
#define SS_TYPE NeoSWSerial
#elif defined( AltSoftSerial_h )
#define SS_TYPE AltSoftSerial
#define RX_PIN -1 // doesn't matter because it only works...
#define TX_PIN -1 // ...on two specific pins
#if defined( NMEAGPS_INTERRUPT_PROCESSING )
#error You cannot use AltSoftSerial with INTERRUPT_PROCESSING. Use NeoICSerial instead.
#endif
#elif defined( NeoICSerial_h )
#define SS_TYPE NeoICSerial
#define RX_PIN -1 // doesn't matter because it only works...
#define TX_PIN -1 // ...on two specific pins
#elif defined( NeoHWSerial_h )
#define gpsPort NeoSerial
#define GPS_PORT_NAME "NeoSerial"
#warning Using Serial (actually NeoSerial) for GPS connection.
#else
// No serial library files were included before this file, just use Serial.
#define gpsPort Serial
#define GPS_PORT_NAME "Serial"
#warning Using Serial for GPS connection.
// You will see this warning if you want to use Serial for the GPS, because no
// software serial port libraries were included. That is a good choice.
//
// To use a different serial port for GPS device, you must include a serial header
// before "#include GPSport.h" in the INO. It may be simpler to replace the
// entire contents of this file with your own declarations. Follow the
// instructions above for declaring your own "gpsPort" variable. Everything else
// in this file should be deleted. See Installation instructions for more
// information.
#endif
#endif
#ifdef SS_TYPE
//---------------------------------------------------------------
// The current Board (an Uno?) does not have an extra serial port.
// Use a software serial library to listen to the GPS device.
// You should expect to get some RX errors, which may
// prevent getting fix data every second. YMMV.
// Default Arduino RX pin number that is connected to the GPS TX pin
#ifndef RX_PIN
#define RX_PIN 4
#endif
// Default Arduino TX pin number that is connected to the GPS RX pin
#ifndef TX_PIN
#define TX_PIN 3
#endif
SS_TYPE gpsPort( RX_PIN, TX_PIN );
// A little preprocessor magic to get a nice string
#define xstr(x) str(x)
#define str(x) #x
#define GPS_PORT_NAME \
xstr(SS_TYPE) "( RX pin " xstr(RX_PIN) \
", TX pin " xstr(TX_PIN) " )"
#ifdef NEOGPS_USE_SERIAL1
// If you *really* want to do this, or you just happened to include
// a software serial library header for something else, you're
// better off *not* including this file. Just declare
// your own gpsPort in your INO file.
#error You should be using Serial1 for the GPS device. \
Software serial libraries are very inefficient and unreliable when \
used for GPS communications!
#endif
#endif
//------------------------------------------------------------
// When NeoHWSerial is used, none of the built-in HardwareSerial
// variables can be used: Serial, Serial1, Serial2 and Serial3
// *cannot* be used. Instead, you must use the corresponding
// NeoSerial, NeoSerial1, NeoSerial2 or NeoSerial3. This define
// is used to substitute the appropriate Serial variable in
// all debug prints.
#ifdef NeoHWSerial_h
#define DEBUG_PORT NeoSerial
#else
#define DEBUG_PORT Serial // default for most sketches
#endif
// End of guessing game.
//------------------------
#endif

View File

@@ -0,0 +1,137 @@
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "Location.h"
using namespace NeoGPS;
//---------------------------------------------------------------------
// Calculate dLon with integers, less one bit to avoid overflow
// (360 * 1e7 = 3600000000, which is too big).
// Retains precision when points are close together.
int32_t safeDLon( int32_t p2, int32_t p1 )
{
int32_t dLonL;
int32_t halfDLon = (p2/2 - p1/2);
if (halfDLon < -1800000000L/2) {
dLonL = (p2 + 1800000000L) - (p1 - 1800000000L);
} else if (1800000000L/2 < halfDLon) {
dLonL = (p2 - 1800000000L) - (p1 + 1800000000L);
} else {
dLonL = p2 - p1;
}
return dLonL;
} // safeDLon
//---------------------------------------------------------------------
float Location_t::DistanceRadians
( const Location_t & p1, const Location_t & p2 )
{
int32_t dLonL = safeDLon( p2.lon(), p1.lon() );
int32_t dLatL = p2.lat() - p1.lat();
if ((abs(dLatL)+abs(dLonL)) < 1000) {
// VERY close together. Just use equirect approximation with precise integers.
// This is not needed for accuracy (that I can measure), but it is
// a quicker calculation.
return EquirectDistanceRadians( p1, p2 );
}
// Haversine calculation from http://www.movable-type.co.uk/scripts/latlong.html
float dLat = dLatL * RAD_PER_DEG * LOC_SCALE;
float haverDLat = sin(dLat/2.0);
haverDLat *= haverDLat; // squared
float dLon = dLonL * RAD_PER_DEG * LOC_SCALE;
float haverDLon = sin(dLon/2.0);
haverDLon *= haverDLon; // squared
float lat1 = p1.latF();
lat1 *= RAD_PER_DEG;
float lat2 = p2.latF();
lat2 *= RAD_PER_DEG;
float a = haverDLat + cos(lat1) * cos(lat2) * haverDLon;
float c = 2 * atan2( sqrt(a), sqrt(1-a) );
return c;
} // DistanceRadians
//---------------------------------------------------------------------
float Location_t::EquirectDistanceRadians
( const Location_t & p1, const Location_t & p2 )
{
// Equirectangular calculation from http://www.movable-type.co.uk/scripts/latlong.html
float dLat = (p2.lat() - p1.lat()) * RAD_PER_DEG * LOC_SCALE;
float dLon = safeDLon( p2.lon(), p1.lon() ) * RAD_PER_DEG * LOC_SCALE;
float x = dLon * cos( p1.lat() * RAD_PER_DEG * LOC_SCALE + dLat/2 );
return sqrt( x*x + dLat*dLat );
} // EquirectDistanceRadians
//---------------------------------------------------------------------
float Location_t::BearingTo( const Location_t & p1, const Location_t & p2 )
{
int32_t dLonL = safeDLon( p2.lon(), p1.lon() );
float dLon = dLonL * RAD_PER_DEG * LOC_SCALE;
int32_t dLatL = p2.lat() - p1.lat();
float lat1 = p1.lat() * RAD_PER_DEG * LOC_SCALE;
float cosLat1 = cos( lat1 );
float x, y, bearing;
if ((abs(dLatL)+abs(dLonL)) < 1000) {
// VERY close together. Just use equirect approximation with precise integers.
x = dLonL * cosLat1;
y = dLatL;
bearing = PI/2.0 - atan2(y, x);
} else {
float lat2 = p2.lat() * RAD_PER_DEG * LOC_SCALE;
float cosLat2 = cos(lat2);
y = sin(dLon) * cosLat2;
x = cosLat1 * sin(lat2) - sin(lat1) * cosLat2 * cos(dLon);
bearing = atan2(y, x);
}
if (bearing < 0.0)
bearing += TWO_PI;
return bearing;
} // BearingTo
//---------------------------------------------------------------------
void Location_t::OffsetBy( float distR, float bearingR )
{
float lat1 = lat() * RAD_PER_DEG * LOC_SCALE;
float newLat = asin( sin(lat1)*cos(distR) +
cos(lat1)*sin(distR)*cos(bearingR) );
float dLon = atan2( sin(bearingR)*sin(distR)*cos(lat1),
cos(distR)-sin(lat1)*sin(newLat));
_lat = (newLat / (RAD_PER_DEG * LOC_SCALE));
_lon += (dLon / (RAD_PER_DEG * LOC_SCALE));
} // OffsetBy

View File

@@ -0,0 +1,128 @@
#ifndef NEOGPS_LOCATION_H
#define NEOGPS_LOCATION_H
#include <Arduino.h>
#include "NeoGPS_cfg.h"
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
class NMEAGPS;
namespace NeoGPS {
class Location_t
{
public:
CONST_CLASS_DATA float LOC_SCALE = 1.0e-7;
Location_t() {}
Location_t( int32_t lat, int32_t lon )
: _lat(lat), _lon(lon)
{}
Location_t( float lat, float lon )
: _lat(lat / LOC_SCALE), _lon(lon / LOC_SCALE)
{}
Location_t( double lat, double lon )
: _lat(lat / LOC_SCALE), _lon(lon / LOC_SCALE)
{}
int32_t lat() const { return _lat; };
void lat( int32_t l ) { _lat = l; };
float latF() const { return ((float) lat()) * LOC_SCALE; };
void latF( float v ) { _lat = v * LOC_SCALE; };
int32_t lon() const { return _lon; };
void lon( int32_t l ) { _lon = l; };
float lonF() const { return ((float) lon()) * LOC_SCALE; };
void lonF( float v ) { _lon = v * LOC_SCALE; };
void init() { _lat = _lon = 0; };
CONST_CLASS_DATA float EARTH_RADIUS_KM = 6371.0088;
CONST_CLASS_DATA float RAD_PER_DEG = PI / 180.0;
CONST_CLASS_DATA float DEG_PER_RAD = 180.0 / PI;
CONST_CLASS_DATA float MI_PER_KM = 0.621371;
//-----------------------------------
// Distance calculations
static float DistanceKm( const Location_t & p1, const Location_t & p2 )
{
return DistanceRadians( p1, p2 ) * EARTH_RADIUS_KM;
}
float DistanceKm( const Location_t & p2 )
{ return DistanceKm( *this, p2 ); }
static float DistanceMiles( const Location_t & p1, const Location_t & p2 )
{
return DistanceRadians( p1, p2 ) * EARTH_RADIUS_KM * MI_PER_KM;
}
float DistanceMiles( const Location_t & p2 )
{ return DistanceMiles( *this, p2 ); }
static float DistanceRadians( const Location_t & p1, const Location_t & p2 );
float DistanceRadians( const Location_t & p2 )
{ return DistanceRadians( *this, p2 ); }
static float EquirectDistanceRadians
( const Location_t & p1, const Location_t & p2 );
float EquirectDistanceRadians( const Location_t & p2 )
{ return EquirectDistanceRadians( *this, p2 ); }
static float EquirectDistanceKm( const Location_t & p1, const Location_t & p2 )
{
return EquirectDistanceRadians( p1, p2 ) * EARTH_RADIUS_KM;
}
float EquirectDistanceKm( const Location_t & p2 ) const
{ return EquirectDistanceKm( *this, p2 ); }
static float EquirectDistanceMiles( const Location_t & p1, const Location_t & p2 )
{
return EquirectDistanceRadians( p1, p2 ) * EARTH_RADIUS_KM * MI_PER_KM;
}
float EquirectDistanceMiles( const Location_t & p2 ) const
{ return EquirectDistanceMiles( *this, p2 ); }
//-----------------------------------
// Bearing calculations
static float BearingTo( const Location_t & p1, const Location_t & p2 ); // radians
float BearingTo( const Location_t & p2 ) const // radians
{ return BearingTo( *this, p2 ); }
static float BearingToDegrees( const Location_t & p1, const Location_t & p2 )
{ return BearingTo( p1, p2 ) * DEG_PER_RAD; }
float BearingToDegrees( const Location_t & p2 ) const // radians
{ return BearingToDegrees( *this, p2 ); }
//-----------------------------------
// Offset a location (note distance is in radians, not degrees)
void OffsetBy( float distR, float bearingR );
//private: //---------------------------------------
friend class NMEAGPS; // This does not work?!?
int32_t _lat; // degrees * 1e7, negative is South
int32_t _lon; // degrees * 1e7, negative is West
} NEOGPS_PACKED;
} // NeoGPS
#endif

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,340 @@
#ifndef NMEAGPS_H
#define NMEAGPS_H
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "CosaCompat.h"
#include <Arduino.h>
#ifdef __AVR__
#include <avr/interrupt.h>
#endif
#include "GPSfix.h"
#include "NMEAGPS_cfg.h"
//------------------------------------------------------
//
// NMEA 0183 Parser for generic GPS Modules.
//
// As bytes are received from the device, they affect the
// internal FSM and set various members of the current /fix/.
// As multiple sentences are received, they are (optionally)
// merged into a single fix. When the last sentence in a time
// interval (usually 1 second) is received, the fix is stored
// in the (optional) buffer of fixes.
//
// Only these NMEA messages are parsed:
// GGA, GLL, GSA, GST, GSV, RMC, VTG, and ZDA.
class NMEAGPS
{
NMEAGPS & operator =( const NMEAGPS & );
NMEAGPS( const NMEAGPS & );
public:
NMEAGPS();
//.......................................................................
// NMEA standard message types (aka "sentences")
enum nmea_msg_t {
NMEA_UNKNOWN,
#if defined(NMEAGPS_PARSE_GGA) | defined(NMEAGPS_RECOGNIZE_ALL)
NMEA_GGA,
#endif
#if defined(NMEAGPS_PARSE_GLL) | defined(NMEAGPS_RECOGNIZE_ALL)
NMEA_GLL,
#endif
#if defined(NMEAGPS_PARSE_GSA) | defined(NMEAGPS_RECOGNIZE_ALL)
NMEA_GSA,
#endif
#if defined(NMEAGPS_PARSE_GST) | defined(NMEAGPS_RECOGNIZE_ALL)
NMEA_GST,
#endif
#if defined(NMEAGPS_PARSE_GSV) | defined(NMEAGPS_RECOGNIZE_ALL)
NMEA_GSV,
#endif
#if defined(NMEAGPS_PARSE_RMC) | defined(NMEAGPS_RECOGNIZE_ALL)
NMEA_RMC,
#endif
#if defined(NMEAGPS_PARSE_VTG) | defined(NMEAGPS_RECOGNIZE_ALL)
NMEA_VTG,
#endif
#if defined(NMEAGPS_PARSE_ZDA) | defined(NMEAGPS_RECOGNIZE_ALL)
NMEA_ZDA,
#endif
NMEAMSG_END // a bookend that tells how many enums there were
};
CONST_CLASS_DATA nmea_msg_t NMEA_FIRST_MSG = (nmea_msg_t) (NMEA_UNKNOWN+1);
CONST_CLASS_DATA nmea_msg_t NMEA_LAST_MSG = (nmea_msg_t) (NMEAMSG_END-1);
//=======================================================================
// FIX-ORIENTED methods: available, read, overrun and handle
//=======================================================================
// The typical sketch should have something like this in loop():
//
// while (gps.available( Serial1 )) {
// gps_fix fix = gps.read();
// if (fix.valid.location) {
// ...
// }
// }
//.......................................................................
// The available(...) functions return the number of *fixes* that
// are available to be "read" from the fix buffer. The GPS port
// object is passed in so a char can be read if port.available().
uint8_t available( Stream & port )
{
if (processing_style == PS_POLLING)
while (port.available())
handle( port.read() );
return _available();
}
uint8_t available() const volatile { return _available(); };
//.......................................................................
// Return the next available fix. When no more fixes
// are available, it returns an empty fix.
const gps_fix read();
//.......................................................................
// The OVERRUN flag is set whenever a fix is not read by the time
// the next update interval starts. You must clear it when you
// detect the condition.
bool overrun() const { return _overrun; }
void overrun( bool val ) { _overrun = val; }
//.......................................................................
// As characters are processed, they can be categorized as
// INVALID (not part of this protocol), OK (accepted),
// or COMPLETED (end-of-message).
enum decode_t { DECODE_CHR_INVALID, DECODE_CHR_OK, DECODE_COMPLETED };
//.......................................................................
// Process one character, possibly saving a buffered fix.
// It implements merging and coherency.
// This can be called from an ISR.
decode_t handle( uint8_t c );
//=======================================================================
// CHARACTER-ORIENTED methods: decode, fix and is_safe
//=======================================================================
//
// *** MOST APPLICATIONS SHOULD USE THE FIX-ORIENTED METHODS ***
//
// Using `decode` is only necessary if you want finer control
// on how fix information is filtered and merged.
//
// Process one character of an NMEA GPS sentence. The internal state
// machine tracks what part of the sentence has been received. As the
// sentence is received, members of the /fix/ structure are updated.
// This character-oriented method *does not* buffer any fixes, and
// /read()/ will always return an empty fix.
//
// @return DECODE_COMPLETED when a sentence has been completely received.
NMEAGPS_VIRTUAL decode_t decode( char c );
//.......................................................................
// Current fix accessor.
// *** MOST APPLICATIONS SHOULD USE read() TO GET THE CURRENT FIX ***
// /fix/ will be constantly changing as characters are received.
//
// For example, fix().longitude() may return nonsense data if
// characters for that field are currently being processed in /decode/.
gps_fix & fix() { return m_fix; };
// NOTE: /is_safe/ *must* be checked before accessing members of /fix/.
// If you need access to the current /fix/ at any time, you should
// use the FIX-ORIENTED methods.
//.......................................................................
// Determine whether the members of /fix/ are "currently" safe.
// It will return true when a complete sentence and the CRC characters
// have been received (or after a CR if no CRC is present).
// It will return false after a new sentence starts.
bool is_safe() const volatile { return (rxState == NMEA_IDLE); }
// NOTE: When INTERRUPT_PROCESSING is enabled, is_safe()
// and fix() could change at any time (i.e., they should be
// considered /volatile/).
//=======================================================================
// DATA MEMBER accessors and mutators
//=======================================================================
//.......................................................................
// Convert a nmea_msg_t to a PROGMEM string.
// Useful for printing the sentence type instead of a number.
// This can return "UNK" if the message is not a valid number.
const __FlashStringHelper *string_for( nmea_msg_t msg ) const;
//.......................................................................
// Most recent NMEA sentence type received.
enum nmea_msg_t nmeaMessage NEOGPS_BF(8);
//.......................................................................
// Storage for Talker and Manufacturer IDs
#ifdef NMEAGPS_SAVE_TALKER_ID
char talker_id[2];
#endif
#ifdef NMEAGPS_SAVE_MFR_ID
char mfr_id[3];
#endif
//.......................................................................
// Various parsing statistics
#ifdef NMEAGPS_STATS
struct statistics_t {
uint32_t ok; // count of successfully parsed sentences
uint32_t errors; // NMEA checksum or other message errors
uint32_t chars;
void init()
{
ok = 0L;
errors = 0L;
chars = 0L;
}
} statistics;
#endif
//.......................................................................
// SATELLITE VIEW array
#ifdef NMEAGPS_PARSE_SATELLITES
struct satellite_view_t
{
uint8_t id;
#ifdef NMEAGPS_PARSE_SATELLITE_INFO
uint8_t elevation; // 0..99 deg
uint16_t azimuth; // 0..359 deg
uint8_t snr NEOGPS_BF(7); // 0..99 dBHz
bool tracked NEOGPS_BF(1);
#endif
} NEOGPS_PACKED;
satellite_view_t satellites[ NMEAGPS_MAX_SATELLITES ];
uint8_t sat_count; // in the above array
bool satellites_valid() const { return (sat_count >= m_fix.satellites); }
#endif
//.......................................................................
// Reset the parsing process.
// This is used internally after a CS error, or could be used
// externally to abort processing if it has been too long
// since any data was received.
void reset()
{
rxState = NMEA_IDLE;
}
//=======================================================================
// CORRELATING Arduino micros() WITH UTC.
//=======================================================================
#if defined(NMEAGPS_TIMESTAMP_FROM_PPS) | \
defined(NMEAGPS_TIMESTAMP_FROM_INTERVAL)
protected:
uint32_t _UTCsecondStart;
#if defined(NMEAGPS_TIMESTAMP_FROM_INTERVAL) & \
( defined(GPS_FIX_DATE) | defined(GPS_FIX_TIME) )
uint32_t _IntervalStart; // quiet time just ended
#endif
public:
// The micros() value when the current UTC second started
uint32_t UTCsecondStart() const
{ lock();
uint32_t ret = _UTCsecondStart;
unlock();
return ret;
};
void UTCsecondStart( uint32_t us ) { _UTCsecondStart = us; };
// The elapsed time since the start of the current UTC second
uint32_t UTCus() const { return micros() - UTCsecondStart(); };
uint32_t UTCms() const { return UTCus() / 1000UL; };
// If you have attached a Pin Change interrupt routine to the PPS pin:
//
// const int PPSpin = 5;
// void PPSisr() { gps.UTCsecondStart( micros() ); };
// void setup()
// {
// attachInterrupt( digitalPinToInterrupt(PPSpin), PPSisr, RISING );
// }
//
// If you are using an Input Capture pin, calculate the elapsed
// microseconds since the capture time (based on the TIMER
// frequency):
//
// void savePPSus() // called as an ISR or from a test in loop
// {
// uint32_t elapsedUS = (currentCount - captureCount) * countUS;
// gps.UTCsecondStart( micros() - elapsedUS );
// }
#endif
//=======================================================================
// COMMUNICATING WITH THE GPS DEVICE: poll, send and send_P
//=======================================================================
//.......................................................................
// Request the specified NMEA sentence. Not all devices will respond.
static void poll( Stream *device, nmea_msg_t msg );
//.......................................................................
// Send a message to the GPS device.
// The '$' is optional, and the '*' and CS will be added automatically.
static void send( Stream *device, const char *msg );
static void send_P( Stream *device, const __FlashStringHelper *msg );
#include "NMEAGPSprivate.h"
} NEOGPS_PACKED;
#endif

View File

@@ -0,0 +1,338 @@
#ifndef NMEAGPS_CFG_H
#define NMEAGPS_CFG_H
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "GPSfix_cfg.h"
//------------------------------------------------------
// Enable/disable the parsing of specific sentences.
//
// Configuring out a sentence prevents it from being recognized; it
// will be completely ignored. (See also NMEAGPS_RECOGNIZE_ALL, below)
//
// FYI: Only RMC and ZDA contain date information. Other
// sentences contain time information. Both date and time are
// required if you will be doing time_t-to-clock_t operations.
#define NMEAGPS_PARSE_GGA
//#define NMEAGPS_PARSE_GLL
//#define NMEAGPS_PARSE_GSA
//#define NMEAGPS_PARSE_GSV
//#define NMEAGPS_PARSE_GST
#define NMEAGPS_PARSE_RMC
//#define NMEAGPS_PARSE_VTG
//#define NMEAGPS_PARSE_ZDA
//------------------------------------------------------
// Select which sentence is sent *last* by your GPS device
// in each update interval. This can be used by your sketch
// to determine when the GPS quiet time begins, and thus
// when you can perform "some" time-consuming operations.
#define LAST_SENTENCE_IN_INTERVAL NMEAGPS::NMEA_RMC
// NOTE: For PUBX-only configs, use
// (NMEAGPS::nmea_msg_t)(NMEAGPS::NMEA_LAST_MSG+1)
//
// If the NMEA_LAST_SENTENCE_IN_INTERVAL is not chosen
// correctly, GPS data may be lost because the sketch
// takes too long elsewhere when this sentence is received.
// Also, fix members may contain information from different
// time intervals (i.e., they are not coherent).
//
// If you don't know which sentence is the last one,
// use NMEAorder.ino to list them. You do not have to select
// the last sentence the device sends if you have disabled
// it. Just select the last sentence that you have *enabled*.
//------------------------------------------------------
// Choose how multiple sentences are merged into a fix:
// 1) No merging
// Each sentence fills out its own fix; there could be
// multiple sentences per interval.
// 2) EXPLICIT_MERGING
// All sentences in an interval are *safely* merged into one fix.
// NMEAGPS_FIX_MAX must be >= 1.
// An interval is defined by NMEA_LAST_SENTENCE_IN_INTERVAL.
// 3) IMPLICIT_MERGING
// All sentences in an interval are merged into one fix, with
// possible data loss. If a received sentence is rejected for
// any reason (e.g., a checksum error), all the values are suspect.
// The fix will be cleared; no members will be valid until new
// sentences are received and accepted. This uses less RAM.
// An interval is defined by NMEA_LAST_SENTENCE_IN_INTERVAL.
// Uncomment zero or one:
#define NMEAGPS_EXPLICIT_MERGING
//#define NMEAGPS_IMPLICIT_MERGING
#ifdef NMEAGPS_IMPLICIT_MERGING
#define NMEAGPS_MERGING NMEAGPS::IMPLICIT_MERGING
// Nothing is done to the fix at the beginning of every sentence...
#define NMEAGPS_INIT_FIX(m)
// ...but we invalidate one part when it starts to get parsed. It *may* get
// validated when the parsing is finished.
#define NMEAGPS_INVALIDATE(m) m_fix.valid.m = false
#else
#ifdef NMEAGPS_EXPLICIT_MERGING
#define NMEAGPS_MERGING NMEAGPS::EXPLICIT_MERGING
#else
#define NMEAGPS_MERGING NMEAGPS::NO_MERGING
#define NMEAGPS_NO_MERGING
#endif
// When NOT accumulating (not IMPLICIT), invalidate the entire fix
// at the beginning of every sentence...
#define NMEAGPS_INIT_FIX(m) m.valid.init()
// ...so the individual parts do not need to be invalidated as they are parsed
#define NMEAGPS_INVALIDATE(m)
#endif
#if ( defined(NMEAGPS_NO_MERGING) + \
defined(NMEAGPS_IMPLICIT_MERGING) + \
defined(NMEAGPS_EXPLICIT_MERGING) ) > 1
#error Only one MERGING technique should be enabled in NMEAGPS_cfg.h!
#endif
//------------------------------------------------------
// Define the fix buffer size. The NMEAGPS object will hold on to
// this many fixes before an overrun occurs. This can be zero,
// but you have to be more careful about using gps.fix() structure,
// because it will be modified as characters are received.
#define NMEAGPS_FIX_MAX 1
#if defined(NMEAGPS_EXPLICIT_MERGING) && (NMEAGPS_FIX_MAX == 0)
#error You must define FIX_MAX >= 1 to allow EXPLICIT merging in NMEAGPS_cfg.h
#endif
//------------------------------------------------------
// Define how fixes are dropped when the FIFO is full.
// true = the oldest fix will be dropped, and the new fix will be saved.
// false = the new fix will be dropped, and all old fixes will be saved.
#define NMEAGPS_KEEP_NEWEST_FIXES true
//------------------------------------------------------
// Enable/Disable interrupt-style processing of GPS characters
// If you are using one of the NeoXXSerial libraries,
// to attachInterrupt, this must be defined.
// Otherwise, it must be commented out.
//#define NMEAGPS_INTERRUPT_PROCESSING
#ifdef NMEAGPS_INTERRUPT_PROCESSING
#define NMEAGPS_PROCESSING_STYLE NMEAGPS::PS_INTERRUPT
#else
#define NMEAGPS_PROCESSING_STYLE NMEAGPS::PS_POLLING
#endif
//------------------------------------------------------
// Enable/disable the talker ID, manufacturer ID and proprietary message processing.
//
// First, some background information. There are two kinds of NMEA sentences:
//
// 1. Standard NMEA sentences begin with "$ttccc", where
// "tt" is the talker ID, and
// "ccc" is the variable-length sentence type (i.e., command).
//
// For example, "$GPGLL,..." is a GLL sentence (Geographic Lat/Long)
// transmitted by talker "GP". This is the most common talker ID. Some
// devices may report "$GNGLL,..." when a mix of GPS and non-GPS
// satellites have been used to determine the GLL data.
//
// 2. Proprietary NMEA sentences (i.e., those unique to a particular
// manufacturer) begin with "$Pmmmccc", where
// "P" is the NMEA-defined prefix indicator for proprietary messages,
// "mmm" is the 3-character manufacturer ID, and
// "ccc" is the variable-length sentence type (it can be empty).
//
// No validation of manufacturer ID and talker ID is performed in this
// base class. For example, although "GP" is a common talker ID, it is not
// guaranteed to be transmitted by your particular device, and it IS NOT
// REQUIRED. If you need validation of these IDs, or you need to use the
// extra information provided by some devices, you have two independent
// options:
//
// 1. Enable SAVING the ID: When /decode/ returns DECODE_COMPLETED, the
// /talker_id/ and/or /mfr_id/ members will contain ID bytes. The entire
// sentence will be parsed, perhaps modifying members of /fix/. You should
// enable one or both IDs if you want the information in all sentences *and*
// you also want to know the ID bytes. This adds two bytes of RAM for the
// talker ID, and 3 bytes of RAM for the manufacturer ID.
//
// 2. Enable PARSING the ID: The virtual /parse_talker_id/ and
// /parse_mfr_id/ will receive each ID character as it is parsed. If it
// is not a valid ID, return /false/ to abort processing the rest of the
// sentence. No CPU time will be wasted on the invalid sentence, and no
// /fix/ members will be modified. You should enable this if you want to
// ignore some IDs. You must override /parse_talker_id/ and/or
// /parse_mfr_id/ in a derived class.
//
//#define NMEAGPS_SAVE_TALKER_ID
//#define NMEAGPS_PARSE_TALKER_ID
//#define NMEAGPS_PARSE_PROPRIETARY
#ifdef NMEAGPS_PARSE_PROPRIETARY
//#define NMEAGPS_SAVE_MFR_ID
#define NMEAGPS_PARSE_MFR_ID
#endif
//------------------------------------------------------
// Enable/disable tracking the current satellite array and,
// optionally, all the info for each satellite.
//
//#define NMEAGPS_PARSE_SATELLITES
//#define NMEAGPS_PARSE_SATELLITE_INFO
#ifdef NMEAGPS_PARSE_SATELLITES
#define NMEAGPS_MAX_SATELLITES (20)
#ifndef GPS_FIX_SATELLITES
#error GPS_FIX_SATELLITES must be defined in GPSfix.h!
#endif
#endif
#if defined(NMEAGPS_PARSE_SATELLITE_INFO) & \
!defined(NMEAGPS_PARSE_SATELLITES)
#error NMEAGPS_PARSE_SATELLITES must be defined!
#endif
//------------------------------------------------------
// Enable/disable gathering interface statistics:
// CRC errors and number of sentences received
#define NMEAGPS_STATS
//------------------------------------------------------
// Configuration item for allowing derived types of NMEAGPS.
// If you derive classes from NMEAGPS, you *must* define NMEAGPS_DERIVED_TYPES.
// If not defined, virtuals are not used, with a slight size (2 bytes) and
// execution time savings.
//#define NMEAGPS_DERIVED_TYPES
#ifdef NMEAGPS_DERIVED_TYPES
#define NMEAGPS_VIRTUAL virtual
#else
#define NMEAGPS_VIRTUAL
#endif
//-----------------------------------
// See if DERIVED_TYPES is required
#if (defined(NMEAGPS_PARSE_TALKER_ID) | defined(NMEAGPS_PARSE_MFR_ID)) & \
!defined(NMEAGPS_DERIVED_TYPES)
#error You must define NMEAGPS_DERIVED_TYPES in NMEAGPS.h in order to parse Talker and/or Mfr IDs!
#endif
//------------------------------------------------------
// Becase the NMEA checksum is not very good at error detection, you can
// choose to enable additional validity checks. This trades a little more
// code and execution time for more reliability.
//
// Validation at the character level is a syntactic check only. For
// example, integer fields must contain characters in the range 0..9,
// latitude hemisphere letters can be 'N' or 'S'. Characters that are not
// valid for a particular field will cause the entire sentence to be
// rejected as an error, *regardless* of whether the checksum would pass.
#define NMEAGPS_VALIDATE_CHARS false
// Validation at the field level is a semantic check. For
// example, latitude degrees must be in the range -90..+90.
// Values that are not valid for a particular field will cause the
// entire sentence to be rejected as an error, *regardless* of whether the
// checksum would pass.
#define NMEAGPS_VALIDATE_FIELDS false
//------------------------------------------------------
// Some devices may omit trailing commas at the end of some
// sentences. This may prevent the last field from being
// parsed correctly, because the parser for some types keep
// the value in an intermediate state until the complete
// field is received (e.g., parseDDDMM, parseFloat and
// parseZDA).
//
// Enabling this will inject a simulated comma when the end
// of a sentence is received and the last field parser
// indicated that it still needs one.
#define NMEAGPS_COMMA_NEEDED
//------------------------------------------------------
// Some applications may want to recognize a sentence type
// without actually parsing any of the fields. Uncommenting
// this define will allow the nmeaMessage member to be set
// when *any* standard message is seen, even though that
// message is not enabled by a NMEAGPS_PARSE_xxx define above.
// No valid flags will be true for those sentences.
#define NMEAGPS_RECOGNIZE_ALL
//------------------------------------------------------
// Sometimes, a little extra space is needed to parse an intermediate form.
// This config items enables extra space.
//#define NMEAGPS_PARSING_SCRATCHPAD
//------------------------------------------------------
// If you need to know the exact UTC time at *any* time,
// not just after a fix arrives, you must calculate the
// offset between the Arduino micros() clock and the UTC
// time in a received fix. There are two ways to do this:
//
// 1) When the GPS quiet time ends and the new update interval begins.
// The timestamp will be set when the first character (the '$') of
// the new batch of sentences arrives from the GPS device. This is fairly
// accurate, but it will be delayed from the PPS edge by the GPS device's
// fix calculation time (usually ~100us). There is very little variance
// in this calculation time (usually < 30us), so all timestamps are
// delayed by a nearly-constant amount.
//
// NOTE: At update rates higher than 1Hz, the updates may arrive with
// some increasing variance.
//#define NMEAGPS_TIMESTAMP_FROM_INTERVAL
// 2) From the PPS pin of the GPS module. It is up to the application
// developer to decide how to capture that event. For example, you could:
//
// a) simply poll for it in loop and call UTCsecondStart(micros());
// b) use attachInterrupt to call a Pin Change Interrupt ISR to save
// the micros() at the time of the interrupt (see NMEAGPS.h), or
// c) connect the PPS to an Input Capture pin. Set the
// associated TIMER frequency, calculate the elapsed time
// since the PPS edge, and add that to the current micros().
//#define NMEAGPS_TIMESTAMP_FROM_PPS
#if defined( NMEAGPS_TIMESTAMP_FROM_INTERVAL ) & \
defined( NMEAGPS_TIMESTAMP_FROM_PPS )
#error You cannot enable both TIMESTAMP_FROM_INTERVAL and PPS in NMEAGPS_cfg.h!
#endif
#endif

View File

@@ -0,0 +1,408 @@
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
protected:
//.......................................................................
// Table entry for NMEA sentence type string and its offset
// in enumerated nmea_msg_t. Proprietary sentences can be implemented
// in derived classes by adding a second table. Additional tables
// can be singly-linked through the /previous/ member. The instantiated
// class's table is the head, and should be returned by the derived
// /msg_table/ function. Tables should be sorted alphabetically.
struct msg_table_t {
uint8_t offset; // nmea_msg_t enum starting value
const msg_table_t *previous;
uint8_t size; // number of entries in table array
const char * const *table; // array of NMEA sentence strings
};
static const msg_table_t nmea_msg_table __PROGMEM;
NMEAGPS_VIRTUAL const msg_table_t *msg_table() const
{ return &nmea_msg_table; };
//.......................................................................
// These virtual methods can accept or reject
// the talker ID (for standard sentences) or
// the manufacturer ID (for proprietary sentences).
// The default is to accept *all* IDs.
// Override them if you want to reject certain IDs, or you want
// to handle COMPLETED sentences from certain IDs differently.
#ifdef NMEAGPS_PARSE_TALKER_ID
NMEAGPS_VIRTUAL bool parseTalkerID( char ) { return true; };
#endif
#ifdef NMEAGPS_PARSE_PROPRIETARY
#ifdef NMEAGPS_PARSE_MFR_ID
NMEAGPS_VIRTUAL bool parseMfrID( char ) { return true; };
#endif
#endif
public:
//.......................................................................
// Set all parsed data to initial values.
void data_init()
{
fix().init();
#ifdef NMEAGPS_PARSE_SATELLITES
sat_count = 0;
#endif
}
//.......................................................................
enum merging_t { NO_MERGING, EXPLICIT_MERGING, IMPLICIT_MERGING };
static const merging_t
merging = NMEAGPS_MERGING; // see NMEAGPS_cfg.h
enum processing_style_t { PS_POLLING, PS_INTERRUPT };
static const processing_style_t
processing_style = NMEAGPS_PROCESSING_STYLE; // see NMEAGPS_cfg.h
static const bool keepNewestFixes = NMEAGPS_KEEP_NEWEST_FIXES;
static const bool validateChars () { return NMEAGPS_VALIDATE_CHARS; }
static const bool validateFields() { return NMEAGPS_VALIDATE_FIELDS; }
//.......................................................................
// Control access to this object. This preserves atomicity when
// the processing style is interrupt-driven.
void lock() const
{
if (processing_style == PS_INTERRUPT)
noInterrupts();
}
void unlock() const
{
if (processing_style == PS_INTERRUPT)
interrupts();
}
protected:
//=======================================================================
// PARSING FINITE-STATE MACHINE
//=======================================================================
// Current fix
gps_fix m_fix;
// Current parser state
uint8_t crc; // accumulated CRC in the sentence
uint8_t fieldIndex; // index of current field in the sentence
uint8_t chrCount; // index of current character in current field
uint8_t decimal; // digits received after the decimal point
struct {
bool negative NEOGPS_BF(1); // field had a leading '-'
bool _comma_needed NEOGPS_BF(1); // field needs a comma to finish parsing
bool group_valid NEOGPS_BF(1); // multi-field group valid
bool _overrun NEOGPS_BF(1); // an entire fix was dropped
bool _intervalComplete NEOGPS_BF(1); // automatically set after LAST received
#if (NMEAGPS_FIX_MAX == 0)
bool _fixesAvailable NEOGPS_BF(1);
#endif
#ifdef NMEAGPS_PARSE_PROPRIETARY
bool proprietary NEOGPS_BF(1); // receiving proprietary message
#endif
} NEOGPS_PACKED;
#ifdef NMEAGPS_PARSING_SCRATCHPAD
union {
uint32_t U4;
uint16_t U2[2];
uint8_t U1[4];
} scratchpad;
#endif
bool comma_needed()
{
#ifdef NMEAGPS_COMMA_NEEDED
return _comma_needed;
#else
return false;
#endif
}
void comma_needed( bool value )
{
#ifdef NMEAGPS_COMMA_NEEDED
_comma_needed = value;
#endif
}
// Internal FSM states
enum rxState_t {
NMEA_IDLE, // Waiting for initial '$'
NMEA_RECEIVING_HEADER, // Parsing sentence type field
NMEA_RECEIVING_DATA, // Parsing fields up to the terminating '*'
NMEA_RECEIVING_CRC // Receiving two-byte transmitted CRC
};
CONST_CLASS_DATA uint8_t NMEA_FIRST_STATE = NMEA_IDLE;
CONST_CLASS_DATA uint8_t NMEA_LAST_STATE = NMEA_RECEIVING_CRC;
rxState_t rxState NEOGPS_BF(8);
//.......................................................................
uint8_t _available() const volatile { return _fixesAvailable; };
//.......................................................................
// Buffered fixes.
#if (NMEAGPS_FIX_MAX > 0)
gps_fix buffer[ NMEAGPS_FIX_MAX ]; // could be empty, see NMEAGPS_cfg.h
uint8_t _fixesAvailable;
uint8_t _firstFix;
uint8_t _currentFix;
#endif
//.......................................................................
// Indicate that the next sentence should initialize the internal data.
// This is useful for coherency or custom filtering.
bool intervalComplete() const { return _intervalComplete; }
void intervalComplete( bool val ) { _intervalComplete = val; }
//.......................................................................
// Identify when an update interval is completed, according to the
// most recently-received sentence. In this base class, it just
// looks at the nmeaMessage member. Derived classes may have
// more complex, specific conditions.
NMEAGPS_VIRTUAL bool intervalCompleted() const
{ return (nmeaMessage == LAST_SENTENCE_IN_INTERVAL); }
// see NMEAGPS_cfg.h
//.......................................................................
// When a fix has been fully assembled from a batch of sentences, as
// determined by the configured merging technique and ending with the
// LAST_SENTENCE_IN_INTERVAL, it is stored in the (optional) buffer
// of fixes. They are removed with /read()/.
void storeFix();
//=======================================================================
// PARSING METHODS
//=======================================================================
//.......................................................................
// Try to recognize an NMEA sentence type, after the IDs have been accepted.
decode_t parseCommand( char c );
decode_t parseCommand( const msg_table_t *msgs, uint8_t cmdCount, char c );
//.......................................................................
// Parse various NMEA sentences
bool parseGGA( char chr );
bool parseGLL( char chr );
bool parseGSA( char chr );
bool parseGST( char chr );
bool parseGSV( char chr );
bool parseRMC( char chr );
bool parseVTG( char chr );
bool parseZDA( char chr );
//.......................................................................
// Depending on the NMEA sentence type, parse one field of an expected type.
NMEAGPS_VIRTUAL bool parseField( char chr );
//.......................................................................
// Parse the primary NMEA field types into /fix/ members.
bool parseFix ( char chr ); // aka STATUS or MODE
bool parseTime ( char chr );
bool parseDDMMYY ( char chr );
bool parseLat ( char chr );
bool parseNS ( char chr );
bool parseLon ( char chr );
bool parseEW ( char chr );
bool parseSpeed ( char chr );
bool parseHeading ( char chr );
bool parseAlt ( char chr );
bool parseGeoidHeight( char chr );
bool parseHDOP ( char chr );
bool parseVDOP ( char chr );
bool parsePDOP ( char chr );
bool parse_lat_err ( char chr );
bool parse_lon_err ( char chr );
bool parse_alt_err ( char chr );
bool parseSatellites ( char chr );
// Helper macro for parsing the 4 consecutive fields of a location
#define PARSE_LOC(i) case i: return parseLat( chr );\
case i+1: return parseNS ( chr ); \
case i+2: return parseLon( chr ); \
case i+3: return parseEW ( chr );
//.......................................................................
// Parse floating-point numbers into a /whole_frac/
// @return true when the value is fully populated.
bool parseFloat( gps_fix::whole_frac & val, char chr, uint8_t max_decimal );
//.......................................................................
// Parse floating-point numbers into a uint16_t
// @return true when the value is fully populated.
bool parseFloat( uint16_t & val, char chr, uint8_t max_decimal );
//.......................................................................
// Parse NMEA lat/lon dddmm.mmmm degrees
bool parseDDDMM
(
#if defined( GPS_FIX_LOCATION )
int32_t & val,
#endif
#if defined( GPS_FIX_LOCATION_DMS )
DMS_t & dms,
#endif
char chr
);
//.......................................................................
// Parse integer into 8-bit int
// @return true when non-empty value
bool parseInt( uint8_t &val, uint8_t chr )
{
negative = false;
bool is_comma = (chr == ',');
if (chrCount == 0) {
if (is_comma)
return false; // empty field!
if (((validateChars() || validateFields()) && (chr == '-')) ||
(validateChars() && !isdigit( chr )))
sentenceInvalid();
else
val = (chr - '0');
} else if (!is_comma) {
if (validateChars() && !isdigit( chr ))
sentenceInvalid();
else
val = (val*10) + (chr - '0');
}
return true;
}
//.......................................................................
// Parse integer into signed 8-bit int
// @return true when non-empty value
bool parseInt( int8_t &val, uint8_t chr )
{
bool is_comma = (chr == ',');
if (chrCount == 0) {
if (is_comma)
return false; // empty field!
negative = (chr == '-');
if (negative) {
comma_needed( true ); // to negate
val = 0;
} else if (validateChars() && !isdigit( chr )) {
sentenceInvalid();
} else {
val = (chr - '0');
}
} else if (!is_comma) {
val = (val*10) + (chr - '0');
} else if (negative) {
val = -val;
}
return true;
}
//.......................................................................
// Parse integer into 16-bit int
// @return true when non-empty value
bool parseInt( uint16_t &val, uint8_t chr )
{
negative = false;
bool is_comma = (chr == ',');
if (chrCount == 0) {
if (is_comma)
return false; // empty field!
if (((validateChars() || validateFields()) && (chr == '-')) ||
(validateChars() && !isdigit( chr )))
sentenceInvalid();
else
val = (chr - '0');
} else if (!is_comma) {
if (validateChars() && !isdigit( chr ))
sentenceInvalid();
else
val = (val*10) + (chr - '0');
}
return true;
}
//.......................................................................
// Parse integer into 32-bit int
// @return true when non-empty value
bool parseInt( uint32_t &val, uint8_t chr )
{
negative = false;
bool is_comma = (chr == ',');
if (chrCount == 0) {
if (is_comma)
return false; // empty field!
if (((validateChars() || validateFields()) && (chr == '-')) ||
(validateChars() && !isdigit( chr )))
sentenceInvalid();
else
val = (chr - '0');
} else if (!is_comma) {
if (validateChars() && !isdigit( chr ))
sentenceInvalid();
else
val = (val*10) + (chr - '0');
}
return true;
}
private:
void sentenceBegin ();
void sentenceOk ();
void sentenceInvalid ();
void sentenceUnrecognized();
void headerReceived ();

View File

@@ -0,0 +1,111 @@
#ifndef NEOGPS_CFG
#define NEOGPS_CFG
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
//------------------------------------------------------------------------
// Enable/disable packed data structures.
//
// Enabling packed data structures will use two less-portable language
// features of GCC to reduce RAM requirements. Although it was expected to slightly increase execution time and code size, the reverse is true on 8-bit AVRs: the code is smaller and faster with packing enabled.
//
// Disabling packed data structures will be very portable to other
// platforms. NeoGPS configurations will use slightly more RAM, and on
// 8-bit AVRs, the speed is slightly slower, and the code is slightly
// larger. There may be no choice but to disable packing on processors
// that do not support packed structures.
//
// There may also be compiler-specific switches that affect packing and the
// code which accesses packed members. YMMV.
#ifdef __AVR__
#define NEOGPS_PACKED_DATA
#endif
//------------------------------------------------------------------------
// Based on the above define, choose which set of packing macros should
// be used in the rest of the NeoGPS package. Do not change these defines.
#ifdef NEOGPS_PACKED_DATA
// This is for specifying the number of bits to be used for a
// member of a struct. Booleans are typically one bit.
#define NEOGPS_BF(b) :b
// This is for requesting the compiler to pack the struct or class members
// "as closely as possible". This is a compiler-dependent interpretation.
#define NEOGPS_PACKED __attribute__((packed))
#else
// Let the compiler do whatever it wants.
#define NEOGPS_PACKED
#define NEOGPS_BF(b)
#endif
//------------------------------------------------------------------------
// Accommodate C++ compiler and IDE changes.
//
// Declaring constants as class data instead of instance data helps avoid
// collisions with #define names, and allows the compiler to perform more
// checks on their usage.
//
// Until C++ 10 and IDE 1.6.8, initialized class data constants
// were declared like this:
//
// static const <valued types> = <constant-value>;
//
// Now, non-simple types (e.g., float) must be declared as
//
// static constexpr <nonsimple-types> = <expression-treated-as-const>;
//
// The good news is that this allows the compiler to optimize out an
// expression that is "promised" to be "evaluatable" as a constant.
// The bad news is that it introduces a new language keyword, and the old
// code raises an error.
//
// TODO: Evaluate the requirement for the "static" keyword.
// TODO: Evaluate using a C++ version preprocessor symbol for the #if.
//
// The CONST_CLASS_DATA define will expand to the appropriate keywords.
//
#if (ARDUINO < 10606) | ((10700 <= ARDUINO) & (ARDUINO <= 10799)) | ((107000 <= ARDUINO) & (ARDUINO <= 107999))
#define CONST_CLASS_DATA static const
#else
#define CONST_CLASS_DATA static constexpr
#endif
//------------------------------------------------------------------------
// The PROGMEM definitions are not correct for Zero, MKR1000 and
// earlier versions of Teensy boards
#if defined(ARDUINO_SAMD_MKRZERO) | defined(ARDUINO_SAMD_ZERO) | \
(defined(TEENSYDUINO) && (TEENSYDUINO < 139))
#undef pgm_read_ptr
#define pgm_read_ptr(addr) (*(const void **)(addr))
#endif
#endif

View File

@@ -0,0 +1,208 @@
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "NeoTime.h"
// For strtoul declaration
#include <stdlib.h>
#include <Print.h>
Print & operator<<( Print& outs, const NeoGPS::time_t& t )
{
outs.print( t.full_year( t.year ) );
outs.write( '-' );
if (t.month < 10) outs.write( '0' );
outs.print( t.month );
outs.write( '-' );
if (t.date < 10) outs.write( '0' );
outs.print( t.date );
outs.write( ' ' );
if (t.hours < 10) outs.write( '0' );
outs.print( t.hours );
outs.write( ':' );
if (t.minutes < 10) outs.write( '0' );
outs.print( t.minutes );
outs.write( ':' );
if (t.seconds < 10) outs.write( '0' );
outs.print( t.seconds );
return outs;
}
using NeoGPS::time_t;
bool time_t::parse(str_P s)
{
static size_t BUF_MAX = 32;
char buf[BUF_MAX];
strcpy_P(buf, s);
char* sp = &buf[0];
uint16_t value = strtoul(sp, &sp, 10);
if (*sp != '-') return false;
year = value % 100;
if (full_year() != value) return false;
value = strtoul(sp + 1, &sp, 10);
if (*sp != '-') return false;
month = value;
value = strtoul(sp + 1, &sp, 10);
if (*sp != ' ') return false;
date = value;
value = strtoul(sp + 1, &sp, 10);
if (*sp != ':') return false;
hours = value;
value = strtoul(sp + 1, &sp, 10);
if (*sp != ':') return false;
minutes = value;
value = strtoul(sp + 1, &sp, 10);
if (*sp != 0) return false;
seconds = value;
return (is_valid());
}
#ifdef TIME_EPOCH_MODIFIABLE
uint16_t time_t::s_epoch_year = Y2K_EPOCH_YEAR;
uint8_t time_t::s_epoch_offset = 0;
uint8_t time_t::s_epoch_weekday = Y2K_EPOCH_WEEKDAY;
uint8_t time_t::s_pivot_year = 0;
#endif
const uint8_t time_t::days_in[] __PROGMEM = {
0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31
};
time_t::time_t(clock_t c)
{
uint16_t dayno = c / SECONDS_PER_DAY;
c -= dayno * (uint32_t) SECONDS_PER_DAY;
day = weekday_for(dayno);
uint16_t y = epoch_year();
for (;;) {
uint16_t days = days_per( y );
if (dayno < days) break;
dayno -= days;
y++;
}
bool leap_year = is_leap(y);
y -= epoch_year();
y += epoch_offset();
while (y > 100)
y -= 100;
year = y;
month = 1;
for (;;) {
uint8_t days = pgm_read_byte(&days_in[month]);
if (leap_year && (month == 2)) days++;
if (dayno < days) break;
dayno -= days;
month++;
}
date = dayno + 1;
hours = c / SECONDS_PER_HOUR;
uint16_t c_ms;
if (hours < 18) // save 16uS
c_ms = (uint16_t) c - (hours * (uint16_t) SECONDS_PER_HOUR);
else
c_ms = c - (hours * (uint32_t) SECONDS_PER_HOUR);
minutes = c_ms / SECONDS_PER_MINUTE;
seconds = c_ms - (minutes * SECONDS_PER_MINUTE);
}
void time_t::init()
{
seconds =
hours =
minutes = 0;
date = 1;
month = 1;
year = epoch_year() % 100;
day = epoch_weekday();
}
time_t::operator clock_t() const
{
clock_t c = days() * SECONDS_PER_DAY;
if (hours < 18)
c += hours * (uint16_t) SECONDS_PER_HOUR;
else
c += hours * (uint32_t) SECONDS_PER_HOUR;
c += minutes * (uint16_t) SECONDS_PER_MINUTE;
c += seconds;
return (c);
}
uint16_t time_t::days() const
{
uint16_t day_count = day_of_year();
uint16_t y = full_year();
while (y-- > epoch_year())
day_count += days_per(y);
return (day_count);
}
uint16_t time_t::day_of_year() const
{
uint16_t dayno = date - 1;
bool leap_year = is_leap();
for (uint8_t m = 1; m < month; m++) {
dayno += pgm_read_byte(&days_in[m]);
if (leap_year && (m == 2)) dayno++;
}
return (dayno);
}
#ifdef TIME_EPOCH_MODIFIABLE
void time_t::use_fastest_epoch()
{
// Figure out when we were compiled and use the year for a really
// fast epoch_year. Format "MMM DD YYYY"
const char* compile_date = (const char *) PSTR(__DATE__);
uint16_t compile_year = 0;
for (uint8_t i = 7; i < 11; i++)
compile_year = compile_year*10 + (pgm_read_byte(&compile_date[i]) - '0');
// Temporarily set a Y2K epoch so we can figure out the day for
// January 1 of this year
epoch_year ( Y2K_EPOCH_YEAR );
epoch_weekday ( Y2K_EPOCH_WEEKDAY );
time_t this_year(0);
this_year.year = compile_year % 100;
this_year.set_day();
uint8_t compile_weekday = this_year.day;
epoch_year ( compile_year );
epoch_weekday( compile_weekday );
pivot_year ( this_year.year );
}
#endif

View File

@@ -0,0 +1,307 @@
#ifndef TIME_H
#define TIME_H
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include <Arduino.h>
#include "NeoGPS_cfg.h"
#include "CosaCompat.h"
namespace NeoGPS {
//------------------------------------------------------
// Enable/disable run-time modification of the epoch.
// If this is defined, epoch mutators are available.
// If this is not defined, the epoch is a hard-coded constant.
// Only epoch accessors are available.
//#define TIME_EPOCH_MODIFIABLE
/**
* Number of seconds elapsed since January 1 of the Epoch Year,
* 00:00:00 +0000 (UTC).
*/
typedef uint32_t clock_t;
const uint8_t SECONDS_PER_MINUTE = 60;
const uint8_t MINUTES_PER_HOUR = 60;
const uint16_t SECONDS_PER_HOUR = (uint16_t) SECONDS_PER_MINUTE * MINUTES_PER_HOUR;
const uint8_t HOURS_PER_DAY = 24;
const uint32_t SECONDS_PER_DAY = (uint32_t) SECONDS_PER_HOUR * HOURS_PER_DAY;
const uint8_t DAYS_PER_WEEK = 7;
/**
* Common date/time structure
*/
struct time_t {
enum weekday_t {
SUNDAY = 1,
MONDAY = 2,
TUESDAY = 3,
WEDNESDAY = 4,
THURSDAY = 5,
FRIDAY = 6,
SATURDAY = 7
};
// NTP epoch year and weekday (Monday)
static const uint16_t NTP_EPOCH_YEAR = 1900;
static const uint8_t NTP_EPOCH_WEEKDAY = MONDAY;
// POSIX epoch year and weekday (Thursday)
static const uint16_t POSIX_EPOCH_YEAR = 1970;
static const uint8_t POSIX_EPOCH_WEEKDAY = THURSDAY;
// Y2K epoch year and weekday (Saturday)
static const uint16_t Y2K_EPOCH_YEAR = 2000;
static const uint8_t Y2K_EPOCH_WEEKDAY = SATURDAY;
uint8_t seconds; //!< 00-59
uint8_t minutes; //!< 00-59
uint8_t hours; //!< 00-23
uint8_t day; //!< 01-07 Day of Week
uint8_t date; //!< 01-31 Day of Month
uint8_t month; //!< 01-12
uint8_t year; //!< 00-99
/**
* Constructor.
*/
time_t() {}
/**
* Construct from seconds since the Epoch.
* @param[in] c clock.
*/
time_t(clock_t c);
/**
* Initialize to January 1 of the Epoch Year, 00:00:00
*/
void init();
/**
* Convert to seconds.
* @return seconds from epoch.
*/
operator clock_t() const;
/**
* Offset by a number of seconds.
* @param[in] offset in seconds.
*/
void operator +=( clock_t offset )
{ *this = offset + operator clock_t(); }
/**
* Set day member from current value. This is a relatively expensive
* operation, so the weekday is only calculated when requested.
*/
void set_day()
{
day = weekday_for(days());
}
/**
* Convert to days.
* @return days from January 1 of the epoch year.
*/
uint16_t days() const;
/**
* Calculate day of the current year.
* @return days from January 1, which is day zero.
*/
uint16_t day_of_year() const;
/**
* Calculate 4-digit year from internal 2-digit year member.
* @return 4-digit year.
*/
uint16_t full_year() const
{
return full_year(year);
}
/**
* Calculate 4-digit year from a 2-digit year
* @param[in] year (4-digit).
* @return true if /year/ is a leap year.
*/
static uint16_t full_year( uint8_t year )
{
uint16_t y = year;
if (y < pivot_year())
y += 100 * (epoch_year()/100 + 1);
else
y += 100 * (epoch_year()/100);
return y;
}
/**
* Determine whether the current year is a leap year.
* @returns true if the two-digit /year/ member is a leap year.
*/
bool is_leap() const
{
return is_leap(full_year());
}
/**
* Determine whether the 4-digit /year/ is a leap year.
* @param[in] year (4-digit).
* @return true if /year/ is a leap year.
*/
static bool is_leap(uint16_t year)
{
if (year % 4) return false;
uint16_t y = year % 400;
return (y == 0) || ((y != 100) && (y != 200) && (y != 300));
}
/**
* Calculate how many days are in the specified year.
* @param[in] year (4-digit).
* @return number of days.
*/
static uint16_t days_per(uint16_t year)
{
return (365 + is_leap(year));
}
/**
* Determine the day of the week for the specified day number
* @param[in] dayno number as counted from January 1 of the epoch year.
* @return weekday number 1..7, as for the /day/ member.
*/
static uint8_t weekday_for(uint16_t dayno)
{
return ((dayno+epoch_weekday()-1) % DAYS_PER_WEEK) + 1;
}
/**
* Check that all members, EXCEPT FOR day, are set to a coherent date/time.
* @return true if valid date/time.
*/
bool is_valid() const
{
return
((year <= 99) &&
(1 <= month) && (month <= 12) &&
((1 <= date) &&
((date <= pgm_read_byte(&days_in[month])) ||
((month == 2) && is_leap() && (date == 29)))) &&
(hours <= 23) &&
(minutes <= 59) &&
(seconds <= 59));
}
/**
* Set the epoch year for all time_t operations. Note that the pivot
* year defaults to the epoch_year % 100. Valid years will be in the
* range epoch_year..epoch_year+99. Selecting a different pivot year
* will slide this range to the right.
* @param[in] y epoch year to set.
* See also /full_year/.
*/
#ifdef TIME_EPOCH_MODIFIABLE
static void epoch_year(uint16_t y)
{
s_epoch_year = y;
epoch_offset( s_epoch_year % 100 );
pivot_year( epoch_offset() );
}
#endif
/**
* Get the epoch year.
* @return year.
*/
static uint16_t epoch_year()
{
return (s_epoch_year);
}
static uint8_t epoch_weekday() { return s_epoch_weekday; };
#ifdef TIME_EPOCH_MODIFIABLE
static void epoch_weekday( uint8_t ew ) { s_epoch_weekday = ew; };
#endif
/**
* The pivot year determine the range of years WRT the epoch_year
* For example, an epoch year of 2000 and a pivot year of 80 will
* allow years in the range 1980 to 2079. Default 0 for Y2K_EPOCH.
*/
static uint8_t pivot_year() { return s_pivot_year; };
#ifdef TIME_EPOCH_MODIFIABLE
static void pivot_year( uint8_t py ) { s_pivot_year = py; };
#endif
#ifdef TIME_EPOCH_MODIFIABLE
/**
* Use the current year for the epoch year. This will result in the
* best performance of conversions, but dates/times before January 1
* of the epoch year cannot be represented.
*/
static void use_fastest_epoch();
#endif
/**
* Parse a character string and fill out members.
* @param[in] s PROGMEM character string with format "YYYY-MM-DD HH:MM:SS".
* @return success.
*/
bool parse(str_P s);
static const uint8_t days_in[] PROGMEM; // month index is 1..12, PROGMEM
protected:
static uint8_t epoch_offset() { return s_epoch_offset; };
#ifdef TIME_EPOCH_MODIFIABLE
static void epoch_offset( uint8_t eo ) { s_epoch_offset = eo; };
static uint16_t s_epoch_year;
static uint8_t s_pivot_year;
static uint8_t s_epoch_offset;
static uint8_t s_epoch_weekday;
#else
static const uint16_t s_epoch_year = Y2K_EPOCH_YEAR;
static const uint8_t s_pivot_year = s_epoch_year % 100;
static const uint8_t s_epoch_offset = s_pivot_year;
static const uint8_t s_epoch_weekday = Y2K_EPOCH_WEEKDAY;
#endif
} NEOGPS_PACKED;
}; // namespace NeoGPS
class Print;
/**
* Print the date/time to the given stream with the format "YYYY-MM-DD HH:MM:SS".
* @param[in] outs output stream.
* @param[in] t time structure.
* @return iostream.
*/
Print & operator <<( Print & outs, const NeoGPS::time_t &t );
#endif

View File

@@ -0,0 +1,421 @@
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "Streamers.h"
#include "NMEAGPS.h"
//#define USE_FLOAT
Print& operator <<( Print &outs, const bool b )
{ outs.print( b ? 't' : 'f' ); return outs; }
Print& operator <<( Print &outs, const char c ) { outs.print(c); return outs; }
Print& operator <<( Print &outs, const uint16_t v ) { outs.print(v); return outs; }
Print& operator <<( Print &outs, const uint32_t v ) { outs.print(v); return outs; }
Print& operator <<( Print &outs, const int32_t v ) { outs.print(v); return outs; }
Print& operator <<( Print &outs, const uint8_t v ) { outs.print(v); return outs; }
Print& operator <<( Print &outs, const __FlashStringHelper *s )
{ outs.print(s); return outs; }
//------------------------------------------
const char gps_fix_header[] __PROGMEM =
"Status,"
#if defined(GPS_FIX_DATE) | defined(GPS_FIX_TIME)
"UTC "
#if defined(GPS_FIX_DATE)
"Date"
#endif
#if defined(GPS_FIX_DATE) & defined(GPS_FIX_TIME)
"/"
#endif
#if defined(GPS_FIX_TIME)
"Time"
#endif
#else
"s"
#endif
","
#ifdef GPS_FIX_LOCATION
"Lat,Lon,"
#endif
#ifdef GPS_FIX_LOCATION_DMS
"DMS,"
#endif
#if defined(GPS_FIX_HEADING)
"Hdg,"
#endif
#if defined(GPS_FIX_SPEED)
"Spd,"
#endif
#if defined(GPS_FIX_ALTITUDE)
"Alt,"
#endif
#if defined(GPS_FIX_HDOP)
"HDOP,"
#endif
#if defined(GPS_FIX_VDOP)
"VDOP,"
#endif
#if defined(GPS_FIX_PDOP)
"PDOP,"
#endif
#if defined(GPS_FIX_LAT_ERR)
"Lat err,"
#endif
#if defined(GPS_FIX_LON_ERR)
"Lon err,"
#endif
#if defined(GPS_FIX_ALT_ERR)
"Alt err,"
#endif
#if defined(GPS_FIX_GEOID_HEIGHT)
"Geoid Ht,"
#endif
#if defined(GPS_FIX_SATELLITES)
"Sats,"
#endif
;
//...............
#ifdef GPS_FIX_LOCATION_DMS
static void printDMS( Print & outs, const DMS_t & dms )
{
if (dms.degrees < 10)
outs.write( '0' );
outs.print( dms.degrees );
outs.write( ' ' );
if (dms.minutes < 10)
outs.write( '0' );
outs.print( dms.minutes );
outs.print( F("\' ") );
if (dms.seconds_whole < 10)
outs.write( '0' );
outs.print( dms.seconds_whole );
outs.write( '.' );
if (dms.seconds_frac < 100)
outs.write( '0' );
if (dms.seconds_frac < 10)
outs.write( '0' );
outs.print( dms.seconds_frac );
outs.print( F("\" ") );
} // printDMS
#endif
//...............
Print & operator <<( Print &outs, const gps_fix &fix )
{
if (fix.valid.status)
outs << (uint8_t) fix.status;
outs << ',';
#if defined(GPS_FIX_DATE) | defined(GPS_FIX_TIME)
bool someTime = false;
#if defined(GPS_FIX_DATE)
someTime |= fix.valid.date;
#endif
#if defined(GPS_FIX_TIME)
someTime |= fix.valid.time;
#endif
if (someTime) {
outs << fix.dateTime << '.';
uint16_t ms = fix.dateTime_ms();
if (ms < 100)
outs << '0';
if (ms < 10)
outs << '0';
outs << ms;
}
outs << ',';
#else
// Date/Time not enabled, just output the interval number
static uint32_t sequence = 0L;
outs << sequence++ << ',';
#endif
#ifdef USE_FLOAT
#ifdef GPS_FIX_LOCATION
if (fix.valid.location) {
outs.print( fix.latitude(), 6 );
outs << ',';
outs.print( fix.longitude(), 6 );
} else
outs << ',';
outs << ',';
#endif
#ifdef GPS_FIX_LOCATION_DMS
if (fix.valid.location) {
printDMS( outs, fix.latitudeDMS );
outs.print( fix.latitudeDMS.NS() );
outs.write( ' ' );
if (fix.longitudeDMS.degrees < 100)
outs.write( '0' );
printDMS( outs, fix.longitudeDMS );
outs.print( fix.longitudeDMS.EW() );
}
outs << ',';
#endif
#ifdef GPS_FIX_HEADING
if (fix.valid.heading)
outs.print( fix.heading(), 2 );
outs << ',';
#endif
#ifdef GPS_FIX_SPEED
if (fix.valid.speed)
outs.print( fix.speed(), 3 ); // knots
outs << ',';
#endif
#ifdef GPS_FIX_ALTITUDE
if (fix.valid.altitude)
outs.print( fix.altitude(), 2 );
outs << ',';
#endif
#ifdef GPS_FIX_HDOP
if (fix.valid.hdop)
outs.print( (fix.hdop * 0.001), 3 );
outs << ',';
#endif
#ifdef GPS_FIX_VDOP
if (fix.valid.vdop)
outs.print( (fix.vdop * 0.001), 3 );
outs << ',';
#endif
#ifdef GPS_FIX_PDOP
if (fix.valid.pdop)
outs.print( (fix.pdop * 0.001), 3 );
outs << ',';
#endif
#ifdef GPS_FIX_LAT_ERR
if (fix.valid.lat_err)
outs.print( fix.lat_err(), 2 );
outs << ',';
#endif
#ifdef GPS_FIX_LON_ERR
if (fix.valid.lon_err)
outs.print( fix.lon_err(), 2 );
outs << ',';
#endif
#ifdef GPS_FIX_ALT_ERR
if (fix.valid.alt_err)
outs.print( fix.alt_err(), 2 );
outs << ',';
#endif
#ifdef GPS_FIX_GEOID_HEIGHT
if (fix.valid.geoidHeight)
outs.print( fix.geoidHeight(), 2 );
outs << ',';
#endif
#else
// not USE_FLOAT ----------------------
#ifdef GPS_FIX_LOCATION
if (fix.valid.location)
outs << fix.latitudeL() << ',' << fix.longitudeL();
else
outs << ',';
outs << ',';
#endif
#ifdef GPS_FIX_LOCATION_DMS
if (fix.valid.location) {
printDMS( outs, fix.latitudeDMS );
outs.print( fix.latitudeDMS.NS() );
outs.write( ' ' );
if (fix.longitudeDMS.degrees < 100)
outs.write( '0' );
printDMS( outs, fix.longitudeDMS );
outs.print( fix.longitudeDMS.EW() );
}
outs << ',';
#endif
#ifdef GPS_FIX_HEADING
if (fix.valid.heading)
outs << fix.heading_cd();
outs << ',';
#endif
#ifdef GPS_FIX_SPEED
if (fix.valid.speed)
outs << fix.speed_mkn();
outs << ',';
#endif
#ifdef GPS_FIX_ALTITUDE
if (fix.valid.altitude)
outs << fix.altitude_cm();
outs << ',';
#endif
#ifdef GPS_FIX_HDOP
if (fix.valid.hdop)
outs << fix.hdop;
outs << ',';
#endif
#ifdef GPS_FIX_VDOP
if (fix.valid.vdop)
outs << fix.vdop;
outs << ',';
#endif
#ifdef GPS_FIX_PDOP
if (fix.valid.pdop)
outs << fix.pdop;
outs << ',';
#endif
#ifdef GPS_FIX_LAT_ERR
if (fix.valid.lat_err)
outs << fix.lat_err_cm;
outs << ',';
#endif
#ifdef GPS_FIX_LON_ERR
if (fix.valid.lon_err)
outs << fix.lon_err_cm;
outs << ',';
#endif
#ifdef GPS_FIX_ALT_ERR
if (fix.valid.alt_err)
outs << fix.alt_err_cm;
outs << ',';
#endif
#ifdef GPS_FIX_GEOID_HEIGHT
if (fix.valid.geoidHeight)
outs << fix.geoidHeight_cm();
outs << ',';
#endif
#endif
#ifdef GPS_FIX_SATELLITES
if (fix.valid.satellites)
outs << fix.satellites;
outs << ',';
#endif
return outs;
}
//-----------------------------
static const char NMEAGPS_header[] __PROGMEM =
#if defined(NMEAGPS_TIMESTAMP_FROM_INTERVAL) | defined(NMEAGPS_TIMESTAMP_FROM_PPS)
"micros(),"
#endif
#if defined(NMEAGPS_PARSE_SATELLITES)
"[sat"
#if defined(NMEAGPS_PARSE_SATELLITE_INFO)
" elev/az @ SNR"
#endif
"],"
#endif
#ifdef NMEAGPS_STATS
"Rx ok,Rx err,Rx chars,"
#endif
"";
void trace_header( Print & outs )
{
outs.print( (const __FlashStringHelper *) &gps_fix_header[0] );
outs.print( (const __FlashStringHelper *) &NMEAGPS_header[0] );
outs << '\n';
}
//--------------------------
void trace_all( Print & outs, const NMEAGPS &gps, const gps_fix &fix )
{
outs << fix;
#if defined(NMEAGPS_TIMESTAMP_FROM_INTERVAL) | defined(NMEAGPS_TIMESTAMP_FROM_PPS)
outs << gps.UTCsecondStart();
outs << ',';
#endif
#if defined(NMEAGPS_PARSE_SATELLITES)
outs << '[';
for (uint8_t i=0; i < gps.sat_count; i++) {
outs << gps.satellites[i].id;
#if defined(NMEAGPS_PARSE_SATELLITE_INFO)
outs << ' ' <<
gps.satellites[i].elevation << '/' << gps.satellites[i].azimuth;
outs << '@';
if (gps.satellites[i].tracked)
outs << gps.satellites[i].snr;
else
outs << '-';
#endif
outs << ',';
}
outs << F("],");
#endif
#ifdef NMEAGPS_STATS
outs << gps.statistics.ok << ','
<< gps.statistics.errors << ','
<< gps.statistics.chars << ',';
#endif
outs << '\n';
} // trace_all

View File

@@ -0,0 +1,50 @@
#ifndef STREAMERS_H
#define STREAMERS_H
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include <Arduino.h>
extern Print & operator <<( Print & outs, const bool b );
extern Print & operator <<( Print & outs, const char c );
extern Print & operator <<( Print & outs, const uint16_t v );
extern Print & operator <<( Print & outs, const uint32_t v );
extern Print & operator <<( Print & outs, const int32_t v );
extern Print & operator <<( Print & outs, const uint8_t v );
extern Print & operator <<( Print & outs, const __FlashStringHelper *s );
class gps_fix;
/**
* Print valid fix data to the given stream with the format
* "status,dateTime,lat,lon,heading,speed,altitude,satellites,
* hdop,vdop,pdop,lat_err,lon_err,alt_err"
* The "header" above contains the actual compile-time configuration.
* A comma-separated field will be empty if the data is NOT valid.
* @param[in] outs output stream.
* @param[in] fix gps_fix instance.
* @return iostream.
*/
extern Print & operator <<( Print &outs, const gps_fix &fix );
class NMEAGPS;
extern void trace_header( Print & outs );
extern void trace_all( Print & outs, const NMEAGPS &gps, const gps_fix &fix );
#endif

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,332 @@
#ifndef _UBXGPS_H_
#define _UBXGPS_H_
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "NMEAGPS_cfg.h"
// Disable the entire file if derived types are not allowed.
#ifdef NMEAGPS_DERIVED_TYPES
#include "ublox/ubxNMEA.h"
#include "ublox/ubxmsg.h"
#include "GPSTime.h"
#include "ublox/ubx_cfg.h"
#include <Stream.h>
#include <stddef.h>
// NOTE: millis() is used for ACK timing
class ubloxGPS : public ubloxNMEA
{
ubloxGPS & operator =( const ubloxGPS & );
ubloxGPS( const ubloxGPS & );
public:
// Constructor needs to know the device to handle the UBX binary protocol
ubloxGPS( Stream *device )
:
storage( (ublox::msg_t *) NULL ),
reply( (ublox::msg_t *) NULL ),
reply_expected( false ),
ack_expected( false ),
m_device( device )
{};
// ublox binary UBX message type.
enum ubx_msg_t {
UBX_MSG = PUBX_LAST_MSG+1
};
static const nmea_msg_t UBX_FIRST_MSG = (nmea_msg_t) UBX_MSG;
static const nmea_msg_t UBX_LAST_MSG = (nmea_msg_t) UBX_MSG;
//................................................................
// Process one character of ublox message. The internal state
// machine tracks what part of the sentence has been received. As the
// tracks what part of the sentence has been received so far. As the
// sentence is received, members of the /fix/ structure are updated.
// @return DECODE_COMPLETED when a sentence has been completely received.
decode_t decode( char c );
//................................................................
// Received message header. Payload is only stored if /storage/ is
// overridden for that message type. This is the UBX-specific
// version of "nmeaMessage".
ublox::msg_t & rx() { return m_rx_msg; }
//................................................................
bool enable_msg( ublox::msg_class_t msg_class, ublox::msg_id_t msg_id )
{
return send( ublox::cfg_msg_t( msg_class, msg_id, 1 ) );
}
bool disable_msg( ublox::msg_class_t msg_class, ublox::msg_id_t msg_id )
{
return send( ublox::cfg_msg_t( msg_class, msg_id, 0 ) );
}
//................................................................
// Send a message (non-blocking).
// Although multiple /send_request/s can be issued,
// all replies will be handled identically.
bool send_request( const ublox::msg_t & msg )
{
write( msg );
return true;
}
bool send_request_P( const ublox::msg_t & msg )
{
write_P( msg );
return true;
}
//................................................................
// Send a message and wait for a reply (blocking).
// No event will be generated for the reply.
// If /msg/ is a UBX_CFG, this will wait for a UBX_CFG_ACK/NAK
// and return true if ACKed.
// If /msg/ is a poll, this will wait for the reply.
// If /msg/ is neither, this will return true immediately.
// If /msg/ is both, this will wait for both the reply and the ACK/NAK.
// If /storage_for/ is implemented, those messages will continue
// to be saved while waiting for this reply.
bool send( const ublox::msg_t & msg, ublox::msg_t *reply_msg = (ublox::msg_t *) NULL );
bool send_P( const ublox::msg_t & msg, ublox::msg_t *reply_msg = (ublox::msg_t *) NULL );
using NMEAGPS::send_P;
//................................................................
// Ask for a specific message (non-blocking).
// The message will receive be received later.
// See also /send_request/.
bool poll_request( const ublox::msg_t & msg )
{
ublox::msg_t poll_msg( msg.msg_class, msg.msg_id, 0 );
return send_request( poll_msg );
}
bool poll_request_P( const ublox::msg_t & msg )
{
ublox::msg_t poll_msg( (ublox::msg_class_t) pgm_read_byte( &msg.msg_class ),
(ublox::msg_id_t) pgm_read_byte( &msg.msg_id ), 0 );
return send_request( poll_msg );
}
//................................................................
// Ask for a specific message (blocking).
// See also /send/.
bool poll( ublox::msg_t & msg )
{
ublox::msg_t poll_msg( msg.msg_class, msg.msg_id, 0 );
return send( poll_msg, &msg );
}
bool poll_P( const ublox::msg_t & msg, ublox::msg_t *reply_msg = (ublox::msg_t *) NULL )
{
ublox::msg_t poll_msg( (ublox::msg_class_t) pgm_read_byte( &msg.msg_class ),
(ublox::msg_id_t) pgm_read_byte( &msg.msg_id ), 0 );
return send( poll_msg, reply_msg );
}
//................................................................
// Return the Stream that was passed into the constructor.
Stream *Device() const { return (Stream *)m_device; };
protected:
/*
* Some UBX messages can be larger than 256 bytes, so
* hide the 8-bit NMEAGPS::chrCount with this 16-bit version.
*/
uint16_t chrCount;
bool parseField( char chr );
enum ubxState_t {
UBX_IDLE = NMEA_IDLE,
UBX_SYNC2 = NMEA_LAST_STATE+1,
UBX_HEAD,
UBX_RECEIVING_DATA,
UBX_CRC_A,
UBX_CRC_B
};
static const ubxState_t UBX_FIRST_STATE = UBX_SYNC2;
static const ubxState_t UBX_LAST_STATE = UBX_CRC_B;
inline void write
( uint8_t c, uint8_t & crc_a, uint8_t & crc_b ) const
{
m_device->print( (char) c );
crc_a += c;
crc_b += crc_a;
};
void write( const ublox::msg_t & msg );
void write_P( const ublox::msg_t & msg );
//................................................................
// When the processing style is polling (not interrupt), this
// should be called frequently by any internal methods that block
// to make sure received chars continue to be processed.
virtual void run()
{
if (processing_style == PS_POLLING)
while (Device()->available())
handle( Device()->read() );
// else
// handled by interrupts
}
void wait_for_idle();
bool wait_for_ack();
// NOTE: /run/ is called from these blocking functions
bool waiting() const
{
return (ack_expected && (!ack_received && !nak_received)) ||
(reply_expected && !reply_received);
}
bool receiving() const
{
return (rxState != (rxState_t)UBX_IDLE) || (m_device && m_device->available());
}
// Override this if the contents of a particular message need to be saved.
// This may execute in an interrupt context, so be quick!
// NOTE: the ublox::msg_t.length will get stepped on, so you may need to
// set it every time if you are using a union for your storage.
virtual ublox::msg_t *storage_for( const ublox::msg_t & rx_msg )
{ return (ublox::msg_t *) NULL; }
virtual bool intervalCompleted() const
{
return ((nmeaMessage == (nmea_msg_t) UBX_MSG) &&
(m_rx_msg.msg_class == UBX_LAST_MSG_CLASS_IN_INTERVAL) &&
(m_rx_msg.msg_id == UBX_LAST_MSG_ID_IN_INTERVAL))
||
NMEAGPS::intervalCompleted();
}
private:
ublox::msg_t *storage; // cached ptr to hold a received msg.
// Storage for a specific received message.
// Used internally by send & poll variants.
// Checked and used before /storage_for/ is called.
ublox::msg_t *reply;
struct {
bool reply_expected NEOGPS_BF(1);
bool reply_received NEOGPS_BF(1);
bool ack_expected NEOGPS_BF(1);
bool ack_received NEOGPS_BF(1);
bool nak_received NEOGPS_BF(1);
bool ack_same_as_sent NEOGPS_BF(1);
} NEOGPS_PACKED;
struct ublox::msg_hdr_t sent;
struct rx_msg_t : ublox::msg_t
{
uint8_t crc_a; // accumulated as packet received
uint8_t crc_b; // accumulated as packet received
rx_msg_t()
{
init();
}
void init()
{
msg_class = ublox::UBX_UNK;
msg_id = ublox::UBX_ID_UNK;
length = 0;
crc_a = 0;
crc_b = 0;
}
} NEOGPS_PACKED;
rx_msg_t m_rx_msg;
void rxBegin();
bool rxEnd();
static const uint8_t SYNC_1 = 0xB5;
static const uint8_t SYNC_2 = 0x62;
Stream *m_device;
bool parseNavStatus ( uint8_t chr );
bool parseNavDOP ( uint8_t chr );
bool parseNavPosLLH ( uint8_t chr );
bool parseNavVelNED ( uint8_t chr );
bool parseNavTimeGPS( uint8_t chr );
bool parseNavTimeUTC( uint8_t chr );
bool parseNavSVInfo ( uint8_t chr );
bool parseHnrPvt( uint8_t chr );
bool parseFix( uint8_t c );
bool parseTOW( uint8_t chr )
{
#if defined(GPS_FIX_TIME) & defined(GPS_FIX_DATE)
if (chrCount == 0) {
m_fix.valid.date =
m_fix.valid.time = false;
}
((uint8_t *) &m_fix.dateTime)[ chrCount ] = chr;
if (chrCount == 3) {
uint32_t tow = *((uint32_t *) &m_fix.dateTime);
//trace << PSTR("@ ") << tow;
uint16_t ms;
if (GPSTime::from_TOWms( tow, m_fix.dateTime, ms )) {
m_fix.dateTime_cs = ms / 10;
m_fix.valid.time = true;
m_fix.valid.date = true;
} else
m_fix.dateTime.init();
//trace << PSTR(".") << m_fix.dateTime_cs;
}
#endif
return true;
}
} NEOGPS_PACKED;
#endif // NMEAGPS_DERIVED_TYPES enabled
#endif

View File

@@ -0,0 +1,147 @@
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "ublox/ubxNMEA.h"
// Disable the entire file if derived types are not allowed.
#ifdef NMEAGPS_DERIVED_TYPES
//---------------------------------------------
bool ubloxNMEA::parseField(char chr)
{
if (nmeaMessage >= (nmea_msg_t) PUBX_FIRST_MSG) {
switch (nmeaMessage) {
case PUBX_00: return parsePUBX_00( chr );
#if defined(NMEAGPS_PARSE_PUBX_04) | defined(NMEAGPS_RECOGNIZE_ALL)
case PUBX_04: return parsePUBX_04( chr );
#endif
default:
break;
}
} else
// Delegate
return NMEAGPS::parseField(chr);
return true;
} // parseField
//----------------------------
bool ubloxNMEA::parsePUBX_00( char chr )
{
bool ok = true;
switch (fieldIndex) {
case 1:
// The first field is actually a message subtype
if (chrCount == 0)
ok = (chr == '0');
else if (chrCount == 1) {
switch (chr) {
#if defined(NMEAGPS_PARSE_PUBX_00) | defined(NMEAGPS_RECOGNIZE_ALL)
case '0': break;
#endif
#if defined(NMEAGPS_PARSE_PUBX_04) | defined(NMEAGPS_RECOGNIZE_ALL)
case '4': nmeaMessage = (nmea_msg_t) PUBX_04; break;
#endif
default : ok = false;
}
} else // chrCount > 1
ok = (chr == ',');
break;
#ifdef NMEAGPS_PARSE_PUBX_00
case 2: return parseTime( chr );
PARSE_LOC(3);
case 7: return parseAlt( chr );
case 8: return parseFix( chr );
case 11: return parseSpeed( chr ); // kph!
case 12: return parseHeading( chr );
case 15: return parseHDOP( chr );
case 16: return parseVDOP( chr );
case 18: return parseSatellites( chr );
#endif
}
return ok;
} // parsePUBX_00
//---------------------------------------------
bool ubloxNMEA::parsePUBX_04( char chr )
{
#ifdef NMEAGPS_PARSE_PUBX_04
switch (fieldIndex) {
case 2: return parseTime( chr );
case 3: return parseDDMMYY( chr );
}
#endif
return true;
} // parsePUBX_04
//---------------------------------------------
bool ubloxNMEA::parseFix( char chr )
{
if (chrCount == 0) {
NMEAGPS_INVALIDATE( status );
if (chr == 'N')
m_fix.status = gps_fix::STATUS_NONE;
else if (chr == 'T')
m_fix.status = gps_fix::STATUS_TIME_ONLY;
else if (chr == 'R')
m_fix.status = gps_fix::STATUS_EST;
else if (chr == 'G')
m_fix.status = gps_fix::STATUS_STD;
else if (chr == 'D')
m_fix.status = gps_fix::STATUS_DGPS;
} else if (chrCount == 1) {
if (((chr == 'T') && (m_fix.status == gps_fix::STATUS_TIME_ONLY)) ||
((chr == 'K') && (m_fix.status == gps_fix::STATUS_EST)) ||
(((chr == '2') || (chr == '3')) &&
((m_fix.status == gps_fix::STATUS_STD) ||
(m_fix.status == gps_fix::STATUS_DGPS))) ||
((chr == 'F') && (m_fix.status == gps_fix::STATUS_NONE)))
// status based on first char was ok guess
m_fix.valid.status = true;
else if ((chr == 'R') && (m_fix.status == gps_fix::STATUS_DGPS)) {
m_fix.status = gps_fix::STATUS_EST; // oops, was DR instead
m_fix.valid.status = true;
}
}
return true;
}
#endif

View File

@@ -0,0 +1,110 @@
#ifndef _UBXNMEA_H_
#define _UBXNMEA_H_
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "NMEAGPS_cfg.h"
// Disable the entire file if derived types are not allowed.
#ifdef NMEAGPS_DERIVED_TYPES
#include "NMEAGPS.h"
//------------------------------------------------------------
// Enable/disable the parsing of specific proprietary NMEA sentences.
//
// Configuring out a sentence prevents its fields from being parsed.
// However, the sentence type may still be recognized by /decode/ and
// stored in member /nmeaMessage/. No valid flags would be available.
//#define NMEAGPS_PARSE_PUBX_00
//#define NMEAGPS_PARSE_PUBX_04
// Ublox proprietary messages do not have a message type. These
// messages start with "$PUBX," which ends with the manufacturer ID. The
// message type is actually specified by the first numeric field. In order
// to parse these messages, /parse_mfr_ID/ must be overridden to set the
// /nmeaMessage/ to PUBX_00 during /parseCommand/. When the first numeric
// field is completed by /parseField/, it may change /nmeamessage/ to one
// of the other PUBX message types.
#if (defined(NMEAGPS_PARSE_PUBX_00) | defined(NMEAGPS_PARSE_PUBX_04))
#if !defined(NMEAGPS_PARSE_PROPRIETARY)
#error NMEAGPS_PARSE_PROPRIETARY must be defined in NMEAGPS_cfg.h in order to parse PUBX messages!
#endif
#if !defined(NMEAGPS_PARSE_MFR_ID)
#error NMEAGPS_PARSE_MFR_ID must be defined in NMEAGPS_cfg.h in order to parse PUBX messages!
#endif
#endif
//=============================================================
// NMEA 0183 Parser for ublox Neo-6 GPS Modules.
//
// @section Limitations
// Very limited support for ublox proprietary NMEA messages.
// Only NMEA messages of types PUBX,00 and PUBX,04 are parsed.
//
class ubloxNMEA : public NMEAGPS
{
ubloxNMEA( const ubloxNMEA & );
public:
ubloxNMEA() {};
/** ublox proprietary NMEA message types. */
enum pubx_msg_t {
PUBX_00 = NMEA_LAST_MSG+1,
#if defined(NMEAGPS_PARSE_PUBX_04) | defined(NMEAGPS_RECOGNIZE_ALL)
PUBX_04,
#endif
PUBX_END
};
static const nmea_msg_t PUBX_FIRST_MSG = (nmea_msg_t) PUBX_00;
static const nmea_msg_t PUBX_LAST_MSG = (nmea_msg_t) (PUBX_END-1);
protected:
bool parseMfrID( char chr )
{ bool ok;
switch (chrCount) {
case 1: ok = (chr == 'U'); break;
case 2: ok = (chr == 'B'); break;
default: if (chr == 'X') {
ok = true;
nmeaMessage = (nmea_msg_t) PUBX_00;
} else
ok = false;
break;
}
return ok;
};
bool parsePUBX_00( char chr );
bool parsePUBX_04( char chr );
bool parseField( char chr );
bool parseFix( char chr );
} NEOGPS_PACKED;
#endif // NMEAGPS_DERIVED_TYPES enabled
#endif

View File

@@ -0,0 +1,64 @@
#ifndef UBX_CFG_H
#define UBX_CFG_H
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
//--------------------------------------------------------------------
// Enable/disable the parsing of specific UBX messages.
//
// Configuring out a message prevents its fields from being parsed.
// However, the message type will still be recognized by /decode/ and
// stored in member /rx_msg/. No valid flags would be available.
#define UBLOX_PARSE_STATUS
#define UBLOX_PARSE_TIMEGPS
#define UBLOX_PARSE_TIMEUTC
#define UBLOX_PARSE_POSLLH
//#define UBLOX_PARSE_DOP
#define UBLOX_PARSE_VELNED
//#define UBLOX_PARSE_SVINFO
//#define UBLOX_PARSE_CFGNAV5
//#define UBLOX_PARSE_MONVER
//#define UBLOX_PARSE_HNR_PVT
//--------------------------------------------------------------------
// Identify the last UBX message in an update interval.
// (There are two parts to a UBX message, the class and the ID.)
// For coherency, you must determine which UBX message is last!
// This section *should* pick the correct last UBX message for NEO-6M devices.
#if defined(UBLOX_PARSE_HNR_PVT)
#define UBX_LAST_MSG_CLASS_IN_INTERVAL ublox::UBX_HNR
#define UBX_LAST_MSG_ID_IN_INTERVAL ublox::UBX_HNR_PVT
#else
#define UBX_LAST_MSG_CLASS_IN_INTERVAL ublox::UBX_NAV
#if defined(UBLOX_PARSE_DOP)
#define UBX_LAST_MSG_ID_IN_INTERVAL ublox::UBX_NAV_DOP
#elif defined(UBLOX_PARSE_VELNED)
#define UBX_LAST_MSG_ID_IN_INTERVAL ublox::UBX_NAV_VELNED
#elif defined(UBLOX_PARSE_POSLLH)
#define UBX_LAST_MSG_ID_IN_INTERVAL ublox::UBX_NAV_POSLLH
#elif defined(UBLOX_PARSE_SVINFO)
#define UBX_LAST_MSG_ID_IN_INTERVAL ublox::UBX_NAV_SVINFO
#else
#define UBX_LAST_MSG_ID_IN_INTERVAL ublox::UBX_NAV_STATUS
#endif
#endif
#endif

View File

@@ -0,0 +1,71 @@
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "ublox/ubxGPS.h"
// Disable the entire file if derived types are not allowed.
#ifdef NMEAGPS_DERIVED_TYPES
using namespace ublox;
bool ublox::configNMEA( ubloxGPS &gps, NMEAGPS::nmea_msg_t msgType, uint8_t rate )
{
static const ubx_nmea_msg_t ubx[] __PROGMEM = {
#if defined(NMEAGPS_PARSE_GGA) | defined(NMEAGPS_RECOGNIZE_ALL)
UBX_GPGGA,
#endif
#if defined(NMEAGPS_PARSE_GLL) | defined(NMEAGPS_RECOGNIZE_ALL)
UBX_GPGLL,
#endif
#if defined(NMEAGPS_PARSE_GSA) | defined(NMEAGPS_RECOGNIZE_ALL)
UBX_GPGSA,
#endif
#if defined(NMEAGPS_PARSE_GST) | defined(NMEAGPS_RECOGNIZE_ALL)
UBX_GPGST,
#endif
#if defined(NMEAGPS_PARSE_GSV) | defined(NMEAGPS_RECOGNIZE_ALL)
UBX_GPGSV,
#endif
#if defined(NMEAGPS_PARSE_RMC) | defined(NMEAGPS_RECOGNIZE_ALL)
UBX_GPRMC,
#endif
#if defined(NMEAGPS_PARSE_VTG) | defined(NMEAGPS_RECOGNIZE_ALL)
UBX_GPVTG,
#endif
#if defined(NMEAGPS_PARSE_ZDA) | defined(NMEAGPS_RECOGNIZE_ALL)
UBX_GPZDA,
#endif
};
uint8_t msg_index = (uint8_t) msgType - (uint8_t) NMEAGPS::NMEA_FIRST_MSG;
if (msg_index >= sizeof(ubx)/sizeof(ubx[0]))
return false;
msg_id_t msg_id = (msg_id_t) pgm_read_byte( &ubx[msg_index] );
return gps.send( cfg_msg_t( UBX_NMEA, msg_id, rate ) );
}
#endif

View File

@@ -0,0 +1,589 @@
#ifndef UBXMSG_H
#define UBXMSG_H
// Copyright (C) 2014-2017, SlashDevin
//
// This file is part of NeoGPS
//
// NeoGPS 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.
//
// NeoGPS 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 NeoGPS. If not, see <http://www.gnu.org/licenses/>.
#include "NMEAGPS_cfg.h"
// Disable the entire file if derived types are not allowed.
#ifdef NMEAGPS_DERIVED_TYPES
#include "NMEAGPS.h"
class ubloxGPS;
namespace ublox {
enum msg_class_t
{ UBX_NAV = 0x01, // Navigation results
UBX_RXM = 0x02, // Receiver Manager messages
UBX_INF = 0x04, // Informational messages
UBX_ACK = 0x05, // ACK/NAK replies to CFG messages
UBX_CFG = 0x06, // Configuration input messages
UBX_MON = 0x0A, // Monitoring messages
UBX_AID = 0x0B, // Assist Now aiding messages
UBX_TIM = 0x0D, // Timing messages
UBX_HNR = 0x28, // High rate navigation results
UBX_NMEA = 0xF0, // NMEA Standard messages
UBX_PUBX = 0xF1, // NMEA proprietary messages (PUBX)
UBX_UNK = 0xFF
} __attribute__((packed));
enum msg_id_t
{
UBX_ACK_NAK = 0x00, // Reply to CFG messages
UBX_ACK_ACK = 0x01, // Reply to CFG messages
UBX_CFG_MSG = 0x01, // Configure which messages to send
UBX_CFG_RST = 0x04, // Reset command
UBX_CFG_RATE = 0x08, // Configure message rate
UBX_CFG_NMEA = 0x17, // Configure NMEA protocol
UBX_CFG_NAV5 = 0x24, // Configure navigation engine settings
UBX_MON_VER = 0x04, // Monitor Receiver/Software version
UBX_NAV_POSLLH = 0x02, // Current Position
UBX_NAV_STATUS = 0x03, // Receiver Navigation Status
UBX_NAV_DOP = 0x04, // Dilutions of Precision
UBX_NAV_ODO = 0x09, // Odometer Solution (NEO-M8 only)
UBX_NAV_RESETODO = 0x10, // Reset Odometer (NEO-M8 only)
UBX_NAV_VELNED = 0x12, // Current Velocity
UBX_NAV_TIMEGPS = 0x20, // Current GPS Time
UBX_NAV_TIMEUTC = 0x21, // Current UTC Time
UBX_NAV_SVINFO = 0x30, // Space Vehicle Information
UBX_HNR_PVT = 0x00, // High rate Position, Velocity and Time
UBX_ID_UNK = 0xFF
} __attribute__((packed));
struct msg_hdr_t {
msg_class_t msg_class;
msg_id_t msg_id;
bool same_kind( const msg_hdr_t & msg ) const
{ return (msg_class == msg.msg_class) && (msg_id == msg.msg_id); }
} __attribute__((packed));
struct msg_t : msg_hdr_t {
uint16_t length; // should be sizeof(this)-sizeof(msg_hdr_t)
#define UBX_MSG_LEN(msg) (sizeof(msg) - sizeof(ublox::msg_t))
msg_t()
{
length = 0;
};
msg_t( enum msg_class_t m, enum msg_id_t i, uint16_t l = 0 )
{
msg_class = m;
msg_id = i;
length = l;
}
void init()
{
uint8_t *mem = (uint8_t *) this;
memset( &mem[ sizeof(msg_t) ], 0, length );
}
} __attribute__((packed));
/**
* Configure message intervals.
*/
enum ubx_nmea_msg_t { // msg_id's for UBX_NMEA msg_class
UBX_GPGGA = 0x00,
UBX_GPGLL = 0x01,
UBX_GPGSA = 0x02,
UBX_GPGSV = 0x03,
UBX_GPRMC = 0x04,
UBX_GPVTG = 0x05,
UBX_GPGST = 0x07,
UBX_GPZDA = 0x08
} __attribute__((packed));
struct cfg_msg_t : msg_t {
msg_class_t cfg_msg_class;
msg_id_t cfg_msg;
uint8_t rate;
cfg_msg_t( msg_class_t m, msg_id_t i, uint8_t r )
: msg_t( UBX_CFG, UBX_CFG_MSG, UBX_MSG_LEN(*this) )
{
cfg_msg_class = m;
cfg_msg = i;
rate = r;
};
} __attribute__((packed));
extern bool configNMEA( ubloxGPS &gps, NMEAGPS::nmea_msg_t msgType, uint8_t rate );
// Reset command
struct cfg_reset_t : msg_t {
struct bbr_section_t
{
bool ephermeris :1;
bool almanac :1;
bool health :1;
bool klobuchard :1;
bool position :1;
bool clock_drift :1;
bool osc_param :1;
bool utc_param :1;
bool rtc :1;
bool reserved1 :2;
bool sfdr_param :1;
bool sfdr_veh_mon_param :1;
bool tct_param :1;
bool reserved2 :1;
bool autonomous_orbit_param:1;
} __attribute__((packed));
enum reset_mode_t
{
HW_RESET = 0x00,
CONTROLLED_SW_RESET = 0x01,
CONTROLLED_SW_RESET_GPS_ONLY = 0x02,
HW_RESET_AFTER_SHUTDOWN = 0x04,
CONTROLLED_GPS_STOP = 0x08,
CONTROLLED_GPS_START = 0x09
} __attribute__((packed));
bbr_section_t clear_bbr_section;
reset_mode_t reset_mode : 8;
uint8_t reserved : 8;
cfg_reset_t()
: msg_t( UBX_CFG, UBX_CFG_RST, UBX_MSG_LEN(*this) )
{ init(); }
} __attribute__((packed));
// Configure navigation rate
enum time_ref_t {
UBX_TIME_REF_UTC=0,
UBX_TIME_REF_GPS=1
} __attribute__((packed));
struct cfg_rate_t : msg_t {
uint16_t GPS_meas_rate;
uint16_t nav_rate;
enum time_ref_t time_ref:16;
cfg_rate_t( uint16_t gr, uint16_t nr, enum time_ref_t tr )
: msg_t( UBX_CFG, UBX_CFG_RATE, UBX_MSG_LEN(*this) )
{
GPS_meas_rate = gr;
nav_rate = nr;
time_ref = tr;
}
} __attribute__((packed));
// Navigation Engine Expert Settings
enum dyn_model_t {
UBX_DYN_MODEL_PORTABLE = 0,
UBX_DYN_MODEL_STATIONARY = 2,
UBX_DYN_MODEL_PEDESTRIAN = 3,
UBX_DYN_MODEL_AUTOMOTIVE = 4,
UBX_DYN_MODEL_SEA = 5,
UBX_DYN_MODEL_AIR_1G = 6,
UBX_DYN_MODEL_AIR_2G = 7,
UBX_DYN_MODEL_AIR_4G = 8
} __attribute__((packed));
enum position_fix_t {
UBX_POS_FIX_2D_ONLY = 1,
UBX_POS_FIX_3D_ONLY = 2,
UBX_POS_FIX_AUTO = 3
} __attribute__((packed));
struct cfg_nav5_t : msg_t {
struct parameter_mask_t {
bool dyn_model :1;
bool min_elev :1;
bool fix :1;
bool dr_limit :1;
bool pos_mask :1;
bool time_mask :1;
bool static_hold_thr :1;
bool dgps_timeout :1;
int _unused_ :8;
} __attribute__((packed));
union {
struct parameter_mask_t apply;
uint16_t apply_word;
} __attribute__((packed));
enum dyn_model_t dyn_model:8;
enum position_fix_t fix_mode:8;
int32_t fixed_alt; // m MSL x0.01
uint32_t fixed_alt_variance; // m^2 x0.0001
int8_t min_elev; // deg
uint8_t dr_limit; // s
uint16_t pos_dop_mask; // x0.1
uint16_t time_dop_mask; // x0.1
uint16_t pos_acc_mask; // m
uint16_t time_acc_mask; // m
uint8_t static_hold_thr; // cm/s
uint8_t dgps_timeout; // s
uint32_t always_zero_1;
uint32_t always_zero_2;
uint32_t always_zero_3;
cfg_nav5_t() : msg_t( UBX_CFG, UBX_CFG_NAV5, UBX_MSG_LEN(*this) )
{
apply_word = 0xFF00;
always_zero_1 =
always_zero_2 =
always_zero_3 = 0;
}
} __attribute__((packed));
// Geodetic Position Solution
struct nav_posllh_t : msg_t {
uint32_t time_of_week; // mS
int32_t lon; // deg * 1e7
int32_t lat; // deg * 1e7
int32_t height_above_ellipsoid; // mm
int32_t height_MSL; // mm
uint32_t horiz_acc; // mm
uint32_t vert_acc; // mm
nav_posllh_t() : msg_t( UBX_NAV, UBX_NAV_POSLLH, UBX_MSG_LEN(*this) ) {};
} __attribute__((packed));
// Receiver Navigation Status
struct nav_status_t : msg_t {
uint32_t time_of_week; // mS
enum status_t {
NAV_STAT_NONE,
NAV_STAT_DR_ONLY,
NAV_STAT_2D,
NAV_STAT_3D,
NAV_STAT_GPS_DR,
NAV_STAT_TIME_ONLY
} __attribute__((packed))
status;
struct flags_t {
bool gps_fix:1;
bool diff_soln:1;
bool week:1;
bool time_of_week:1;
} __attribute__((packed))
flags;
static gps_fix::status_t to_status( enum gps_fix::status_t status, flags_t flags )
{
if (!flags.gps_fix)
return gps_fix::STATUS_NONE;
if (flags.diff_soln)
return gps_fix::STATUS_DGPS;
return status;
}
struct {
bool dgps_input:1;
bool _skip_:6;
bool map_matching:1;
} __attribute__((packed))
fix_status;
enum {
PSM_ACQUISITION,
PSM_TRACKING,
PSM_POWER_OPTIMIZED_TRACKING,
PSM_INACTIVE
}
power_safe:2; // FW > v7.01
uint32_t time_to_first_fix; // ms time tag
uint32_t uptime; // ms since startup/reset
nav_status_t() : msg_t( UBX_NAV, UBX_NAV_STATUS, UBX_MSG_LEN(*this) ) {};
} __attribute__((packed));
// Dilutions of Precision
struct nav_dop_t : msg_t {
uint32_t time_of_week; // mS
uint16_t gdop; // Geometric
uint16_t pdop; // Position
uint16_t tdop; // Time
uint16_t vdop; // Vertical
uint16_t hdop; // Horizontal
uint16_t ndop; // Northing
uint16_t edop; // Easting
nav_dop_t() : msg_t( UBX_NAV, UBX_NAV_DOP, UBX_MSG_LEN(*this) ) {};
} __attribute__((packed));
// Odometer Solution (NEO-M8 only)
struct nav_odo_t : msg_t {
uint8_t version;
uint8_t reserved[3];
uint32_t time_of_week; // mS
uint32_t distance; // m
uint32_t total_distance; // m
uint32_t distanceSTD; // m (1-sigma)
nav_odo_t() : msg_t( UBX_NAV, UBX_NAV_ODO, UBX_MSG_LEN(*this) ) {};
} __attribute__((packed));
// Reset Odometer (NEO-M8 only)
struct nav_resetodo_t : msg_t {
// no payload, it's just a command
nav_resetodo_t() : msg_t( UBX_NAV, UBX_NAV_RESETODO, UBX_MSG_LEN(*this) ) {};
} __attribute__((packed));
// Velocity Solution in North/East/Down
struct nav_velned_t : msg_t {
uint32_t time_of_week; // mS
int32_t vel_north; // cm/s
int32_t vel_east; // cm/s
int32_t vel_down; // cm/s
uint32_t speed_3D; // cm/s
uint32_t speed_2D; // cm/s
int32_t heading; // degrees * 1e5
uint32_t speed_acc; // cm/s
uint32_t heading_acc; // degrees * 1e5
nav_velned_t() : msg_t( UBX_NAV, UBX_NAV_VELNED, UBX_MSG_LEN(*this) ) {};
} __attribute__((packed));
// GPS Time Solution
struct nav_timegps_t : msg_t {
uint32_t time_of_week; // mS
int32_t fractional_ToW; // nS
int16_t week;
int8_t leap_seconds; // GPS-UTC
struct valid_t {
bool time_of_week:1;
bool week:1;
bool leap_seconds:1;
} __attribute__((packed))
valid;
nav_timegps_t() : msg_t( UBX_NAV, UBX_NAV_TIMEGPS, UBX_MSG_LEN(*this) ) {};
} __attribute__((packed));
// UTC Time Solution
struct nav_timeutc_t : msg_t {
uint32_t time_of_week; // mS
uint32_t time_accuracy; // nS
int32_t fractional_ToW; // nS
uint16_t year; // 1999..2099
uint8_t month; // 1..12
uint8_t day; // 1..31
uint8_t hour; // 0..23
uint8_t minute; // 0..59
uint8_t second; // 0..59
struct valid_t {
bool time_of_week:1;
bool week_number:1;
bool UTC:1;
} __attribute__((packed))
valid;
nav_timeutc_t() : msg_t( UBX_NAV, UBX_NAV_TIMEUTC, UBX_MSG_LEN(*this) ) {};
} __attribute__((packed));
// Space Vehicle Information
struct nav_svinfo_t : msg_t {
uint32_t time_of_week; // mS
uint8_t num_channels;
enum { ANTARIS_OR_4, UBLOX_5, UBLOX_6 } chipgen:8;
uint16_t reserved2;
struct sv_t {
uint8_t channel; // 255 = no channel
uint8_t id; // Satellite ID
bool used_for_nav:1;
bool diff_corr :1; // differential correction available
bool orbit_avail :1; // orbit info available
bool orbit_eph :1; // orbit info is ephemeris
bool unhealthy :1; // SV should not be used
bool orbit_alm :1; // orbit info is Almanac Plus
bool orbit_AOP :1; // orbit info is AssistNow Autonomous
bool smoothed :1; // Carrier smoothed pseudorange used
enum {
IDLE,
SEARCHING,
ACQUIRED,
UNUSABLE,
CODE_LOCK,
CODE_AND_CARRIER_LOCK_1,
CODE_AND_CARRIER_LOCK_2,
CODE_AND_CARRIER_LOCK_3
}
quality:8;
uint8_t snr; // dbHz
uint8_t elevation; // degrees
uint16_t azimuth; // degrees
uint32_t pr_res; // pseudo range residual in cm
};
// Calculate the number of bytes required to hold the
// specified number of channels.
static uint16_t size_for( uint8_t channels )
{ return sizeof(nav_svinfo_t) + (uint16_t)channels * sizeof(sv_t); }
// Initialze the msg_hdr for the specified number of channels
void init( uint8_t max_channels )
{
msg_class = UBX_NAV;
msg_id = UBX_NAV_SVINFO;
length = size_for( max_channels ) - sizeof(ublox::msg_t);
}
} __attribute__((packed));
// High Rate PVT
struct hnr_pvt_t : msg_t {
uint32_t time_of_week; // mS
uint16_t year; // 1999..2099
uint8_t month; // 1..12
uint8_t day; // 1..31
uint8_t hour; // 0..23
uint8_t minute; // 0..59
uint8_t second; // 0..59
struct valid_t {
bool date:1;
bool time:1;
bool fully_resolved:1;
} __attribute__((packed))
valid;
int32_t fractional_ToW; // nS
nav_status_t::status_t status;
struct flags_t {
bool gps_fix:1;
bool diff_soln:1;
bool week:1;
bool time_of_week:1;
bool heading_valid:1;
} __attribute__((packed))
flags;
static gps_fix::status_t to_status( enum gps_fix::status_t status, flags_t flags )
{
if (!flags.gps_fix)
return gps_fix::STATUS_NONE;
if (flags.diff_soln)
return gps_fix::STATUS_DGPS;
return status;
}
uint16_t reserved1;
int32_t lon; // deg * 1e7
int32_t lat; // deg * 1e7
int32_t height_above_ellipsoid; // mm
int32_t height_MSL; // mm
int32_t speed_2D; // mm/s
int32_t speed_3D; // mm/s
int32_t heading_motion; // degrees * 1e5
int32_t heading_vehicle; // degrees * 1e5
uint32_t horiz_acc; // mm
uint32_t vert_acc; // mm
uint32_t speed_acc; // mm/s
uint32_t heading_acc; // degrees * 1e5
uint32_t reserved4;
hnr_pvt_t() : msg_t( UBX_HNR, UBX_HNR_PVT, UBX_MSG_LEN(*this) ) {};
} __attribute__((packed));
struct cfg_nmea_t : msg_t {
bool always_output_pos :1; // invalid or failed
bool output_invalid_pos :1;
bool output_invalid_time:1;
bool output_invalid_date:1;
bool use_GPS_only :1;
bool output_heading :1; // even if frozen
bool __not_used__ :2;
enum {
NMEA_V_2_1 = 0x21,
NMEA_V_2_3 = 0x23,
NMEA_V_4_0 = 0x40, // Neo M8 family only
NMEA_V_4_1 = 0x41 // Neo M8 family only
}
nmea_version : 8;
enum {
SV_PER_TALKERID_UNLIMITED = 0,
SV_PER_TALKERID_8 = 8,
SV_PER_TALKERID_12 = 12,
SV_PER_TALKERID_16 = 16
}
num_sats_per_talker_id : 8;
bool compatibility_mode:1;
bool considering_mode :1;
bool max_line_length_82:1; // Neo M8 family only
uint8_t __not_used_1__ :5;
cfg_nmea_t() : msg_t( UBX_CFG, UBX_CFG_NMEA, UBX_MSG_LEN(*this) ) {};
} __attribute__((packed));
struct cfg_nmea_v1_t : cfg_nmea_t {
bool filter_gps :1;
bool filter_sbas :1;
uint8_t __not_used_2__:2;
bool filter_qzss :1;
bool filter_glonass:1;
bool filter_beidou :1;
uint32_t __not_used_3__:25;
bool proprietary_sat_numbering; // for non-NMEA satellites
enum {
MAIN_TALKER_ID_COMPUTED,
MAIN_TALKER_ID_GP,
MAIN_TALKER_ID_GL,
MAIN_TALKER_ID_GN,
MAIN_TALKER_ID_GA,
MAIN_TALKER_ID_GB
}
main_talker_id : 8;
bool gsv_uses_main_talker_id; // false means COMPUTED
enum cfg_nmea_version_t {
CFG_NMEA_V_0, // length = 12
CFG_NMEA_V_1 // length = 20 (default)
}
version : 8;
// Remaining fields are CFG_NMEA_V_1 only!
char beidou_talker_id[2]; // NULs mean default
uint8_t __reserved__[6];
cfg_nmea_v1_t( cfg_nmea_version_t v = CFG_NMEA_V_1 )
{
version = v;
if (version == CFG_NMEA_V_0)
length = 12;
else {
length = 20;
for (uint8_t i=0; i<8;) // fills 'reserved' too
beidou_talker_id[i++] = 0;
}
};
} __attribute__((packed));
};
#endif // NMEAGPS_DERIVED_TYPES enabled
#endif