#include //====================================================================== // Program: NMEAaverage.ino // // Description: This program averages locations over time to compute // a higher-accuracy *static* location. It also shows // how to use the distance functions in the Location class. // // Prerequisites: // 1) NMEA.ino works with your device (correct TX/RX pins and baud rate) // 2) At least once sentence with a location field has been enabled // // 'Serial' is for debug output to the Serial Monitor window. // // License: // 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 //------------------------------------------------------------ // Check that the config files are set up properly #if !defined( NMEAGPS_PARSE_GGA ) && \ !defined( NMEAGPS_PARSE_GLL ) && \ !defined( NMEAGPS_PARSE_RMC ) #error You must uncomment at least one of NMEAGPS_PARSE_GGA, GGL or RMC in NMEAGPS_cfg.h! #endif #if !defined( GPS_FIX_LOCATION ) #error You must uncomment GPS_FIX_LOCATION in GPSfix_cfg.h! #endif #if !defined( GPS_FIX_TIME ) #error You must uncomment GPS_FIX_TIME in GPSfix_cfg.h! #endif #if !defined( GPS_FIX_DATE ) #error You must uncomment GPS_FIX_DATE in GPSfix_cfg.h! #endif //------------------------------------------------------------ static NMEAGPS gps; // This parses the GPS characters static gps_fix fix; // This holds the latest GPS fix //------------------------------------------------------------ using namespace NeoGPS; static gps_fix first; // good GPS data static clock_t firstSecs; // cached dateTime in seconds since EPOCH static Location_t avgLoc; // gradually-calculated average location static uint16_t count; // number of samples static int32_t sumDLat, sumDLon; // accumulated deltas static bool doneAccumulating; // accumulation completed //------------------------------------------------------------ const char nCD [] PROGMEM = "N"; const char nneCD[] PROGMEM = "NNE"; const char neCD [] PROGMEM = "NE"; const char eneCD[] PROGMEM = "ENE"; const char eCD [] PROGMEM = "E"; const char eseCD[] PROGMEM = "ESE"; const char seCD [] PROGMEM = "SE"; const char sseCD[] PROGMEM = "SSE"; const char sCD [] PROGMEM = "S"; const char sswCD[] PROGMEM = "SSW"; const char swCD [] PROGMEM = "SW"; const char wswCD[] PROGMEM = "WSW"; const char wCD [] PROGMEM = "W"; const char wnwCD[] PROGMEM = "WNW"; const char nwCD [] PROGMEM = "NW"; const char nnwCD[] PROGMEM = "NNW"; const char * const dirStrings[] PROGMEM = { nCD, nneCD, neCD, eneCD, eCD, eseCD, seCD, sseCD, sCD, sswCD, swCD, wswCD, wCD, wnwCD, nwCD, nnwCD }; const __FlashStringHelper *compassDir( uint16_t bearing ) // degrees CW from N { const int16_t directions = sizeof(dirStrings)/sizeof(dirStrings[0]); const int16_t degreesPerDir = 360 / directions; int8_t dir = (bearing + degreesPerDir/2) / degreesPerDir; while (dir < 0) dir += directions; while (dir >= directions) dir -= directions; return (const __FlashStringHelper *) pgm_read_ptr( &dirStrings[ dir ] ); } // compassDir //------------------------------------------------------------ static void doSomeWork() { static bool warned = false; // that we're waiting for a valid location if (fix.valid.location && fix.valid.date && fix.valid.time) { if (count == 0) { // Just save the first good fix first = fix; firstSecs = (clock_t) first.dateTime; count = 1; } else { // After the first fix, accumulate locations until we have // a good average. Then display the offset from the average. if (warned) { // We were waiting for the fix to be re-acquired. warned = false; DEBUG_PORT.println(); } DEBUG_PORT.print( count ); if (!doneAccumulating) { // Enough time? if (((clock_t)fix.dateTime - firstSecs) > 2 * SECONDS_PER_HOUR) doneAccumulating = true; } int32_t dLat, dLon; if (!doneAccumulating) { // Use deltas from the first location dLat = fix.location.lat() - first.location.lat(); sumDLat += dLat; int32_t avgDLat = sumDLat / count; dLon = fix.location.lon() - first.location.lon(); sumDLon += dLon; int32_t avgDLon = sumDLon / count; // Then calculated the average location as the first location // plus the averaged deltas. avgLoc.lat( first.location.lat() + avgDLat ); avgLoc.lon( first.location.lon() + avgDLon ); count++; } DEBUG_PORT.print( ',' ); DEBUG_PORT.print( avgLoc.lat() ); DEBUG_PORT.print( ',' ); DEBUG_PORT.print( avgLoc.lon() ); DEBUG_PORT.print( ',' ); dLat = avgLoc.lat() - fix.location.lat(); DEBUG_PORT.print( dLat ); DEBUG_PORT.print( ',' ); dLon = avgLoc.lon() - fix.location.lon(); DEBUG_PORT.print( dLon ); // Calculate the distance from the current fix to the average location float avgDistError = avgLoc.DistanceKm( fix.location ); DEBUG_PORT.print( ',' ); DEBUG_PORT.print( avgDistError * 100000.0 ); // cm // Calculate the bearing from the current fix to the average location. // NOTE: other libraries will have trouble with this calculation, // because these coordinates are *VERY* close together. Naive // floating-point calculations will not have enough significant // digits. float avgBearingErr = fix.location.BearingTo( avgLoc ); float bearing = avgBearingErr * Location_t::DEG_PER_RAD; DEBUG_PORT.print( ',' ); DEBUG_PORT.print( bearing, 6 ); DEBUG_PORT.print( ',' ); DEBUG_PORT.print( compassDir( bearing ) ); // Calculate a point that is 10km away from the average location, // at the error bearing Location_t tenKmAway( avgLoc ); tenKmAway.OffsetBy( 10.0 / Location_t::EARTH_RADIUS_KM, avgBearingErr ); DEBUG_PORT.print( ',' ); DEBUG_PORT.print( tenKmAway.lat() ); DEBUG_PORT.print( ',' ); DEBUG_PORT.print( tenKmAway.lon() ); // Calculate the bearing from the average location to that point. // This should be very close to the avgBearingErr, and will // reflect the calculation error. This is because the // current fix is *VERY* close to the average location. float tb = avgLoc.BearingToDegrees( tenKmAway ); DEBUG_PORT.print( ',' ); DEBUG_PORT.print( tb, 6 ); DEBUG_PORT.println(); } } else { if (!warned) { warned = true; DEBUG_PORT.print( F("Waiting for fix...") ); } else { DEBUG_PORT.print( '.' ); } } } // doSomeWork //------------------------------------------------------------ static void GPSloop() { while (gps.available( gpsPort )) { fix = gps.read(); doSomeWork(); } } // GPSloop //------------------------------------------------------------ void setup() { DEBUG_PORT.begin(9600); while (!DEBUG_PORT) ; DEBUG_PORT.print( F("NMEAaverage.INO: started\n") ); DEBUG_PORT.print( F(" fix object size = ") ); DEBUG_PORT.println( sizeof(gps.fix()) ); DEBUG_PORT.print( F(" gps object size = ") ); DEBUG_PORT.println( sizeof(gps) ); DEBUG_PORT.println( F("Looking for GPS device on " GPS_PORT_NAME) ); DEBUG_PORT.println( F("Comparing current fix with averaged location.\n" "count,avg lat,avg lon,dlat,dlon,distance(cm)," "bearing deg,compass,lat/lon 10km away & recalc bearing") ); DEBUG_PORT.flush(); gpsPort.begin( 9600 ); } //------------------------------------------------------------ void loop() { GPSloop(); }