From 6b637f82f8bda3b06d8e65534dd1ae063879fce8 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 2 Dec 2024 18:17:17 +0100 Subject: [PATCH] lla; fix conversion to ECEF --- src/lib/lat_lon_alt/lat_lon_alt.cpp | 2 +- src/lib/lat_lon_alt/test_lat_lon_alt.cpp | 14 ++++++++++++++ 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/lib/lat_lon_alt/lat_lon_alt.cpp b/src/lib/lat_lon_alt/lat_lon_alt.cpp index d9ac442e3a..40b86a635a 100644 --- a/src/lib/lat_lon_alt/lat_lon_alt.cpp +++ b/src/lib/lat_lon_alt/lat_lon_alt.cpp @@ -77,7 +77,7 @@ Vector3d LatLonAlt::toEcef() const return Vector3d(r_total * cos_lat * cos_lon, r_total * cos_lat * sin_lon, - ((1.0 - Wgs84::eccentricity2) * r_total) * sin_lat); + ((1.0 - Wgs84::eccentricity2) * r_e + static_cast(_altitude)) * sin_lat); } Vector3f LatLonAlt::computeAngularRateNavFrame(const Vector3f &v_ned) const diff --git a/src/lib/lat_lon_alt/test_lat_lon_alt.cpp b/src/lib/lat_lon_alt/test_lat_lon_alt.cpp index b7a42ebd48..f2419951d5 100644 --- a/src/lib/lat_lon_alt/test_lat_lon_alt.cpp +++ b/src/lib/lat_lon_alt/test_lat_lon_alt.cpp @@ -105,3 +105,17 @@ TEST(TestLatLonAlt, subLatLonAlt) EXPECT_NEAR(delta_pos(1), delta_pos_true(1), 1e-2); EXPECT_EQ(delta_pos(2), delta_pos_true(2)); } + +TEST(TestLatLonAlt, fromAndToECEF) +{ + for (double lat = -M_PI; lat < M_PI; lat += M_PI / 4.0) { + for (double lon = -M_PI; lon < M_PI; lon += M_PI / 4.0) { + for (float alt = -500.f; alt < 8000.f; alt += 500.f) { + LatLonAlt lla(lat, lon, alt); + + LatLonAlt res = LatLonAlt::fromEcef(lla.toEcef()); + EXPECT_TRUE(!(lla - res).longerThan(10e-6f)) << "lat: " << lat << ", lon: " << lon << ", alt: " << alt; + } + } + } +}