diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index f61756d7d9ece..14cb3b36a2599 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -35,6 +35,10 @@ class Location // 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 c4a7cd90dcf9c..f97bc769c678e 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)); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 743b8b7e61d30..9a1528b9dca4e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -5058,7 +5058,7 @@ bool GCS_MAVLINK::location_from_command_t(const mavlink_command_int_t &in, Locat out.lat = in.x; out.lng = in.y; - out.set_alt_cm(int32_t(in.z * 100), frame); + out.set_alt_m(in.z, frame); return true; }