Skip to content

Instantly share code, notes, and snippets.

@FergusInLondon
Last active March 13, 2024 16:24
Show Gist options
  • Save FergusInLondon/125ca48710a04ae10d66fedcd58286eb to your computer and use it in GitHub Desktop.
Save FergusInLondon/125ca48710a04ae10d66fedcd58286eb to your computer and use it in GitHub Desktop.
bearing + angle of elevation calculations
from collections import namedtuple
from math import radians, sin, cos, atan2, sqrt, degrees
# Point is a convenience wrapper to hold the co-ordinates belonging to
# a particular "point" - i.e. the base station location, or the remote
# device location.
#
# Units: lat, long = radians. alt = meters.
Point = namedtuple('Point', ['lat', 'lon', 'alt'])
def point(lat: float, lon: float, alt: float) -> Point:
return Point(lat=radians(lat), lon=radians(lon), alt=alt)
# Direction is a convenience wrapper that holds the information about the
# "direction" between two Points; these should translate pretty well for
# use with a 2-axis gimbal.
#
# Units: bearing = degrees. elevation, distance = meters.
Direction = namedtuple('Direction', ['bearing', 'elevation', 'distance'])
class Location:
def __init__(self, location: Point, precision: int = 2) -> None:
self.location = location
self.precision = precision
def update(self, **kwargs):
update = { k : kwargs[k] for k in kwargs if k in ['lat', 'lon', 'alt'] }
if not (len(update) > 0 and len(update) < 3):
raise ValueError(f"location update must consist of a subset of ['lat', 'lon', 'alt'] - got {list(kwargs.keys())}")
self.location = self.location._replace(**update)
@staticmethod
def _calculate(base: Point, remote: Point, precision: int = 2) -> tuple:
R = 6371000.0 # Radius of the Earth
delta_lat = remote.lat - base.lat
delta_lon = remote.lon - base.lon
# Distance: Haversine Formula
distance = R * (2 * atan2(
sqrt((a := sin(delta_lat / 2)**2 + cos(base.lat) * cos(remote.lat) * sin(delta_lon / 2)**2)),
sqrt(1 - a)
))
# Angle of Elevation
angle_of_elevation = degrees(atan2(remote.alt - base.alt, distance))
# Bearing
y = sin(delta_lon) * cos(remote.lat)
x = cos(base.lat) * sin(remote.lat) - (sin(base.lat) * cos(remote.lat) * cos(delta_lon))
bearing = (degrees(atan2(y, x)) + 360) % 360 # @note: is the degrees() call in the correct location here?
return Direction(
bearing = round(bearing, precision),
elevation = round(angle_of_elevation, precision),
distance = round(distance, precision)
)
def direction(self, remote: Point) -> Direction:
return self._calculate(self.location, remote, self.precision)
if __name__ == "__main__":
def geographic_tests():
TestCase = namedtuple("TestCase", ["RemotePoint", "ExpectedDirection"])
for idx, t in enumerate([
# (Remote Point, Expected Direction)
TestCase(point(51.4986765, -0.104676284, 0), Direction(149.09, 0, 1073.14)),
TestCase(point(51.4987206, -0.112233761, 0), Direction(178.25, 0, 916.32)),
TestCase(point(51.5008267, -0.119832024, 0), Direction(216.14, 0, 844.14)),
TestCase(point(51.5020523, -0.124047118, 0), Direction(235.37, 0, 959.66)),
TestCase(point(51.5044345, -0.123437039, 0), Direction(249.43, 0, 798.26)),
TestCase(point(51.5072307, -0.121800917, 0), Direction(272.75, 0, 634.81)),
TestCase(point(51.5099059, -0.118168171, 0), Direction(310.59, 0, 503.90)),
TestCase(point(51.5111140, -0.108711939, 0), Direction(30.46, 0, 536.18)),
]):
if not compare(t.ExpectedDirection, (actual := Location._calculate(point(51.5069574, -0.112639096, 0), t.RemotePoint))):
print(f"[geographic - {idx}] TEST FAILED. expected '{t.ExpectedDirection}', actual '{actual}'")
exit()
def elevation_tests():
TestCase = namedtuple("TestCase", ["BaseAltitude", "RemoteAltitude", "ExpectedElevation"])
for idx, t in enumerate([TestCase(-5, 10, 0.80), TestCase(0, 430, 21.84)]):
base = Location(point(51.5069574, -0.112639096, t.BaseAltitude), 2)
if not compare(
Direction(149.09, t.ExpectedElevation, 1073.14),
(actual := base.direction(point(51.4986765, -0.104676284, t.RemoteAltitude))
)
):
print(f"[elevation - {idx}] TEST FAILED. expected '{t.ExpectedElevation}', actual '{actual.elevation}'")
exit()
def compare(expected, actual):
for prop in ["bearing", "elevation", "distance"]:
if getattr(expected, prop) != getattr(actual, prop):
return False
return True
geographic_tests()
elevation_tests()
print("all test cases passed.")
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment