From 57c80cd232a212fa762a9d95dddff81cb575fd8b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 6 Jun 2024 12:39:38 +1000 Subject: [PATCH] AP_Common: rearrange get_vector_from_origin_NEU to be less destructive this change means that if the origin call failes we don't half-update the object before returning false --- libraries/AP_Common/Location.cpp | 15 ++++++++++----- libraries/AP_Common/Location.h | 11 +++++++---- libraries/AP_Common/tests/test_location.cpp | 5 +++-- 3 files changed, 20 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index 273ecb9fe50b16..241f5224f6819e 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -222,6 +222,8 @@ bool Location::get_alt_m(AltFrame desired_frame, float &ret_alt) const } #if AP_AHRS_ENABLED +// converts location to a vector from origin; if this method returns +// false then vec_ne is unmodified bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const { Location ekf_origin; @@ -233,18 +235,21 @@ bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const return true; } +// converts location to a vector from origin; if this method returns +// false then vec_neu is unmodified bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const { - // convert lat, lon - if (!get_vector_xy_from_origin_NE(vec_neu.xy())) { - return false; - } - // convert altitude int32_t alt_above_origin_cm = 0; if (!get_alt_cm(AltFrame::ABOVE_ORIGIN, alt_above_origin_cm)) { return false; } + + // convert lat, lon + if (!get_vector_xy_from_origin_NE(vec_neu.xy())) { + return false; + } + vec_neu.z = alt_above_origin_cm; return true; diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index e84be52611105e..4b9ad8ba2ffb01 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -55,11 +55,14 @@ class Location // - above-origin and origin is not set bool change_alt_frame(AltFrame desired_frame); - // get position as a vector (in cm) from origin (x,y only or x,y,z) - // return false on failure to get the vector which can only - // happen if the EKF origin has not been set yet - // x, y and z are in centimetres + // get position as a vector (in cm) from origin (x,y only or + // x,y,z) return false on failure to get the vector which can only + // happen if the EKF origin has not been set yet x, y and z are in + // centimetres. If this method returns false then vec_ne is + // unmodified. bool get_vector_xy_from_origin_NE(Vector2f &vec_ne) const WARN_IF_UNUSED; + // converts location to a vector from origin; if this method returns + // false then vec_neu is unmodified bool get_vector_from_origin_NEU(Vector3f &vec_neu) const WARN_IF_UNUSED; // return horizontal distance in meters between two locations diff --git a/libraries/AP_Common/tests/test_location.cpp b/libraries/AP_Common/tests/test_location.cpp index d545c2c754df14..c4a7cd90dcf9c1 100644 --- a/libraries/AP_Common/tests/test_location.cpp +++ b/libraries/AP_Common/tests/test_location.cpp @@ -280,8 +280,9 @@ TEST(Location, Tests) EXPECT_EQ(0, test_location4.loiter_xtrack); EXPECT_TRUE(test_location4.initialised()); - const Location test_location_empty{test_vect, Location::AltFrame::ABOVE_HOME}; - EXPECT_FALSE(test_location_empty.get_vector_from_origin_NEU(test_vec3)); + // 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)); } TEST(Location, Distance)