UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-08-02 04:58:15 +08:00
Code Issues Packages Projects Releases Wiki Activity

lla; fix conversion to ECEF

Browse Source
This commit is contained in:
bresch
2024-12-02 18:17:17 +01:00
committed by Mathieu Bresciani
parent 4696338d29
commit 6b637f82f8
2 changed files with 15 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/lat_lon_alt/lat_lon_alt.cpp
View File

@@ -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<double>(_altitude)) * sin_lat);
}
Vector3f LatLonAlt::computeAngularRateNavFrame(const Vector3f &v_ned) const

14
src/lib/lat_lon_alt/test_lat_lon_alt.cpp
View File

@@ -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;
}
}
}
}