Adding bearing method to TinyGPS.

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};