From 4384b9ea3639dfdf1e8b54b862cb7b3eac622008 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 22 Jul 2021 14:57:06 +1000 Subject: [PATCH] AP_Common: update tests for 4.1 --- libraries/AP_Common/tests/test_location.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Common/tests/test_location.cpp b/libraries/AP_Common/tests/test_location.cpp index f252d6f0c5..9ad5a59751 100644 --- a/libraries/AP_Common/tests/test_location.cpp +++ b/libraries/AP_Common/tests/test_location.cpp @@ -139,7 +139,7 @@ TEST(Location, LocOffsetDouble) -353632620, 1491652373, Vector2d{4682795.4576701336, 5953662.7673837934}, Vector2d{4682797.1904749088, 5953664.1586009059}, - Vector2d{1.7365739867091179,1.7025913001452864}, + Vector2d{1.7365739867091179,1.2050807}, }; for (auto &test : tests) { @@ -409,4 +409,23 @@ TEST(Location, Line) EXPECT_TRUE(test_wp.past_interval_finish_line(test_home, test_wp_last)); } +/* + check if we obey basic euclidian geometry rules of position + addition/subtraction + */ +TEST(Location, OffsetError) +{ + // test at 10km from origin + const float ofs_ne = 10e3 / sqrtf(2.0); + for (float lat = -80; lat <= 80; lat += 10.0) { + Location origin{int32_t(lat*1e7), 0, 0, Location::AltFrame::ABOVE_HOME}; + Location loc = origin; + loc.offset(ofs_ne, ofs_ne); + Location loc2 = loc; + loc2.offset(-ofs_ne, -ofs_ne); + float dist = origin.get_distance(loc2); + EXPECT_FLOAT_EQ(dist, 0); + } +} + AP_GTEST_MAIN()