From 0077066ffb01944163a8b8da94fa7f03032ee428 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 28 Oct 2024 12:01:07 +0900 Subject: [PATCH] AP_Common: Location: add set_alt_m we have get_alt_m already, and there's a bunch of places that *100 --- libraries/AP_Common/Location.h | 4 ++++ libraries/AP_Common/tests/test_location.cpp | 7 +++++++ 2 files changed, 11 insertions(+) diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index f61756d7d9..14cb3b36a2 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -35,6 +35,10 @@ public: // set altitude void set_alt_cm(int32_t alt_cm, AltFrame frame); + // set_alt_m - set altitude in metres + void set_alt_m(float alt_m, AltFrame frame) { + set_alt_cm(alt_m*100, frame); + } // get altitude (in cm) in the desired frame // returns false on failure to get altitude in the desired frame which can only happen if the original frame or desired frame is: diff --git a/libraries/AP_Common/tests/test_location.cpp b/libraries/AP_Common/tests/test_location.cpp index c4a7cd90dc..f97bc769c6 100644 --- a/libraries/AP_Common/tests/test_location.cpp +++ b/libraries/AP_Common/tests/test_location.cpp @@ -280,6 +280,13 @@ TEST(Location, Tests) EXPECT_EQ(0, test_location4.loiter_xtrack); EXPECT_TRUE(test_location4.initialised()); + // test set_alt_m API: + Location loc = test_home; + loc.set_alt_m(1.71, Location::AltFrame::ABSOLUTE); + int32_t alt_in_cm_from_m; + EXPECT_TRUE(loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_in_cm_from_m)); + EXPECT_EQ(171, alt_in_cm_from_m); + // can't create a Location using a vector here as there's no origin for the vector to be relative to: // const Location test_location_empty{test_vect, Location::AltFrame::ABOVE_HOME}; // EXPECT_FALSE(test_location_empty.get_vector_from_origin_NEU(test_vec3));