bearing

minimap.py

@@ -104,6 +104,34 @@ def haversine(coord1, coord2):
     
     return c * r
 
+def haver_dist(coord1, coord2):
+    lati1 = math.radians(coord1[0])
+    long1 = math.radians(coord1[1])
+    lati2 = math.radians(coord2[0])
+    long2 = math.radians(coord2[1])
+    dlong = long2 - long1
+    dlati = lati2 - lati1
+    a = (
+        math.sin(dlati / 2) ** 2
+        + math.cos(lati1) * math.cos(lati2) * math.sin(dlon / 2) ** 2
+    )
+    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+    distance = R * c
+    return distance*1000
+
+def bearing(coord1, coord2):
+    lati1 = math.radians(coord1[0])
+    long1 = math.radians(coord1[1])
+    lati2 = math.radians(coord2[0])
+    long2 = math.radians(coord2[1])
+    y = math.sin(long2 - long1) * math.cos(lati2)
+    x = math.cos(lati1) * math.sin(lati2) - math.sin(lati1) * math.cos(
+        lati2
+    ) * math.cos(long2 - long1)
+    intbearing = math.atan2(y, x)
+    intbearing = math.degrees(intbearing)
+    intbearing = (intbearing + 360) % 360
+    return intbearing
 
 def update_display():
     display.fill(0)
@@ -118,11 +146,13 @@ def update_display():
     d = location_avg()
     for (name,val), i in zip(l['points'].items(), range(len(l['points']))):
         if len(val) > 0:
-            distance = haversine(d, (val[0][0],val[0][1]))
+            d = haver_dist(d, (val[0][0],val[0][1]))
+            b = bearing(d, (val[0][0],val[0][1]))
         else:
-            distance = float('inf')
-        print(name, "is", distance)
-        display.text(f"{name}: {distance}", 0, 10*(i+1), 1)
+            d = float('inf')
+            b = float('NaN')
+        print(name, ":", d, b)
+        display.text(f"{name}: {d} {b}", 0, 10*(i+1), 1)
     display.show()