From 4a98bdd9d65590952f7ec82d61c27ce3694034f1 Mon Sep 17 00:00:00 2001
From: Sam <35611307+syund@users.noreply.github.com>
Date: Sat, 9 Oct 2021 13:31:27 -0400
Subject: [PATCH] Move bearing under GeoCoord

---
 src/gps/GeoCoord.cpp    | 25 +++++++++++++++++++++++++
 src/gps/GeoCoord.h      |  1 +
 src/graphics/Screen.cpp | 29 ++---------------------------
 3 files changed, 28 insertions(+), 27 deletions(-)

diff --git a/src/gps/GeoCoord.cpp b/src/gps/GeoCoord.cpp
index 96c27bdf8..c3bac700a 100644
--- a/src/gps/GeoCoord.cpp
+++ b/src/gps/GeoCoord.cpp
@@ -365,4 +365,29 @@ float GeoCoord::latLongToMeter(double lat_a, double lng_a, double lat_b, double
         tt = 0.0; // Must have been the same point?
 
     return (float)(6366000 * tt);
+}
+
+/**
+ * Computes the bearing in degrees between two points on Earth.  Ported from my
+ * old Gaggle android app.
+ *
+ * @param lat1
+ * Latitude of the first point
+ * @param lon1
+ * Longitude of the first point
+ * @param lat2
+ * Latitude of the second point
+ * @param lon2
+ * Longitude of the second point
+ * @return Bearing between the two points in radians. A value of 0 means due
+ * north.
+ */
+float GeoCoord::bearing(double lat1, double lon1, double lat2, double lon2)
+{
+    double lat1Rad = toRadians(lat1);
+    double lat2Rad = toRadians(lat2);
+    double deltaLonRad = toRadians(lon2 - lon1);
+    double y = sin(deltaLonRad) * cos(lat2Rad);
+    double x = cos(lat1Rad) * sin(lat2Rad) - (sin(lat1Rad) * cos(lat2Rad) * cos(deltaLonRad));
+    return atan2(y, x);
 }
\ No newline at end of file
diff --git a/src/gps/GeoCoord.h b/src/gps/GeoCoord.h
index 236d66a9f..7a2c1aa9d 100644
--- a/src/gps/GeoCoord.h
+++ b/src/gps/GeoCoord.h
@@ -113,6 +113,7 @@ class GeoCoord {
         static void latLongToOLC(const double lat, const double lon, OLC &olc);
         static void convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude);
         static float latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b);
+        static float bearing(double lat1, double lon1, double lat2, double lon2);
 
         // Lat lon alt getters
         int32_t getLatitude() const { return _latitude; }
diff --git a/src/graphics/Screen.cpp b/src/graphics/Screen.cpp
index 8a7997a09..d197f7765 100644
--- a/src/graphics/Screen.cpp
+++ b/src/graphics/Screen.cpp
@@ -437,31 +437,6 @@ static void drawGPScoordinates(OLEDDisplay *display, int16_t x, int16_t y, const
     }
 }
 
-/**
- * Computes the bearing in degrees between two points on Earth.  Ported from my
- * old Gaggle android app.
- *
- * @param lat1
- * Latitude of the first point
- * @param lon1
- * Longitude of the first point
- * @param lat2
- * Latitude of the second point
- * @param lon2
- * Longitude of the second point
- * @return Bearing between the two points in radians. A value of 0 means due
- * north.
- */
-static float bearing(double lat1, double lon1, double lat2, double lon2)
-{
-    double lat1Rad = toRadians(lat1);
-    double lat2Rad = toRadians(lat2);
-    double deltaLonRad = toRadians(lon2 - lon1);
-    double y = sin(deltaLonRad) * cos(lat2Rad);
-    double x = cos(lat1Rad) * sin(lat2Rad) - (sin(lat1Rad) * cos(lat2Rad) * cos(deltaLonRad));
-    return atan2(y, x);
-}
-
 namespace
 {
 
@@ -526,7 +501,7 @@ static float estimatedHeading(double lat, double lon)
     if (d < 10) // haven't moved enough, just keep current bearing
         return b;
 
-    b = bearing(oldLat, oldLon, lat, lon);
+    b = GeoCoord::bearing(oldLat, oldLon, lat, lon);
     oldLat = lat;
     oldLon = lon;
 
@@ -654,7 +629,7 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_
 
             // FIXME, also keep the guess at the operators heading and add/substract
             // it.  currently we don't do this and instead draw north up only.
-            float bearingToOther = bearing(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
+            float bearingToOther = GeoCoord::bearing(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
             headingRadian = bearingToOther - myHeading;
             drawNodeHeading(display, compassX, compassY, headingRadian);
         }