@@ -5,17 +5,14 @@ Suggestion to add satellites, courseTo(), and cardinal() by Matt Monson.
55Location precision improvements suggested by Wayne Holder.
66Copyright (C) 2008-2013 Mikal Hart
77All rights reserved.
8-
98This library is free software; you can redistribute it and/or
109modify it under the terms of the GNU Lesser General Public
1110License as published by the Free Software Foundation; either
1211version 2.1 of the License, or (at your option) any later version.
13-
1412This library is distributed in the hope that it will be useful,
1513but WITHOUT ANY WARRANTY; without even the implied warranty of
1614MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
1715Lesser General Public License for more details.
18-
1916You should have received a copy of the GNU Lesser General Public
2017License along with this library; if not, write to the Free Software
2118Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
@@ -26,6 +23,7 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
2623#include < string.h>
2724#include < ctype.h>
2825#include < stdlib.h>
26+
2927#define _GPRMCterm " GPRMC"
3028#define _GPGGAterm " GPGGA"
3129#define _GNRMCterm " GNRMC"
@@ -216,7 +214,8 @@ bool TinyGPSPlus::endOfTermHandler()
216214 curSentenceType = GPS_SENTENCE_OTHER;
217215
218216 // Any custom candidates of this sentence type?
219- for (customCandidates = customElts; customCandidates != NULL && strcmp (customCandidates->sentenceName , term) < 0 ; customCandidates = customCandidates->next );
217+ for (customCandidates = customElts; customCandidates != NULL && strcmp (customCandidates->sentenceName , term) < 0 ; customCandidates =
218+ customCandidates->next );
220219 if (customCandidates != NULL && strcmp (customCandidates->sentenceName , term) > 0 )
221220 customCandidates = NULL ;
222221
@@ -273,13 +272,59 @@ bool TinyGPSPlus::endOfTermHandler()
273272 }
274273
275274 // Set custom values as needed
276- for (TinyGPSCustom *p = customCandidates; p != NULL && strcmp (p->sentenceName , customCandidates->sentenceName ) == 0 && p->termNumber <= curTermNumber; p = p->next )
275+ for (TinyGPSCustom *p = customCandidates; p != NULL && strcmp (p->sentenceName , customCandidates->sentenceName ) == 0 && p->termNumber <=
276+ curTermNumber; p = p->next )
277277 if (p->termNumber == curTermNumber)
278278 p->set (term);
279279
280280 return false ;
281281}
282282
283+ /* static */
284+ double TinyGPSPlus::distanceBetween (double lat1, double long1, double lat2, double long2)
285+ {
286+ // returns distance in meters between two positions, both specified
287+ // as signed decimal-degrees latitude and longitude. Uses great-circle
288+ // distance computation for hypothetical sphere of radius 6372795 meters.
289+ // Because Earth is no exact sphere, rounding errors may be up to 0.5%.
290+ // Courtesy of Maarten Lamers
291+ double delta = radians (long1-long2);
292+ double sdlong = sin (delta);
293+ double cdlong = cos (delta);
294+ lat1 = radians (lat1);
295+ lat2 = radians (lat2);
296+ double slat1 = sin (lat1);
297+ double clat1 = cos (lat1);
298+ double slat2 = sin (lat2);
299+ double clat2 = cos (lat2);
300+ delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
301+ delta = sq (delta);
302+ delta += sq (clat2 * sdlong);
303+ delta = sqrt (delta);
304+ double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
305+ delta = atan2 (delta, denom);
306+ return delta * 6372795 ;
307+ }
308+
309+ double TinyGPSPlus::courseTo (double lat1, double long1, double lat2, double long2)
310+ {
311+ // returns course in degrees (North=0, West=270) from position 1 to position 2,
312+ // both specified as signed decimal-degrees latitude and longitude.
313+ // Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
314+ // Courtesy of Maarten Lamers
315+ double dlon = radians (long2-long1);
316+ lat1 = radians (lat1);
317+ lat2 = radians (lat2);
318+ double a1 = sin (dlon) * cos (lat2);
319+ double a2 = sin (lat1) * cos (lat2) * cos (dlon);
320+ a2 = cos (lat1) * sin (lat2) - a2;
321+ a2 = atan2 (a1, a2);
322+ if (a2 < 0.0 )
323+ {
324+ a2 += TWO_PI;
325+ }
326+ return degrees (a2);
327+ }
283328
284329const char *TinyGPSPlus::cardinal (double course)
285330{
@@ -306,7 +351,6 @@ void TinyGPSLocation::setLongitude(const char *term)
306351 TinyGPSPlus::parseDegrees (term, rawNewLngData);
307352}
308353
309-
310354double TinyGPSLocation::lat ()
311355{
312356 updated = false ;
0 commit comments