import math
|
|
import pytest
|
|
from hitl import geo
|
|
|
GGA_SAMPLE = "$GNGGA,080112.000,3949.8105069,N,11616.6876082,E,4,44,0.42,48.502,M,-6.684,M,1.0,0409*73"
|
|
|
def test_parse_origin_from_gga():
|
origin = geo.parse_origin(GGA_SAMPLE)
|
assert origin.latitude_deg == pytest.approx(39.830175115)
|
assert origin.longitude_deg == pytest.approx(116.278126803)
|
assert origin.altitude_m == pytest.approx(48.502)
|
|
|
def test_parse_origin_short_format():
|
origin = geo.parse_origin("3949.8105,N,11616.6876,E,50.0")
|
assert origin.latitude_deg == pytest.approx(39.830175)
|
assert origin.longitude_deg == pytest.approx(116.278126)
|
assert origin.altitude_m == pytest.approx(50.0)
|
|
|
def test_enu_roundtrip_accuracy():
|
origin = geo.parse_origin(GGA_SAMPLE)
|
east, north, up = 5.0, -3.0, 1.2
|
lat, lon, alt = geo.enu_to_lla(east, north, up, origin)
|
x, y, z = geo.geo_to_ecef(lat, lon, alt)
|
dx = x - origin.ecef[0]
|
dy = y - origin.ecef[1]
|
dz = z - origin.ecef[2]
|
east_rt, north_rt, up_rt = geo.ecef_to_enu(dx, dy, dz, origin.latitude_rad, origin.longitude_rad)
|
|
assert east_rt == pytest.approx(east, abs=0.01)
|
assert north_rt == pytest.approx(north, abs=0.01)
|
assert up_rt == pytest.approx(up, abs=0.01)
|
|
|
def test_heading_math_to_nav():
|
assert geo.heading_math_to_nav(math.radians(0.0)) == pytest.approx(90.0)
|
assert geo.heading_math_to_nav(math.radians(90.0)) == pytest.approx(0.0)
|
assert geo.heading_math_to_nav(math.radians(-90.0)) == pytest.approx(180.0)
|
