Agra-GPS / FreePilot_V2-3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FreePilot PinDetect mbed-src

Fork of FreePilot_V2-2 by Agra-GPS

Diff: gps.h

Revision:
57:0299098b2d0e
Parent:
56:456d454d9ced
--- a/gps.h	Sun Mar 29 16:03:18 2015 +0000
+++ b/gps.h	Wed Apr 01 01:19:30 2015 +0000
@@ -292,4 +292,25 @@
     } else {
         return false;
     }
-}
\ No newline at end of file
+}
+
+ double CalculateHeading(double lat1, double long1, double lat2, double long2)
+{
+    double a = lat1 * PI / 180;
+    double b = long1 * PI / 180;
+    double c = lat2 * PI / 180;
+    double d = long2 * PI / 180;
+
+    if (cos(c) * sin(d - b) == 0)
+        if (c > a)
+            return 0;
+        else
+            return 180;
+    else
+    {
+        double angle = atan2(cos(c) * sin(d - b), sin(c) * cos(a) - sin(a) * cos(c) * cos(d - b));
+        return angle * 180 / PI;
+
+    }
+}
+