diff --git a/examples/linux/NMEA.cpp b/examples/linux/NMEA.cpp new file mode 100644 index 0000000..e8d7f44 --- /dev/null +++ b/examples/linux/NMEA.cpp @@ -0,0 +1,154 @@ +//====================================================================== +// Description: This program uses the fix-oriented methods available() and +// read() to handle complete fix structures. +// +// When the last character of the LAST_SENTENCE_IN_INTERVAL (see NMEAGPS_cfg.h) +// is decoded, a completed fix structure becomes available and is returned +// from read(). The new fix is saved the 'fix_data' structure, and can be used +// anywhere, at any time. +// +// If no messages are enabled in NMEAGPS_cfg.h, or +// no 'gps_fix' members are enabled in GPSfix_cfg.h, no information will be +// parsed, copied or printed. +// +// Prerequisites: +// 1) Your GPS device has been correctly powered. +// 2) Your GPS device is correctly connected using a serial adapter. +// By default /dev/ttyUSB0 is used. +// 3) You know the default baud rate of your GPS device. +// By default 9600 is assumed. If this doesn't work change it in serial.cpp +// 4) LAST_SENTENCE_IN_INTERVAL is defined to be the sentence that is +// sent *last* in each update interval (usually once per second). +// The default is NMEAGPS::NMEA_RMC (see NMEAGPS_cfg.h). Other +// programs may need to use the sentence identified by NMEAorder.ino. +// 5) NMEAGPS_RECOGNIZE_ALL is defined in NMEAGPS_cfg.h +// +//====================================================================== + +//------------------------------------------------------------ +// For the NeoGPS example programs, "Streamers" is common set +// of printing and formatting routines for GPS data, in a +// Comma-Separated Values text format (aka CSV). The CSV +// data will be printed to the "debug output device". +// If you don't need these formatters, simply delete this section. + +// platform.h must be included before NMEAGPS.h +#include + +#include +#include + +#include + +#include + +//------------------------------------------------------------ +// This object parses received characters +// into the gps.fix() data structure + +static NMEAGPS gps; + +//------------------------------------------------------------ +// Define a set of GPS fix information. It will +// hold on to the various pieces as they are received from +// an RMC sentence. It can be used anywhere in your sketch. + +static gps_fix fix_data; + +//---------------------------------------------------------------- +// This function gets called about once per second, during the GPS +// quiet time. It's the best place to do anything that might take +// a while: print a bunch of things, write to SD, send an SMS, etc. +// +// By doing the "hard" work during the quiet time, the CPU can get back to +// reading the GPS chars as they come in, so that no chars are lost. + +static auto & DEBUG_PORT = std::cout; + +static void doSomeWork() +{ + // Print all the things! + + trace_all( DEBUG_PORT, gps, fix_data ); + +} // doSomeWork + +//------------------------------------ +// This is the main GPS parsing loop. + +static void GPSloop(GpsPort gps_port) +{ + while (gps.available( gps_port )) { + fix_data = gps.read(); + doSomeWork(); + } + +} // GPSloop + +//-------------------------- + +GpsPort setup(const char * usbDev, const char * speed) +{ + // Start the normal trace output + DEBUG_PORT << "NMEA: started\n"; + DEBUG_PORT << " fix object size = "; + DEBUG_PORT << (uint32_t)sizeof(gps.fix()); + DEBUG_PORT << "\n gps object size = "; + DEBUG_PORT << (uint32_t)sizeof(gps); + DEBUG_PORT << "\nLooking for GPS device on "; + DEBUG_PORT << ( usbDev == nullptr ? "default" : usbDev ); + DEBUG_PORT << " with speed "; + DEBUG_PORT << ( speed == nullptr ? "default" : speed ); + + #ifndef NMEAGPS_RECOGNIZE_ALL + #error You must define NMEAGPS_RECOGNIZE_ALL in NMEAGPS_cfg.h! + #endif + + #ifdef NMEAGPS_INTERRUPT_PROCESSING + #error You must *NOT* define NMEAGPS_INTERRUPT_PROCESSING in NMEAGPS_cfg.h! + #endif + + #if !defined( NMEAGPS_PARSE_GGA ) & !defined( NMEAGPS_PARSE_GLL ) & \ + !defined( NMEAGPS_PARSE_GSA ) & !defined( NMEAGPS_PARSE_GSV ) & \ + !defined( NMEAGPS_PARSE_RMC ) & !defined( NMEAGPS_PARSE_VTG ) & \ + !defined( NMEAGPS_PARSE_ZDA ) & !defined( NMEAGPS_PARSE_GST ) + + DEBUG_PORT << ( F("\nWARNING: No NMEA sentences are enabled: no fix data will be displayed.\n") ); + + #else + if (gps.merging == NMEAGPS::NO_MERGING) { + DEBUG_PORT << F("\nWARNING: displaying data from "); + DEBUG_PORT << gps.string_for( LAST_SENTENCE_IN_INTERVAL ); + DEBUG_PORT << F(" sentences ONLY, and only if "); + DEBUG_PORT << gps.string_for( LAST_SENTENCE_IN_INTERVAL ); + DEBUG_PORT << F(" is enabled.\n" + " Other sentences may be parsed, but their data will not be displayed."); + DEBUG_PORT << "\n"; + } + #endif + + DEBUG_PORT << "\nGPS quiet time is assumed to begin after a "; + DEBUG_PORT << gps.string_for( LAST_SENTENCE_IN_INTERVAL ); + DEBUG_PORT << " sentence is received.\n" + " You should confirm this with NMEAorder.ino\n"; + DEBUG_PORT << "\n"; + + GpsPort gps_port = GpsPort(usbDev, speed); + + trace_header( DEBUG_PORT ); + + return gps_port; +} + +//-------------------------- + +int main(int argc, char *argv[]) { + auto dev = argc >= 2 ? argv[1] : nullptr; + auto speed = argc >=3 ? argv[2] : nullptr; + + auto gps_port = setup(dev, speed); + for (;;) { + GPSloop(gps_port); + } + return 0; +} diff --git a/examples/ublox/ublox.ino b/examples/ublox/ublox.ino index d501fac..5814446 100644 --- a/examples/ublox/ublox.ino +++ b/examples/ublox/ublox.ino @@ -289,6 +289,7 @@ static MyGPS gps( &gpsPort ); static void configNMEA( uint8_t rate ) { + (void) rate; for (uint8_t i=NMEAGPS::NMEA_FIRST_MSG; i<=NMEAGPS::NMEA_LAST_MSG; i++) { ublox::configNMEA( gps, (NMEAGPS::nmea_msg_t) i, rate ); } diff --git a/src/CosaCompat.h b/src/CosaCompat.h deleted file mode 100644 index ad9c080..0000000 --- a/src/CosaCompat.h +++ /dev/null @@ -1,34 +0,0 @@ -#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 . - -#ifdef __AVR__ - - #include - -#else - - #define PGM_P const char * - -#endif - -typedef PGM_P str_P; -#define __PROGMEM PROGMEM - -#endif \ No newline at end of file diff --git a/src/DMS.h b/src/DMS.h index bcd9986..a3b5891 100644 --- a/src/DMS.h +++ b/src/DMS.h @@ -1,55 +1,4 @@ -#ifndef DMS_H -#define DMS_H +#pragma once -// 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 . - -#include "NeoGPS_cfg.h" -#include -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 \ No newline at end of file +#include "DMS.header.h" +#include "DMS.impl.h" diff --git a/src/DMS.header.h b/src/DMS.header.h new file mode 100644 index 0000000..0f24b63 --- /dev/null +++ b/src/DMS.header.h @@ -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 . + +#include "NeoGPS_cfg.h" + +#include "Platform.h" + +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( NEO_GPS_PRINT & outs ) const NEO_GPS_PRINT_DEFAULT_IMPL_WARN; + +} NEOGPS_PACKED; + +NEO_GPS_PRINT & operator << ( NEO_GPS_PRINT & outs, const DMS_t & ) NEO_GPS_PRINT_DEFAULT_IMPL_WARN; + +#endif diff --git a/src/DMS.cpp b/src/DMS.impl.h similarity index 79% rename from src/DMS.cpp rename to src/DMS.impl.h index 198eb0a..880b012 100644 --- a/src/DMS.cpp +++ b/src/DMS.impl.h @@ -15,9 +15,11 @@ // You should have received a copy of the GNU General Public License // along with NeoGPS. If not, see . -#include "DMS.h" +// Just to be sure. This file should only be included in DMS.h which +// protects against multiple includes already. +#pragma once -#include +#include "DMS.header.h" //---------------------------------------------------------------- // Note that no division is used, and shifts are on byte boundaries. Fast! @@ -66,26 +68,26 @@ void DMS_t::From( int32_t deg_1E7 ) //---------------------------------------------------------------- -Print & operator << ( Print & outs, const DMS_t & dms ) +NEO_GPS_PRINT & operator << ( NEO_GPS_PRINT & outs, const DMS_t & dms ) { if (dms.degrees < 10) - outs.write( '0' ); - outs.print( dms.degrees ); - outs.write( ' ' ); + outs << '0'; + outs << +dms.degrees; + outs << ' '; if (dms.minutes < 10) - outs.write( '0' ); - outs.print( dms.minutes ); - outs.print( F("\' ") ); + outs << '0'; + outs << +dms.minutes; + outs << F("\' "); if (dms.seconds_whole < 10) - outs.write( '0' ); - outs.print( dms.seconds_whole ); - outs.write( '.' ); + outs << '0'; + outs << +dms.seconds_whole; + outs << '.'; if (dms.seconds_frac < 100) - outs.write( '0' ); + outs << '0'; if (dms.seconds_frac < 10) - outs.write( '0' ); - outs.print( dms.seconds_frac ); - outs.print( F("\" ") ); + outs << '0'; + outs << +dms.seconds_frac; + outs << F("\" "); return outs; @@ -93,14 +95,14 @@ Print & operator << ( Print & outs, const DMS_t & dms ) //---------------------------------------------------------------- -void DMS_t::printDDDMMmmmm( Print & outs ) const +void DMS_t::printDDDMMmmmm( NEO_GPS_PRINT & outs ) const { - outs.print( degrees ); + outs << degrees; if (minutes < 10) - outs.print( '0' ); - outs.print( minutes ); - outs.print( '.' ); + outs << '0'; + outs << +minutes; + outs << '.'; // Calculate the fractional minutes from the seconds, // *without* using floating-point numbers. @@ -112,10 +114,10 @@ void DMS_t::printDDDMMmmmm( Print & outs ) const // print leading zeroes, if necessary if (mmmm < 1000) - outs.print( '0' ); + outs << '0'; if (mmmm < 100) - outs.print( '0' ); + outs << '0'; if (mmmm < 10) - outs.print( '0' ); - outs.print( mmmm ); -} \ No newline at end of file + outs << '0'; + outs << +mmmm; +} diff --git a/src/GPSTime.h b/src/GPSTime.h index 235dc07..ab53b91 100644 --- a/src/GPSTime.h +++ b/src/GPSTime.h @@ -1,85 +1,4 @@ -#ifndef GPSTIME_H -#define GPSTIME_H +#pragma once -// 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 . - -#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 \ No newline at end of file +#include "GPSTime.header.h" +#include "GPSTime.impl.h" diff --git a/src/GPSTime.header.h b/src/GPSTime.header.h new file mode 100644 index 0000000..1f544a4 --- /dev/null +++ b/src/GPSTime.header.h @@ -0,0 +1,87 @@ +#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 . + +#include "Platform.h" + +#include "NeoTime.header.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 diff --git a/src/GPSTime.cpp b/src/GPSTime.impl.h similarity index 83% rename from src/GPSTime.cpp rename to src/GPSTime.impl.h index cee247f..70bb5ca 100644 --- a/src/GPSTime.cpp +++ b/src/GPSTime.impl.h @@ -15,7 +15,11 @@ // You should have received a copy of the GNU General Public License // along with NeoGPS. If not, see . -#include "GPSTime.h" +// Just to be sure. This file should only be included in GPSTime.h which +// protects against multiple includes already. +#pragma once + +#include "GPSTime.header.h" uint8_t GPSTime::leap_seconds = 0; NeoGPS::clock_t GPSTime::s_start_of_week = 0; diff --git a/src/GPSfix.h b/src/GPSfix.h index a8eff41..742e5fd 100644 --- a/src/GPSfix.h +++ b/src/GPSfix.h @@ -495,4 +495,4 @@ class gps_fix } NEOGPS_PACKED; -#endif \ No newline at end of file +#endif diff --git a/src/GPSport.h b/src/GPSport.h index e0e3ece..4290160 100644 --- a/src/GPSport.h +++ b/src/GPSport.h @@ -124,6 +124,10 @@ // so that you could use one of the other recommended ports. //----------------------------------------------------------- +#ifndef ARDUINO + #error "GPSport.h is only useful for the Arduino platform." +#endif + // 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, diff --git a/src/Location.h b/src/Location.h index 8b0e054..659b55e 100644 --- a/src/Location.h +++ b/src/Location.h @@ -1,128 +1,4 @@ -#ifndef NEOGPS_LOCATION_H -#define NEOGPS_LOCATION_H +#pragma once -#include - -#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 . - -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 \ No newline at end of file +#include "Location.header.h" +#include "Location.impl.h" diff --git a/src/Location.header.h b/src/Location.header.h new file mode 100644 index 0000000..fb81fdd --- /dev/null +++ b/src/Location.header.h @@ -0,0 +1,128 @@ +#ifndef NEOGPS_LOCATION_H +#define NEOGPS_LOCATION_H + +#include "Platform.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 . + +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 diff --git a/src/Location.cpp b/src/Location.impl.h similarity index 87% rename from src/Location.cpp rename to src/Location.impl.h index 10eb453..dacfb02 100644 --- a/src/Location.cpp +++ b/src/Location.impl.h @@ -15,9 +15,12 @@ // You should have received a copy of the GNU General Public License // along with NeoGPS. If not, see . -#include "Location.h" -using namespace NeoGPS; +// Just to be sure. This file should only be included in Location.h which +// protects against multiple includes already. +#pragma once + +#include "Location.header.h" //--------------------------------------------------------------------- // Calculate dLon with integers, less one bit to avoid overflow @@ -41,8 +44,8 @@ int32_t safeDLon( int32_t p2, int32_t p1 ) //--------------------------------------------------------------------- -float Location_t::DistanceRadians - ( const Location_t & p1, const Location_t & p2 ) +float NeoGPS::Location_t::DistanceRadians + ( const NeoGPS::Location_t & p1, const NeoGPS::Location_t & p2 ) { int32_t dLonL = safeDLon( p2.lon(), p1.lon() ); int32_t dLatL = p2.lat() - p1.lat(); @@ -77,8 +80,8 @@ float Location_t::DistanceRadians //--------------------------------------------------------------------- -float Location_t::EquirectDistanceRadians - ( const Location_t & p1, const Location_t & p2 ) +float NeoGPS::Location_t::EquirectDistanceRadians + ( const NeoGPS::Location_t & p1, const NeoGPS::Location_t & p2 ) { // Equirectangular calculation from http://www.movable-type.co.uk/scripts/latlong.html @@ -91,7 +94,7 @@ float Location_t::EquirectDistanceRadians //--------------------------------------------------------------------- -float Location_t::BearingTo( const Location_t & p1, const Location_t & p2 ) +float NeoGPS::Location_t::BearingTo( const NeoGPS::Location_t & p1, const NeoGPS::Location_t & p2 ) { int32_t dLonL = safeDLon( p2.lon(), p1.lon() ); float dLon = dLonL * RAD_PER_DEG * LOC_SCALE; @@ -123,7 +126,7 @@ float Location_t::BearingTo( const Location_t & p1, const Location_t & p2 ) //--------------------------------------------------------------------- -void Location_t::OffsetBy( float distR, float bearingR ) +void NeoGPS::Location_t::OffsetBy( float distR, float bearingR ) { float lat1 = lat() * RAD_PER_DEG * LOC_SCALE; float newLat = asin( sin(lat1)*cos(distR) + @@ -134,4 +137,4 @@ void Location_t::OffsetBy( float distR, float bearingR ) _lat = (newLat / (RAD_PER_DEG * LOC_SCALE)); _lon += (dLon / (RAD_PER_DEG * LOC_SCALE)); -} // OffsetBy \ No newline at end of file +} // OffsetBy diff --git a/src/NMEAGPS.h b/src/NMEAGPS.h index 6e28372..2ce2184 100644 --- a/src/NMEAGPS.h +++ b/src/NMEAGPS.h @@ -1,340 +1,4 @@ -#ifndef NMEAGPS_H -#define NMEAGPS_H +#pragma once -// 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 . - -#include "CosaCompat.h" - -#include -#ifdef __AVR__ - #include -#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 +#include "NMEAGPS.header.h" +#include "NMEAGPS.impl.h" diff --git a/src/NMEAGPS.header.h b/src/NMEAGPS.header.h new file mode 100644 index 0000000..29f9c68 --- /dev/null +++ b/src/NMEAGPS.header.h @@ -0,0 +1,337 @@ +#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 . + +#include "Platform.h" + +#include "GPSfix.h" +#include "NMEAGPS_cfg.h" +#include + +//------------------------------------------------------ +// +// 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( NEO_GPS_STREAM & port ) NEO_GPS_STREAM_DEFAULT_IMPL_WARN + { + 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 ARDUINO && \ + (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( NEO_GPS_STREAM *device, nmea_msg_t msg ) NEO_GPS_STREAM_DEFAULT_IMPL_WARN; + + //....................................................................... + // Send a message to the GPS device. + // The '$' is optional, and the '*' and CS will be added automatically. + + static void send( NEO_GPS_STREAM *device, const char *msg ) NEO_GPS_STREAM_DEFAULT_IMPL_WARN; + static void send_P( NEO_GPS_STREAM *device, const __FlashStringHelper *msg ) NEO_GPS_STREAM_DEFAULT_IMPL_WARN; + + #include "NMEAGPSprivate.h" + +} NEOGPS_PACKED; + +#endif diff --git a/src/NMEAGPS.cpp b/src/NMEAGPS.impl.h similarity index 97% rename from src/NMEAGPS.cpp rename to src/NMEAGPS.impl.h index b62ae60..35123d5 100644 --- a/src/NMEAGPS.cpp +++ b/src/NMEAGPS.impl.h @@ -15,9 +15,15 @@ // You should have received a copy of the GNU General Public License // along with NeoGPS. If not, see . -#include "NMEAGPS.h" -#include +// Just to be sure. This file should only be included in NMEAGPS.h which +// protects against multiple includes already. +#pragma once + +#pragma push_macro( "CR" ) +#pragma push_macro( "LF" ) + +#include "NMEAGPS.header.h" // Check configurations @@ -535,12 +541,13 @@ NMEAGPS::decode_t NMEAGPS::parseCommand uint8_t msg_offset = pgm_read_byte( &msgs->offset ); bool check_this_table = true; #else + (void) msgs; const uint8_t table_size = sizeof(std_nmea)/sizeof(std_nmea[0]); const uint8_t msg_offset = NMEA_FIRST_MSG; const bool check_this_table = true; #endif decode_t res = DECODE_CHR_INVALID; - uint8_t entry; + uint8_t entry = 0; if (nmeaMessage == NMEA_UNKNOWN) { // We're just starting @@ -627,7 +634,9 @@ const __FlashStringHelper *NMEAGPS::string_for( nmea_msg_t msg ) const if (msg == NMEA_UNKNOWN) return F("UNK"); - const msg_table_t *msgs = msg_table(); + #ifdef NMEAGPS_DERIVED_TYPES + const msg_table_t *msgs = msg_table(); + #endif for (;;) { #ifdef NMEAGPS_DERIVED_TYPES @@ -722,6 +731,8 @@ bool NMEAGPS::parseGGA( char chr ) case 9: return parseAlt( chr ); case 11: return parseGeoidHeight( chr ); } + #else + (void) chr; #endif return true; @@ -738,6 +749,8 @@ bool NMEAGPS::parseGLL( char chr ) case 5: return parseTime( chr ); case 7: return parseFix( chr ); } + #else + (void) chr; #endif return true; @@ -823,6 +836,8 @@ bool NMEAGPS::parseGSA( char chr ) #endif #endif } + #else + (void) chr; #endif return true; @@ -840,6 +855,8 @@ bool NMEAGPS::parseGST( char chr ) case 7: return parse_lon_err( chr ); case 8: return parse_alt_err( chr ); } + #else + (void) chr; #endif return true; @@ -914,6 +931,8 @@ bool NMEAGPS::parseGSV( char chr ) } } } + #else + (void) chr; #endif return true; @@ -934,6 +953,8 @@ bool NMEAGPS::parseRMC( char chr ) case 9: return parseDDMMYY ( chr ); // case 12: return parseFix ( chr ); ublox only! } + #else + (void) chr; #endif return true; @@ -950,6 +971,8 @@ bool NMEAGPS::parseVTG( char chr ) case 5: return parseSpeed( chr ); case 9: return parseFix( chr ); } + #else + (void) chr; #endif return true; @@ -1021,6 +1044,8 @@ bool NMEAGPS::parseZDA( char chr ) break; #endif } + #else + (void) chr; #endif return true; @@ -1113,6 +1138,8 @@ bool NMEAGPS::parseTime(char chr) sentenceInvalid(); break; } + #else + (void) chr; #endif return true; @@ -1192,6 +1219,8 @@ bool NMEAGPS::parseDDMMYY( char chr ) sentenceInvalid(); break; } + #else + (void) chr; #endif return true; @@ -1353,6 +1382,7 @@ bool NMEAGPS::parseFloat( uint16_t & val, char chr, uint8_t max_decimal ) static uint32_t divu3( uint32_t n ) { + // FIXME for all microcontrollers interesting #ifdef __AVR__ uint32_t q = (n >> 2) + (n >> 4); // q = n*0.0101 (approx). q = q + (q >> 4); // q = n*0.01010101. @@ -1557,6 +1587,8 @@ bool NMEAGPS::parseLat( char chr ) } } } + #else + (void) chr; #endif return true; @@ -1590,6 +1622,8 @@ bool NMEAGPS::parseNS( char chr ) sentenceInvalid(); } } + #else + (void) chr; #endif return true; @@ -1634,6 +1668,8 @@ bool NMEAGPS::parseLon( char chr ) } } } + #else + (void) chr; #endif return true; @@ -1668,6 +1704,8 @@ bool NMEAGPS::parseEW( char chr ) sentenceInvalid(); } } + #else + (void) chr; #endif return true; @@ -1688,6 +1726,8 @@ bool NMEAGPS::parseSpeed( char chr ) else m_fix.valid.speed = (chrCount != 0); } + #else + (void) chr; #endif return true; @@ -1709,6 +1749,8 @@ bool NMEAGPS::parseHeading( char chr ) else m_fix.valid.heading = (chrCount != 0); } + #else + (void) chr; #endif return true; @@ -1728,6 +1770,8 @@ bool NMEAGPS::parseAlt(char chr ) else m_fix.valid.altitude = (chrCount != 0); } + #else + (void) chr; #endif return true; @@ -1743,6 +1787,8 @@ bool NMEAGPS::parseGeoidHeight( char chr ) NMEAGPS_INVALIDATE( geoidHeight ); if (parseFloat( m_fix.geoidHt, chr, 2 )) m_fix.valid.geoidHeight = (chrCount != 0); + #else + (void) chr; #endif return true; @@ -1763,6 +1809,8 @@ bool NMEAGPS::parseSatellites( char chr ) else m_fix.valid.satellites = true; } + #else + (void) chr; #endif return true; @@ -1782,6 +1830,8 @@ bool NMEAGPS::parseHDOP( char chr ) else m_fix.valid.hdop = (chrCount != 0); } + #else + (void) chr; #endif return true; @@ -1801,6 +1851,8 @@ bool NMEAGPS::parseVDOP( char chr ) else m_fix.valid.vdop = (chrCount != 0); } + #else + (void) chr; #endif return true; @@ -1820,6 +1872,8 @@ bool NMEAGPS::parsePDOP( char chr ) else m_fix.valid.pdop = (chrCount != 0); } + #else + (void) chr; #endif return true; @@ -1842,6 +1896,8 @@ bool NMEAGPS::parse_lat_err( char chr ) else m_fix.valid.lat_err = (chrCount != 0); } + #else + (void) chr; #endif return true; @@ -1862,6 +1918,8 @@ bool NMEAGPS::parse_lon_err( char chr ) else m_fix.valid.lon_err = (chrCount != 0); } + #else + (void) chr; #endif return true; @@ -1882,6 +1940,8 @@ bool NMEAGPS::parse_alt_err( char chr ) else m_fix.valid.alt_err = (chrCount != 0); } + #else + (void) chr; #endif return true; @@ -1921,7 +1981,7 @@ const gps_fix NMEAGPS::read() //---------------------------------------------------------------- -void NMEAGPS::poll( Stream *device, nmea_msg_t msg ) +void NMEAGPS::poll( NEO_GPS_STREAM *device, nmea_msg_t msg ) { // Only the ublox documentation references talker ID "EI". // Other manufacturer's devices use "II" and "GP" talker IDs for the GPQ sentence. @@ -1991,13 +2051,13 @@ void NMEAGPS::poll( Stream *device, nmea_msg_t msg ) //---------------------------------------------------------------- -static void send_trailer( Stream *device, uint8_t crc ) +static void send_trailer( NEO_GPS_STREAM *device, uint8_t crc ) { - device->print('*'); + device->print( '*' ); char hexDigit = formatHex( crc>>4 ); device->print( hexDigit ); - + hexDigit = formatHex( crc ); device->print( hexDigit ); @@ -2008,12 +2068,12 @@ static void send_trailer( Stream *device, uint8_t crc ) //---------------------------------------------------------------- -void NMEAGPS::send( Stream *device, const char *msg ) +void NMEAGPS::send( NEO_GPS_STREAM *device, const char *msg ) { if (msg && *msg) { if (*msg == '$') msg++; - device->print('$'); + device->print( '$' ); uint8_t sent_trailer = 0; uint8_t crc = 0; while (*msg) { @@ -2032,13 +2092,13 @@ void NMEAGPS::send( Stream *device, const char *msg ) //---------------------------------------------------------------- -void NMEAGPS::send_P( Stream *device, const __FlashStringHelper *msg ) +void NMEAGPS::send_P( NEO_GPS_STREAM *device, const __FlashStringHelper *msg ) { if (msg) { const char *ptr = (const char *)msg; char chr = pgm_read_byte(ptr++); - device->print('$'); + device->print( '$' ); if (chr == '$') chr = pgm_read_byte(ptr++); uint8_t sent_trailer = 0; @@ -2058,3 +2118,6 @@ void NMEAGPS::send_P( Stream *device, const __FlashStringHelper *msg ) } } // send_P + +#pragma pop_macro( "LF" ) +#pragma pop_macro( "CR" ) diff --git a/src/NMEAGPSprivate.h b/src/NMEAGPSprivate.h index 5f1ed6c..d6abe33 100644 --- a/src/NMEAGPSprivate.h +++ b/src/NMEAGPSprivate.h @@ -15,6 +15,9 @@ // You should have received a copy of the GNU General Public License // along with NeoGPS. If not, see . +// This pragma doesn't make much sense, but will make some IDEs happy. +#pragma once + protected: //....................................................................... // Table entry for NMEA sentence type string and its offset @@ -86,16 +89,16 @@ // Control access to this object. This preserves atomicity when // the processing style is interrupt-driven. - void lock() const + void lock() const NEO_GPS_SYSTEM_DEFAULT_IMPL_WARN { if (processing_style == PS_INTERRUPT) - noInterrupts(); + NEO_GPS_SYSTEM::lock(); } - void unlock() const + void unlock() const NEO_GPS_SYSTEM_DEFAULT_IMPL_WARN { if (processing_style == PS_INTERRUPT) - interrupts(); + NEO_GPS_SYSTEM::unlock(); } protected: diff --git a/src/NeoGPS_cfg.h b/src/NeoGPS_cfg.h index e5aefc0..022ba46 100644 --- a/src/NeoGPS_cfg.h +++ b/src/NeoGPS_cfg.h @@ -87,7 +87,10 @@ // The CONST_CLASS_DATA define will expand to the appropriate keywords. // -#if (ARDUINO < 10606) | ((10700 <= ARDUINO) & (ARDUINO <= 10799)) | ((107000 <= ARDUINO) & (ARDUINO <= 107999)) +#if defined ARDUINO && \ + ((ARDUINO < 10606) || \ + ((10700 <= ARDUINO) && (ARDUINO <= 10799)) || \ + ((107000 <= ARDUINO) && (ARDUINO <= 107999))) #define CONST_CLASS_DATA static const diff --git a/src/NeoTime.h b/src/NeoTime.h index 6c3564e..fa33a0a 100644 --- a/src/NeoTime.h +++ b/src/NeoTime.h @@ -1,307 +1,4 @@ -#ifndef TIME_H -#define TIME_H +#pragma once -// 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 . - -#include - -#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 +#include "NeoTime.header.h" +#include "NeoTime.impl.h" diff --git a/src/NeoTime.header.h b/src/NeoTime.header.h new file mode 100644 index 0000000..0d59603 --- /dev/null +++ b/src/NeoTime.header.h @@ -0,0 +1,304 @@ +#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 . + +#include "Platform.h" + +#include "NeoGPS_cfg.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 + +/** + * 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. + */ +NEO_GPS_PRINT & operator <<( NEO_GPS_PRINT & outs, const NeoGPS::time_t &t ) NEO_GPS_PRINT_DEFAULT_IMPL_WARN; + +#endif diff --git a/src/NeoTime.cpp b/src/NeoTime.impl.h similarity index 74% rename from src/NeoTime.cpp rename to src/NeoTime.impl.h index 92adc8a..408e187 100644 --- a/src/NeoTime.cpp +++ b/src/NeoTime.impl.h @@ -15,43 +15,48 @@ // You should have received a copy of the GNU General Public License // along with NeoGPS. If not, see . -#include "NeoTime.h" + +// Just to be sure. This file should only be included in NeoTime.h which +// protects against multiple includes already. +#pragma once + +#include "NeoTime.header.h" // For strtoul declaration #include -#include - -Print & operator<<( Print& outs, const NeoGPS::time_t& t ) +NEO_GPS_PRINT & operator<<( NEO_GPS_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 ); + outs << +t.full_year( t.year ); + outs << '-'; + if (t.month < 10) outs << '0'; + outs << +t.month; + outs << '-'; + if (t.date < 10) outs << '0'; + outs << +t.date; + outs << ' '; + if (t.hours < 10) outs << '0'; + outs << +t.hours; + outs << ':'; + if (t.minutes < 10) outs << '0'; + outs << +t.minutes; + outs << ':'; + if (t.seconds < 10) outs << '0'; + outs << +t.seconds; return outs; } -using NeoGPS::time_t; - -bool time_t::parse(str_P s) +bool NeoGPS::time_t::parse(str_P s) { - static size_t BUF_MAX = 32; - char buf[BUF_MAX]; - strcpy_P(buf, s); - char* sp = &buf[0]; + #ifdef AVR + static size_t BUF_MAX = 32; + char buf[BUF_MAX]; + strcpy_P(buf, s); + char* sp = &buf[0]; + #else + char* sp = (char *)&s[0]; + #endif uint16_t value = strtoul(sp, &sp, 10); if (*sp != '-') return false; @@ -82,17 +87,17 @@ bool time_t::parse(str_P s) } #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; + uint16_t NeoGPS::time_t::s_epoch_year = Y2K_EPOCH_YEAR; + uint8_t NeoGPS::time_t::s_epoch_offset = 0; + uint8_t NeoGPS::time_t::s_epoch_weekday = Y2K_EPOCH_WEEKDAY; + uint8_t NeoGPS::time_t::s_pivot_year = 0; #endif -const uint8_t time_t::days_in[] __PROGMEM = { +const uint8_t NeoGPS::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) +NeoGPS::time_t::time_t(clock_t c) { uint16_t dayno = c / SECONDS_PER_DAY; c -= dayno * (uint32_t) SECONDS_PER_DAY; @@ -133,7 +138,7 @@ time_t::time_t(clock_t c) seconds = c_ms - (minutes * SECONDS_PER_MINUTE); } -void time_t::init() +void NeoGPS::time_t::init() { seconds = hours = @@ -144,7 +149,7 @@ void time_t::init() day = epoch_weekday(); } -time_t::operator clock_t() const +NeoGPS::time_t::operator clock_t() const { clock_t c = days() * SECONDS_PER_DAY; if (hours < 18) @@ -157,7 +162,7 @@ time_t::operator clock_t() const return (c); } -uint16_t time_t::days() const +uint16_t NeoGPS::time_t::days() const { uint16_t day_count = day_of_year(); @@ -168,7 +173,7 @@ uint16_t time_t::days() const return (day_count); } -uint16_t time_t::day_of_year() const +uint16_t NeoGPS::time_t::day_of_year() const { uint16_t dayno = date - 1; bool leap_year = is_leap(); @@ -182,7 +187,7 @@ uint16_t time_t::day_of_year() const } #ifdef TIME_EPOCH_MODIFIABLE - void time_t::use_fastest_epoch() + void NeoGPS::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" @@ -196,7 +201,7 @@ uint16_t time_t::day_of_year() const epoch_year ( Y2K_EPOCH_YEAR ); epoch_weekday ( Y2K_EPOCH_WEEKDAY ); - time_t this_year(0); + NeoGPS::time_t this_year(0); this_year.year = compile_year % 100; this_year.set_day(); uint8_t compile_weekday = this_year.day; @@ -205,4 +210,4 @@ uint16_t time_t::day_of_year() const epoch_weekday( compile_weekday ); pivot_year ( this_year.year ); } -#endif \ No newline at end of file +#endif diff --git a/src/Platform.h b/src/Platform.h new file mode 100644 index 0000000..ec80c8e --- /dev/null +++ b/src/Platform.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +#ifdef ARDUINO + #include +#else + #include + #include + #include +#endif + +#ifdef __AVR__ + #include + #include +#else + #define PGM_P const char * + #define pgm_read_byte(x) (*(x)) + #define __FlashStringHelper char + #define F(x) (x) +#endif + +typedef PGM_P str_P; + +#include + +#ifndef PI + #define PI 3.14159265358979323846 +#endif +#ifndef TWO_PI + #define TWO_PI PI * 2.0 +#endif + +#ifndef PROGMEM + #define PROGMEM +#endif + +#define __PROGMEM PROGMEM diff --git a/src/Streamers.h b/src/Streamers.h index f942931..26c27f3 100644 --- a/src/Streamers.h +++ b/src/Streamers.h @@ -1,50 +1,4 @@ -#ifndef STREAMERS_H -#define STREAMERS_H +#pragma once -// 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 . - -#include - -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 \ No newline at end of file +#include "Streamers.header.h" +#include "Streamers.impl.h" diff --git a/src/Streamers.header.h b/src/Streamers.header.h new file mode 100644 index 0000000..69bba31 --- /dev/null +++ b/src/Streamers.header.h @@ -0,0 +1,38 @@ +#pragma once + +// 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 . + +#include "Platform.h" + +#include + +/** + * 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 The outs object. + */ +NEO_GPS_PRINT & operator <<( NEO_GPS_PRINT & outs, const gps_fix &fix ) NEO_GPS_PRINT_DEFAULT_IMPL_WARN; + +void trace_header( NEO_GPS_PRINT & outs ) NEO_GPS_PRINT_DEFAULT_IMPL_WARN; + +void trace_all( NEO_GPS_PRINT & outs, const NMEAGPS &gps, const gps_fix &fix ) NEO_GPS_PRINT_DEFAULT_IMPL_WARN; diff --git a/src/Streamers.cpp b/src/Streamers.impl.h similarity index 69% rename from src/Streamers.cpp rename to src/Streamers.impl.h index 1c2ab04..a3445e4 100644 --- a/src/Streamers.cpp +++ b/src/Streamers.impl.h @@ -15,26 +15,14 @@ // You should have received a copy of the GNU General Public License // along with NeoGPS. If not, see . -#include "Streamers.h" -#include "NMEAGPS.h" +// Just to be sure. This file should only be included in Streamers.h which +// protects against multiple includes already. +#pragma once -//#define USE_FLOAT +#include "Streamers.header.h" -Print& operator <<( Print &outs, const bool b ) - { outs.print( b ? 't' : 'f' ); return outs; } +#include "GPSfix.h" -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; } //------------------------------------------ @@ -119,39 +107,38 @@ const char gps_fix_header[] __PROGMEM = #ifdef GPS_FIX_LOCATION_DMS - static void printDMS( Print & outs, const DMS_t & dms ) + static void printDMS( NEO_GPS_PRINT & outs, const DMS_t & dms ) { if (dms.degrees < 10) - outs.write( '0' ); - outs.print( dms.degrees ); - outs.write( ' ' ); + outs << '0'; + outs << +dms.degrees << ' '; if (dms.minutes < 10) - outs.write( '0' ); - outs.print( dms.minutes ); - outs.print( F("\' ") ); + outs << '0'; + outs << +dms.minutes; + outs << F("\' "); if (dms.seconds_whole < 10) - outs.write( '0' ); - outs.print( dms.seconds_whole ); - outs.write( '.' ); + outs << '0'; + outs << +dms.seconds_whole; + outs << '.'; if (dms.seconds_frac < 100) - outs.write( '0' ); + outs << '0'; if (dms.seconds_frac < 10) - outs.write( '0' ); - outs.print( dms.seconds_frac ); - outs.print( F("\" ") ); + outs << '0'; + outs << +dms.seconds_frac; + outs << F("\" "); } // printDMS #endif //............... -Print & operator <<( Print &outs, const gps_fix &fix ) +NEO_GPS_PRINT & operator <<( NEO_GPS_PRINT &outs, const gps_fix &fix ) { if (fix.valid.status) - outs << (uint8_t) fix.status; + outs << (uint) fix.status; outs << ','; #if defined(GPS_FIX_DATE) | defined(GPS_FIX_TIME) @@ -172,7 +159,7 @@ Print & operator <<( Print &outs, const gps_fix &fix ) outs << '0'; if (ms < 10) outs << '0'; - outs << ms; + outs << +ms; } outs << ','; @@ -180,16 +167,16 @@ Print & operator <<( Print &outs, const gps_fix &fix ) // Date/Time not enabled, just output the interval number static uint32_t sequence = 0L; - outs << sequence++ << ','; + outs << +sequence++ << ','; #endif #ifdef USE_FLOAT #ifdef GPS_FIX_LOCATION if (fix.valid.location) { - outs.print( fix.latitude(), 6 ); + printFloat(outs, fix.latitude(), 6); outs << ','; - outs.print( fix.longitude(), 6 ); + printFloat(outs, fix.longitude(), 6); } else outs << ','; outs << ','; @@ -197,66 +184,66 @@ Print & operator <<( Print &outs, const gps_fix &fix ) #ifdef GPS_FIX_LOCATION_DMS if (fix.valid.location) { printDMS( outs, fix.latitudeDMS ); - outs.print( fix.latitudeDMS.NS() ); + outs << fix.latitudeDMS.NS(); outs.write( ' ' ); if (fix.longitudeDMS.degrees < 100) - outs.write( '0' ); + outs << '0'; printDMS( outs, fix.longitudeDMS ); - outs.print( fix.longitudeDMS.EW() ); + outs << fix.longitudeDMS.EW(); } outs << ','; #endif #ifdef GPS_FIX_HEADING if (fix.valid.heading) - outs.print( fix.heading(), 2 ); + printFloat( outs, fix.heading(), 2); outs << ','; #endif #ifdef GPS_FIX_SPEED if (fix.valid.speed) - outs.print( fix.speed(), 3 ); // knots + printFloat( outs, fix.speed(), 3); outs << ','; #endif #ifdef GPS_FIX_ALTITUDE if (fix.valid.altitude) - outs.print( fix.altitude(), 2 ); + printFloat( outs, fix.altitude(), 2); outs << ','; #endif #ifdef GPS_FIX_HDOP if (fix.valid.hdop) - outs.print( (fix.hdop * 0.001), 3 ); + printFloat( outs, (fix.hdop * 0.001), 3 ); outs << ','; #endif #ifdef GPS_FIX_VDOP if (fix.valid.vdop) - outs.print( (fix.vdop * 0.001), 3 ); + printFloat( outs, (fix.vdop * 0.001), 3 ); outs << ','; #endif #ifdef GPS_FIX_PDOP if (fix.valid.pdop) - outs.print( (fix.pdop * 0.001), 3 ); + printFloat( outs, (fix.pdop * 0.001), 3 ); outs << ','; #endif #ifdef GPS_FIX_LAT_ERR if (fix.valid.lat_err) - outs.print( fix.lat_err(), 2 ); + printFloat( outs, fix.lat_err(), 2 ); outs << ','; #endif #ifdef GPS_FIX_LON_ERR if (fix.valid.lon_err) - outs.print( fix.lon_err(), 2 ); + printFloat( outs, fix.lon_err(), 2 ); outs << ','; #endif #ifdef GPS_FIX_ALT_ERR if (fix.valid.alt_err) - outs.print( fix.alt_err(), 2 ); + printFloat( outs, fix.alt_err(), 2 ); outs << ','; #endif #ifdef GPS_FIX_GEOID_HEIGHT if (fix.valid.geoidHeight) - outs.print( fix.geoidHeight(), 2 ); + printFloat( outs, fix.geoidHeight(), 2 ); outs << ','; #endif @@ -266,7 +253,7 @@ Print & operator <<( Print &outs, const gps_fix &fix ) #ifdef GPS_FIX_LOCATION if (fix.valid.location) - outs << fix.latitudeL() << ',' << fix.longitudeL(); + outs << +fix.latitudeL() << ',' << +fix.longitudeL(); else outs << ','; outs << ','; @@ -274,66 +261,66 @@ Print & operator <<( Print &outs, const gps_fix &fix ) #ifdef GPS_FIX_LOCATION_DMS if (fix.valid.location) { printDMS( outs, fix.latitudeDMS ); - outs.print( fix.latitudeDMS.NS() ); - outs.write( ' ' ); + outs << fix.latitudeDMS.NS(); + outs << ' '; if (fix.longitudeDMS.degrees < 100) - outs.write( '0' ); + outs << '0'; printDMS( outs, fix.longitudeDMS ); - outs.print( fix.longitudeDMS.EW() ); + outs << fix.longitudeDMS.EW(); } outs << ','; #endif #ifdef GPS_FIX_HEADING if (fix.valid.heading) - outs << fix.heading_cd(); + outs << +fix.heading_cd(); outs << ','; #endif #ifdef GPS_FIX_SPEED if (fix.valid.speed) - outs << fix.speed_mkn(); + outs << +fix.speed_mkn(); outs << ','; #endif #ifdef GPS_FIX_ALTITUDE if (fix.valid.altitude) - outs << fix.altitude_cm(); + outs << +fix.altitude_cm(); outs << ','; #endif #ifdef GPS_FIX_HDOP if (fix.valid.hdop) - outs << fix.hdop; + outs << +fix.hdop; outs << ','; #endif #ifdef GPS_FIX_VDOP if (fix.valid.vdop) - outs << fix.vdop; + outs << +fix.vdop; outs << ','; #endif #ifdef GPS_FIX_PDOP if (fix.valid.pdop) - outs << fix.pdop; + outs << +fix.pdop; outs << ','; #endif #ifdef GPS_FIX_LAT_ERR if (fix.valid.lat_err) - outs << fix.lat_err_cm; + outs << +fix.lat_err_cm; outs << ','; #endif #ifdef GPS_FIX_LON_ERR if (fix.valid.lon_err) - outs << fix.lon_err_cm; + outs << +fix.lon_err_cm; outs << ','; #endif #ifdef GPS_FIX_ALT_ERR if (fix.valid.alt_err) - outs << fix.alt_err_cm; + outs << +fix.alt_err_cm; outs << ','; #endif #ifdef GPS_FIX_GEOID_HEIGHT if (fix.valid.geoidHeight) - outs << fix.geoidHeight_cm(); + outs << +fix.geoidHeight_cm(); outs << ','; #endif @@ -341,7 +328,7 @@ Print & operator <<( Print &outs, const gps_fix &fix ) #ifdef GPS_FIX_SATELLITES if (fix.valid.satellites) - outs << fix.satellites; + outs << +fix.satellites; outs << ','; #endif @@ -369,22 +356,22 @@ static const char NMEAGPS_header[] __PROGMEM = ""; -void trace_header( Print & outs ) +void trace_header( NEO_GPS_PRINT & outs ) { - outs.print( (const __FlashStringHelper *) &gps_fix_header[0] ); - outs.print( (const __FlashStringHelper *) &NMEAGPS_header[0] ); + outs << ((const __FlashStringHelper *) &gps_fix_header[0]); + outs << ((const __FlashStringHelper *) &NMEAGPS_header[0]); outs << '\n'; } //-------------------------- -void trace_all( Print & outs, const NMEAGPS &gps, const gps_fix &fix ) +void trace_all( NEO_GPS_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 << +gps.UTCsecondStart(); outs << ','; #endif @@ -392,14 +379,14 @@ void trace_all( Print & outs, const NMEAGPS &gps, const gps_fix &fix ) outs << '['; for (uint8_t i=0; i < gps.sat_count; i++) { - outs << gps.satellites[i].id; + outs << +gps.satellites[i].id; #if defined(NMEAGPS_PARSE_SATELLITE_INFO) outs << ' ' << - gps.satellites[i].elevation << '/' << gps.satellites[i].azimuth; + +gps.satellites[i].elevation << '/' << +gps.satellites[i].azimuth; outs << '@'; if (gps.satellites[i].tracked) - outs << gps.satellites[i].snr; + outs << +gps.satellites[i].snr; else outs << '-'; #endif diff --git a/src/platforms/Print.h b/src/platforms/Print.h new file mode 100644 index 0000000..1798b6e --- /dev/null +++ b/src/platforms/Print.h @@ -0,0 +1,36 @@ +#pragma once + +#include + +#ifndef NEO_GPS_PRINT + #define NEO_GPS_PRINT NeoGPS::Print + #ifdef __GNUC__ + #define NEO_GPS_PRINT_DEFAULT_IMPL_WARN \ + __attribute__((deprecated("You are using a function which uses NEO_GPS_PRINT without defining the type. Compilation will fail. To fix this #define NEO_GPS_PRINT your_print_type_t before including platform.h"))) + #endif + +/** + * This file implements a non working dummy implemenation for Print + **/ +namespace NeoGPS { + + class Print { + }; + + Print & operator << ( Print & print, char ) { return print; } + Print & operator << ( Print & print, uint8_t ) { return print; } + Print & operator << ( Print & print, uint16_t ) { return print; } + Print & operator << ( Print & print, uint32_t ) { return print; } + Print & operator << ( Print & print, int32_t ) { return print; } + Print & operator << ( Print & print, const char * ) { return print; } + + #ifdef USE_FLOAT + void printFloat( Print &, float f, uint8_t decPlaces ) {} + #endif +} + +#endif + +#ifndef NEO_GPS_PRINT_DEFAULT_IMPL_WARN + #define NEO_GPS_PRINT_DEFAULT_IMPL_WARN +#endif diff --git a/src/platforms/Stream.h b/src/platforms/Stream.h new file mode 100644 index 0000000..3a64781 --- /dev/null +++ b/src/platforms/Stream.h @@ -0,0 +1,26 @@ +#pragma once + +#ifndef NEO_GPS_STREAM + #define NEO_GPS_STREAM NeoGPS::Stream + #ifdef __GNUC__ + #define NEO_GPS_STREAM_DEFAULT_IMPL_WARN \ + __attribute__((deprecated("You are using a function which uses NEO_GPS_STREAM without defining the type. Compilation will fail. To fix this #define NEO_GPS_STREAM your_stream_type_t before including platform.h"))) + #endif + +/** + * This file implements a non working dummy implementation for Stream. + **/ +namespace NeoGPS { + + class Stream { + public: + bool available() { return false; } + char read() { return '\0'; } + void print(char) {} + }; +} +#endif + +#ifndef NEO_GPS_STREAM_DEFAULT_IMPL_WARN + #define NEO_GPS_STREAM_DEFAULT_IMPL_WARN +#endif diff --git a/src/platforms/System.h b/src/platforms/System.h new file mode 100644 index 0000000..5f78d9f --- /dev/null +++ b/src/platforms/System.h @@ -0,0 +1,25 @@ +#pragma once + +#ifndef NEO_GPS_SYSTEM + #define NEO_GPS_SYSTEM NeoGPS::System + #ifdef __GNUC__ + #define NEO_GPS_SYSTEM_DEFAULT_IMPL_WARN \ + __attribute__((deprecated("You are using a function which uses NEO_GPS_STREAM without defining the type. Compilation will succeed, but the default implementation does not lock / unlock (enable / disable irqs). To fix this #define NEO_GPS_SYSTEM your_system_wrapper_class_t before including platform.h"))) + #endif + +/** + * This file implements a non working dummy implementation for interrupts and noInterrupts. + **/ +namespace NeoGPS { + + class System { + public: + static void lock() {} + static void unlock() {} + }; +} +#endif + +#ifndef NEO_GPS_SYSTEM_DEFAULT_IMPL_WARN + #define NEO_GPS_SYSTEM_DEFAULT_IMPL_WARN +#endif diff --git a/src/platforms/arduino/platform.h b/src/platforms/arduino/platform.h new file mode 100644 index 0000000..049bf08 --- /dev/null +++ b/src/platforms/arduino/platform.h @@ -0,0 +1,36 @@ +#pragma once + +#include + +#include +#include + +#define NEO_GPS_SYSTEM System + +#define NEO_GPS_STREAM Stream + +#define NEO_GPS_PRINT Print + +class System { +public: + static void lock() { noInterrupts(); } + static void unlock() { interrutps(); } +}; + +template +void printFloat( Print &outs, T f, uint8_t decimalPlaces) { outs.print(f, decimalPlaces); } + +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; } + diff --git a/src/platforms/linux/GpsPort.h b/src/platforms/linux/GpsPort.h new file mode 100644 index 0000000..540638d --- /dev/null +++ b/src/platforms/linux/GpsPort.h @@ -0,0 +1,4 @@ +#pragma once + +#include "GpsPort.header.h" +#include "GpsPort.impl.h" diff --git a/src/platforms/linux/GpsPort.header.h b/src/platforms/linux/GpsPort.header.h new file mode 100644 index 0000000..bce76d8 --- /dev/null +++ b/src/platforms/linux/GpsPort.header.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +class GpsPort { +private: + int _device; + +public: + GpsPort(const char* usbDev = "/dev/ttyUSB0", const char* speed = "9600"); + bool available(); + char read(); + void print(char); + +private: + int set_interface_attribs(int fd, int speed); +}; + diff --git a/src/platforms/linux/GpsPort.impl.h b/src/platforms/linux/GpsPort.impl.h new file mode 100644 index 0000000..0c3002d --- /dev/null +++ b/src/platforms/linux/GpsPort.impl.h @@ -0,0 +1,117 @@ +#pragma once + +#include "GpsPort.header.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +GpsPort::GpsPort(const char* usbDev, const char* speedTxt) +{ + if (usbDev == nullptr) { + usbDev = "/dev/ttyUSB0"; + } + + int fd = open(usbDev, O_RDWR | O_NOCTTY | O_SYNC); + if (fd < 0) { + printf("Error opening %s: %s\n", usbDev, strerror(errno)); + _device = -1; + } + + int speed; + switch(atoi(speedTxt == nullptr ? "9600" : speedTxt)) { + case 38400: + speed = B38400; + break; + case 19200: + speed = B19200; + break; + default: + case 9600: + speed = B9600; + break; + } + /*baudrate 9600, 8 bits, no parity, 1 stop bit */ + set_interface_attribs(fd, speed); + + _device = fd; +} + +bool GpsPort::available() +{ + fd_set rfds; + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 0; + + FD_ZERO( &rfds ); + FD_SET( _device, &rfds ); + + int retval = select( 1, &rfds, NULL, NULL, &tv ); + if (retval == -1) { + printf("Error select: %s\n", strerror(errno)); + } + return retval > 0; +} + +char GpsPort::read() +{ + char c; + int rdlen = ::read( _device, &c, 1 ); + if (rdlen > 0) { + return c; + } else if (rdlen < 0) { + printf("Error from read: %s\n", strerror(errno)); + } + return '\0'; +} + +void GpsPort::print(char c) +{ + /* simple output */ + int wlen = ::write(_device, &c, 1); + if (wlen != 1) { + printf("Error from write: %d, %d\n", wlen, errno); + } + tcdrain(_device); /* delay for output */ +} + +int GpsPort::set_interface_attribs(int fd, int speed) +{ + struct termios tty; + + if (tcgetattr(fd, &tty) < 0) { + printf("Error from tcgetattr: %s\n", strerror(errno)); + return -1; + } + + cfsetospeed(&tty, (speed_t)speed); + cfsetispeed(&tty, (speed_t)speed); + + tty.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ + tty.c_cflag &= ~CSIZE; + tty.c_cflag |= CS8; /* 8-bit characters */ + tty.c_cflag &= ~PARENB; /* no parity bit */ + tty.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ + tty.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ + + /* setup for non-canonical mode */ + tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); + tty.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + tty.c_oflag &= ~OPOST; + + /* fetch bytes as they become available */ + tty.c_cc[VMIN] = 1; + tty.c_cc[VTIME] = 1; + + if (tcsetattr(fd, TCSANOW, &tty) != 0) { + printf("Error from tcsetattr: %s\n", strerror(errno)); + return -1; + } + return 0; +} diff --git a/src/platforms/linux/platform.h b/src/platforms/linux/platform.h new file mode 100644 index 0000000..87bdbf8 --- /dev/null +++ b/src/platforms/linux/platform.h @@ -0,0 +1,32 @@ +#pragma once + +#ifndef NEO_GPS_PRINT + #include + #include + + #define NEO_GPS_PRINT decltype(std::cout) + + #ifdef USE_FLOAT + void printFloat( NEO_GPS_PRINT & io, float f, uint8_t decPlaces ) { + std::streamsize ss = std::cout.precision(); + io << std::setprecision(decPlaces) << std::fixed << f << std::setprecision(ss); + } + #endif +#endif + +#ifndef NEO_GPS_STREAM + #include "GpsPort.header.h" + #define NEO_GPS_STREAM GpsPort +#endif + +#ifndef NEO_GPS_SYSTEM + #define NEO_GPS_SYSTEM System + + class System { + public: + static void lock() {} + static void unlock() {} + }; +#endif + + diff --git a/src/ublox/ubxGPS.h b/src/ublox/ubxGPS.h index f052034..829b388 100644 --- a/src/ublox/ubxGPS.h +++ b/src/ublox/ubxGPS.h @@ -1,332 +1,4 @@ -#ifndef _UBXGPS_H_ -#define _UBXGPS_H_ +#pragma once -// 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 . - -#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 -#include - -// 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 +#include "ubxGPS.header.h" +#include "ubxGPS.impl.h" diff --git a/src/ublox/ubxGPS.header.h b/src/ublox/ubxGPS.header.h new file mode 100644 index 0000000..5be99b1 --- /dev/null +++ b/src/ublox/ubxGPS.header.h @@ -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 . + +#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 +#include + +// 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 diff --git a/src/ublox/ubxGPS.cpp b/src/ublox/ubxGPS.impl.h similarity index 97% rename from src/ublox/ubxGPS.cpp rename to src/ublox/ubxGPS.impl.h index f857ba8..c337399 100644 --- a/src/ublox/ubxGPS.cpp +++ b/src/ublox/ubxGPS.impl.h @@ -15,11 +15,15 @@ // You should have received a copy of the GNU General Public License // along with NeoGPS. If not, see . +// Just to be sure. This file should only be included in ubxGPS.h which +// protects against multiple includes already. +#pragma once + #include "NMEAGPS_cfg.h" // Disable the entire file if derived types are not allowed. #ifdef NMEAGPS_DERIVED_TYPES -#include "ublox/ubxGPS.h" +#include "ubxGPS.header.h" // Check configurations @@ -37,8 +41,6 @@ #endif -using namespace ublox; - //---------------------------------- void ubloxGPS::rxBegin() @@ -362,7 +364,7 @@ void ubloxGPS::write( const msg_t & msg ) sent.msg_class = msg.msg_class; sent.msg_id = msg.msg_id; -//trace << F("::write ") << msg.msg_class << F("/") << msg.msg_id << endl; +//trace << F("::write ") << msg.msg_class << F("/") << (int) msg.msg_id << endl; } //--------------------------------------------------------- @@ -407,7 +409,7 @@ void ubloxGPS::write_P( const msg_t & msg ) bool ubloxGPS::send( const msg_t & msg, msg_t *reply_msg ) { -//trace << F("::send - ") << (uint8_t) msg.msg_class << F(" ") << (uint8_t) msg.msg_id << F(" "); +//trace << F("::send - ") << (int) msg.msg_class << F(" ") << (int) msg.msg_id << F(" "); bool ok = true; write( msg ); @@ -465,7 +467,7 @@ bool ubloxGPS::parseField( char c ) switch (rx().msg_class) { case UBX_NAV: //================================================= -//if (chrCount == 0) Serial << F( " NAV ") << (uint8_t) rx().msg_id; +//if (chrCount == 0) Serial << F( " NAV ") << (int) rx().msg_id; switch (rx().msg_id) { case UBX_NAV_STATUS : return parseNavStatus ( chr ); case UBX_NAV_POSLLH : return parseNavPosLLH ( chr ); @@ -668,12 +670,12 @@ bool ubloxGPS::parseNavPosLLH( uint8_t chr ) if (chrCount == 19) { gps_fix::whole_frac *altp = &m_fix.alt; int32_t height_MSLmm = *((int32_t *)altp); -//trace << F(" alt = ") << height_MSLmm; +//trace << F(" alt = ") << +height_MSLmm; m_fix.alt.whole = height_MSLmm / 1000UL; m_fix.alt.frac = ((uint16_t)(height_MSLmm - (m_fix.alt.whole * 1000UL)))/10; -//trace << F(" = ") << m_fix.alt.whole << F("."); +//trace << F(" = ") << +m_fix.alt.whole << F("."); //if (m_fix.alt.frac < 10) trace << '0'; -//trace << m_fix.alt.frac; +//trace << +m_fix.alt.frac; m_fix.valid.altitude = true; } break; @@ -772,7 +774,7 @@ bool ubloxGPS::parseNavVelNED( uint8_t chr ) m_fix.spd.frac = (nmiph_E19 * 125) >> 16; m_fix.valid.speed = true; -//trace << m_fix.speed_mkn() << F(" nmi/h "); +//trace << +m_fix.speed_mkn() << F(" nmi/h "); } break; #endif @@ -786,14 +788,14 @@ bool ubloxGPS::parseNavVelNED( uint8_t chr ) gps_fix::whole_frac *hdgp = &m_fix.hdg; uint32_t ui = *((uint32_t *)hdgp); //trace << F("hdg "); -//trace << ui; +//trace << +ui; //trace << F("E-5, "); m_fix.hdg.whole = ui / 100000UL; ui -= ((uint32_t)m_fix.hdg.whole) * 100000UL; m_fix.hdg.frac = (ui/1000UL); // hundredths m_fix.valid.heading = true; -//trace << m_fix.heading_cd() << F("E-2 "); +//trace << +m_fix.heading_cd() << F("E-2 "); } break; #endif @@ -827,7 +829,7 @@ bool ubloxGPS::parseNavTimeGPS( uint8_t chr ) *((ublox::nav_timegps_t::valid_t *) &chr); if (!v.leap_seconds) GPSTime::leap_seconds = 0; // oops! -//else trace << F("leap ") << GPSTime::leap_seconds << ' '; +//else trace << F("leap ") << +GPSTime::leap_seconds << ' '; if (GPSTime::leap_seconds != 0) { if (!v.time_of_week) { m_fix.valid.date = @@ -835,7 +837,7 @@ bool ubloxGPS::parseNavTimeGPS( uint8_t chr ) } else if ((GPSTime::start_of_week() == 0) && m_fix.valid.date && m_fix.valid.time) { GPSTime::start_of_week( m_fix.dateTime ); -//trace << m_fix.dateTime << '.' << m_fix.dateTime_cs; +//trace << m_fix.dateTime << '.' << +m_fix.dateTime_cs; } } } @@ -894,7 +896,7 @@ bool ubloxGPS::parseNavTimeUTC( uint8_t chr ) (GPSTime::leap_seconds != 0)) GPSTime::start_of_week( m_fix.dateTime ); #endif -//trace << m_fix.dateTime << F(".") << m_fix.dateTime_cs; +//trace << m_fix.dateTime << F(".") << +m_fix.dateTime_cs; //trace << ' ' << v.UTC << ' ' << v.time_of_week << ' ' << start_of_week(); } break; diff --git a/src/ublox/ubxNMEA.h b/src/ublox/ubxNMEA.h index 5ce707e..b436993 100644 --- a/src/ublox/ubxNMEA.h +++ b/src/ublox/ubxNMEA.h @@ -1,110 +1,4 @@ -#ifndef _UBXNMEA_H_ -#define _UBXNMEA_H_ +#pragma once -// 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 . - -#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 +#include "ubxNMEA.header.h" +#include "ubxNMEA.impl.h" diff --git a/src/ublox/ubxNMEA.header.h b/src/ublox/ubxNMEA.header.h new file mode 100644 index 0000000..5ce707e --- /dev/null +++ b/src/ublox/ubxNMEA.header.h @@ -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 . + +#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 diff --git a/src/ublox/ubxNMEA.cpp b/src/ublox/ubxNMEA.impl.h similarity index 95% rename from src/ublox/ubxNMEA.cpp rename to src/ublox/ubxNMEA.impl.h index 4858a3c..46ded86 100644 --- a/src/ublox/ubxNMEA.cpp +++ b/src/ublox/ubxNMEA.impl.h @@ -15,7 +15,11 @@ // You should have received a copy of the GNU General Public License // along with NeoGPS. If not, see . -#include "ublox/ubxNMEA.h" +// Just to be sure. This file should only be included in ubxNMEA.h which +// protects against multiple includes already. +#pragma once + +#include "ubxNMEA.header.h" // Disable the entire file if derived types are not allowed. #ifdef NMEAGPS_DERIVED_TYPES @@ -144,4 +148,4 @@ bool ubloxNMEA::parseFix( char chr ) return true; } -#endif \ No newline at end of file +#endif diff --git a/src/ublox/ubxmsg.h b/src/ublox/ubxmsg.h index 57cda43..36d964d 100644 --- a/src/ublox/ubxmsg.h +++ b/src/ublox/ubxmsg.h @@ -1,589 +1,4 @@ -#ifndef UBXMSG_H -#define UBXMSG_H +#pragma once -// 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 . - -#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 \ No newline at end of file +#include "ubxmsg.header.h" +#include "ubxmsg.impl.h" diff --git a/src/ublox/ubxmsg.header.h b/src/ublox/ubxmsg.header.h new file mode 100644 index 0000000..8e49f6e --- /dev/null +++ b/src/ublox/ubxmsg.header.h @@ -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 . + +#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 diff --git a/src/ublox/ubxmsg.cpp b/src/ublox/ubxmsg.impl.h similarity index 92% rename from src/ublox/ubxmsg.cpp rename to src/ublox/ubxmsg.impl.h index 2ac4f1a..62dfa6d 100644 --- a/src/ublox/ubxmsg.cpp +++ b/src/ublox/ubxmsg.impl.h @@ -15,7 +15,11 @@ // You should have received a copy of the GNU General Public License // along with NeoGPS. If not, see . -#include "ublox/ubxGPS.h" +// Just to be sure. This file should only be included in ubxmsg.h which +// protects against multiple includes already. +#pragma once + +#include "ubxGPS.header.h" // Disable the entire file if derived types are not allowed. #ifdef NMEAGPS_DERIVED_TYPES @@ -68,4 +72,4 @@ bool ublox::configNMEA( ubloxGPS &gps, NMEAGPS::nmea_msg_t msgType, uint8_t rate return gps.send( cfg_msg_t( UBX_NMEA, msg_id, rate ) ); } -#endif \ No newline at end of file +#endif diff --git a/tests/FakeGPS/FakeContents.h b/tests/FakeGPS/FakeContents.h new file mode 100644 index 0000000..c45b427 --- /dev/null +++ b/tests/FakeGPS/FakeContents.h @@ -0,0 +1,3 @@ +#pragma once + +#include "fake_contents/FakeContent.internet_sample.h" diff --git a/tests/FakeGPS/FakeGPS.h b/tests/FakeGPS/FakeGPS.h new file mode 100644 index 0000000..02da3e4 --- /dev/null +++ b/tests/FakeGPS/FakeGPS.h @@ -0,0 +1,5 @@ +#pragma once + +#include "FakeContents.h" +#include "FakeGPS.header.h" +#include "FakeGPS.impl.h" diff --git a/tests/FakeGPS/FakeGPS.header.h b/tests/FakeGPS/FakeGPS.header.h new file mode 100644 index 0000000..7403f4f --- /dev/null +++ b/tests/FakeGPS/FakeGPS.header.h @@ -0,0 +1,17 @@ +#pragma once + +#include + +class FakeGPS { +private: + const char* _fakeContent; + bool _repeat; + struct timeval _next_char_tv; + +public: + FakeGPS(const char* fakeOutput, bool repeat); + bool available(); + char read(); + void print(char); +}; + diff --git a/tests/FakeGPS/FakeGPS.impl.h b/tests/FakeGPS/FakeGPS.impl.h new file mode 100644 index 0000000..6957e3c --- /dev/null +++ b/tests/FakeGPS/FakeGPS.impl.h @@ -0,0 +1,50 @@ +#pragma once + +#include "FakeGPS.header.h" + +#include +#include + + +FakeGPS::FakeGPS(const char* fakeContent, bool repeat) + : _fakeContent(fakeContent), _repeat(repeat) { + gettimeofday(&_next_char_tv, NULL); + } + +bool FakeGPS::available() { + struct timeval now; + gettimeofday(&now, NULL); + return _next_char_tv.tv_sec > 0 && + (_next_char_tv.tv_sec < now.tv_sec || + (_next_char_tv.tv_sec == now.tv_sec && + _next_char_tv.tv_usec <= now.tv_usec)); +} + + +char FakeGPS::read() { + static int runner = 0; + static int len = strlen(_fakeContent); + + char c = _fakeContent[runner]; + runner = (runner + 1) % len; + + if (c == '\n') { + if (!_repeat && runner == 0) { + _next_char_tv.tv_sec = 0; + } else { + _next_char_tv.tv_usec += 100000; + + if (_next_char_tv.tv_usec > 1000000) { + _next_char_tv.tv_usec -= 1000000; + ++_next_char_tv.tv_sec; + } + } + } + + return c; +} + +void FakeGPS::print(char c) { + (void) c; + return; +} diff --git a/tests/FakeGPS/fake_contents/FakeContent.internet_sample.h b/tests/FakeGPS/fake_contents/FakeContent.internet_sample.h new file mode 100644 index 0000000..3075b58 --- /dev/null +++ b/tests/FakeGPS/fake_contents/FakeContent.internet_sample.h @@ -0,0 +1,17 @@ +#pragma once + +namespace fake_gps_content { + + static const char * INTERNET_SAMPLE = + "$GPRMC,162254.00,A,3723.02837,N,12159.39853,W,0.820,188.36,110706,,,A*74\r\n" + "$GPVTG,188.36,T,,M,0.820,N,1.519,K,A*3F\r\n" + "$GPGGA,162254.00,3723.02837,N,12159.39853,W,1,03,2.36,525.6,M,-25.6,M,,*65\r\n" + "$GPGSA,A,2,25,01,22,,,,,,,,,,2.56,2.36,1.00*02\r\n" + "$GPGSV,4,1,14,25,15,175,30,14,80,041,,19,38,259,14,01,52,223,18*76\r\n" + "$GPGSV,4,2,14,18,16,079,,11,19,312,,14,80,041,,21,04,135,25*7D\r\n" + "$GPGSV,4,3,14,15,27,134,18,03,25,222,,22,51,057,16,09,07,036,*79\r\n" + "$GPGSV,4,4,14,07,01,181,,15,25,135,*76\r\n" + "$GPGLL,3723.02837,N,12159.39853,W,162254.00,A,A*7C\r\n" + "$GPZDA,162254.00,11,07,2006,00,00*63\r\n"; + +} diff --git a/tests/FakeGPS/platform.h b/tests/FakeGPS/platform.h new file mode 100644 index 0000000..60ebd71 --- /dev/null +++ b/tests/FakeGPS/platform.h @@ -0,0 +1,32 @@ +#pragma once + +#ifndef NEO_GPS_PRINT + #include + #include + + #define NEO_GPS_PRINT decltype(std::cout) + + #ifdef USE_FLOAT + void printFloat( NEO_GPS_PRINT & io, float f, uint8_t decPlaces ) { + std::streamsize ss = std::cout.precision(); + io << std::setprecision(decPlaces) << std::fixed << f << std::setprecision(ss); + } + #endif +#endif + +#ifndef NEO_GPS_STREAM + #include "FakeGPS.header.h" + #define NEO_GPS_STREAM FakeGPS +#endif + +#ifndef NEO_GPS_SYSTEM + #define NEO_GPS_SYSTEM System + + class System { + public: + static void lock() {} + static void unlock() {} + }; +#endif + + diff --git a/tests/NMEA/NMEA.cpp b/tests/NMEA/NMEA.cpp new file mode 100644 index 0000000..ff92b3f --- /dev/null +++ b/tests/NMEA/NMEA.cpp @@ -0,0 +1,132 @@ +//====================================================================== +// Description: This program uses the fix-oriented methods available() and +// read() to handle complete fix structures. +// +// When the last character of the LAST_SENTENCE_IN_INTERVAL (see NMEAGPS_cfg.h) +// is decoded, a completed fix structure becomes available and is returned +// from read(). The new fix is saved the 'fix' structure, and can be used +// anywhere, at any time. +// +// If no messages are enabled in NMEAGPS_cfg.h, or +// no 'gps_fix' members are enabled in GPSfix_cfg.h, no information will be +// parsed, copied or printed. +// +//====================================================================== + +//------------------------------------------------------------ +// For the NeoGPS example programs, "Streamers" is common set +// of printing and formatting routines for GPS data, in a +// Comma-Separated Values text format (aka CSV). The CSV +// data will be printed to the "debug output device". +// If you don't need these formatters, simply delete this section. + +// platform.h must be included before NMEAGPS.h +#include + +// We fake the GPS using FakeGPS. +#include + +// platform.h defines delctype(std::cout) as the NEO_GPS_PRINT type. +// We can therefore pass std::cout to all functions which expect a +// NEO_GPS_PRINT object. +#include + +#include + +#include + +//------------------------------------------------------------ +// This object parses received characters +// into the gps.fix() data structure + +static NMEAGPS gps; + +//------------------------------------------------------------ +// Define a set of GPS fix information. It will +// hold on to the various pieces as they are received from +// an RMC sentence. It can be used anywhere in your sketch. + +static gps_fix fix; + +//---------------------------------------------------------------- +// This function gets called about once per second, during the GPS +// quiet time. It's the best place to do anything that might take +// a while: print a bunch of things, write to SD, send an SMS, etc. +// +// By doing the "hard" work during the quiet time, the CPU can get back to +// reading the GPS chars as they come in, so that no chars are lost. + +static auto & DEBUG_PORT = std::cout; + +static void doSomeWork() +{ + // Print all the things! + + trace_all( DEBUG_PORT, gps, fix ); + +} // doSomeWork + +//------------------------------------ +// This is the main GPS parsing loop. + +static void GPSloop(FakeGPS & fakeGPS) +{ + while (gps.available( fakeGPS )) { + fix = gps.read(); + doSomeWork(); + } + +} // GPSloop + +//-------------------------- + +void outputHeader() +{ + DEBUG_PORT << "NMEA.cpp: started" << std::endl; + DEBUG_PORT << " fix object size = " << +sizeof(gps.fix()) << std::endl; + DEBUG_PORT << " gps object size = " << +sizeof(gps) << std::endl; + + #ifndef NMEAGPS_RECOGNIZE_ALL + #error You must define NMEAGPS_RECOGNIZE_ALL in NMEAGPS_cfg.h! + #endif + + #ifdef NMEAGPS_INTERRUPT_PROCESSING + #error You must *NOT* define NMEAGPS_INTERRUPT_PROCESSING in NMEAGPS_cfg.h! + #endif + + #if !defined( NMEAGPS_PARSE_GGA ) && !defined( NMEAGPS_PARSE_GLL ) && \ + !defined( NMEAGPS_PARSE_GSA ) && !defined( NMEAGPS_PARSE_GSV ) && \ + !defined( NMEAGPS_PARSE_RMC ) && !defined( NMEAGPS_PARSE_VTG ) && \ + !defined( NMEAGPS_PARSE_ZDA ) && !defined( NMEAGPS_PARSE_GST ) + + DEBUG_PORT << std::endl << "WARNING: No NMEA sentences are enabled: no fix data will be displayed."; + + #else + if (gps.merging == NMEAGPS::NO_MERGING) { + DEBUG_PORT << std::endl; + DEBUG_PORT << "WARNING: displaying data from " << gps.string_for( LAST_SENTENCE_IN_INTERVAL ) + << " sentences ONLY, and only if " << gps.string_for( LAST_SENTENCE_IN_INTERVAL ) + << " is enabled." << std::endl; + DEBUG_PORT << " Other sentences may be parsed, but their data will not be displayed."; + } + #endif + + DEBUG_PORT << std::endl << "GPS quiet time is assumed to begin after a " + << gps.string_for( LAST_SENTENCE_IN_INTERVAL ) + << " sentence is received." << std::endl; + DEBUG_PORT << " You should confirm this with NMEAorder.cpp" << std::endl << std::endl; + + trace_header( DEBUG_PORT ); +} + +//-------------------------- + + +int main() { + outputHeader(); + auto fakeGPS = FakeGPS(fake_gps_content::INTERNET_SAMPLE, true); + for (;;) { + GPSloop(fakeGPS); + } + return 0; +} diff --git a/tests/NMEA/build.dummy.txt b/tests/NMEA/build.dummy.txt new file mode 100644 index 0000000..5d1560e --- /dev/null +++ b/tests/NMEA/build.dummy.txt @@ -0,0 +1 @@ +g++ -flto -g -Wall ../../../src/DMS.cpp ../../../src/GPSTime.cpp ../../../src/Location.cpp ../../../src/NeoTime.cpp ../../../src/NMEAGPS.cpp ../GpsPort.dummy.cpp ../Print.cpp NMEA.cpp ../serial.cpp -I ../ -I ../../../src diff --git a/tests/NMEA/build.txt b/tests/NMEA/build.txt new file mode 100644 index 0000000..aebc237 --- /dev/null +++ b/tests/NMEA/build.txt @@ -0,0 +1 @@ +g++ -flto -Wall ../../../src/DMS.cpp ../../../src/GPSTime.cpp ../../../src/Location.cpp ../../../src/NeoTime.cpp ../../../src/NMEAGPS.cpp ../GpsPort.cpp ../Print.cpp NMEA.cpp ../serial.cpp -I ../ -I ../../../src