diff options
Diffstat (limited to 'libraries/TinyGPS')
-rw-r--r-- | libraries/TinyGPS/TinyGPS.cpp | 19 | ||||
-rw-r--r-- | libraries/TinyGPS/TinyGPS.h | 5 |
2 files changed, 24 insertions, 0 deletions
diff --git a/libraries/TinyGPS/TinyGPS.cpp b/libraries/TinyGPS/TinyGPS.cpp index d4ee60f..db0b49a 100644 --- a/libraries/TinyGPS/TinyGPS.cpp +++ b/libraries/TinyGPS/TinyGPS.cpp @@ -301,3 +301,22 @@ float TinyGPS::distance_between (float lat1, float long1, float lat2, float long delta = atan2(delta, denom);
return delta * 6372795;
}
+
+/* static */
+float TinyGPS::bearing_between (float lat1, float long1, float lat2, float long2)
+{
+ float delta = radians(long1-long2);
+ float sdlong = sin(delta);
+ float cdlong = cos(delta);
+ lat1 = radians(lat1);
+ lat2 = radians(lat2);
+ float slat1 = sin(lat1);
+ float clat1 = cos(lat1);
+ float slat2 = sin(lat2);
+ float clat2 = cos(lat2);
+ float y = sdlong * clat1;
+ float x = clat2*slat1 - slat2*clat1*cdlong;
+ // Yeah, the mod operation should look funny.
+ // I'm implicitly swapping the order of the arguments...
+ return (int) (degrees(atan2(y, x)) + 180) % 360;
+}
diff --git a/libraries/TinyGPS/TinyGPS.h b/libraries/TinyGPS/TinyGPS.h index 9c75ad0..1ffdbdd 100644 --- a/libraries/TinyGPS/TinyGPS.h +++ b/libraries/TinyGPS/TinyGPS.h @@ -111,6 +111,11 @@ class TinyGPS static float distance_between (float lat1, float long1, float lat2, float long2);
+ /**
+ * Return the bearing in degrees from lat1/long1 to lat2/long2.
+ */
+ static float bearing_between (float lat1, float long1, float lat2, float long2);
+
private:
enum {_GPS_SENTENCE_GPGGA, _GPS_SENTENCE_GPRMC, _GPS_SENTENCE_OTHER};
|