aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Common: rearrange get_vector_from_origin_NEU to be less destructive

Browse Source 
this change means that if the origin call failes we don't half-update the object before returning false
This commit is contained in:
Peter Barker 2024-06-06 12:39:38 +10:00 committed by Andrew Tridgell
parent 500ca22c75
commit 911375fa9a
3 changed files with 20 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Common/Location.cpp
View File

@ -222,6 +222,8 @@ bool Location::get_alt_m(AltFrame desired_frame, float &ret_alt) const
} }
#if AP_AHRS_ENABLED #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 bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
{ {
Location ekf_origin; Location ekf_origin;
@ -233,18 +235,21 @@ bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
return true; 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 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 // convert altitude
int32_t alt_above_origin_cm = 0; int32_t alt_above_origin_cm = 0;
if (!get_alt_cm(AltFrame::ABOVE_ORIGIN, alt_above_origin_cm)) { if (!get_alt_cm(AltFrame::ABOVE_ORIGIN, alt_above_origin_cm)) {
return false; return false;
} }
// convert lat, lon
if (!get_vector_xy_from_origin_NE(vec_neu.xy())) {
return false;
}
vec_neu.z = alt_above_origin_cm; vec_neu.z = alt_above_origin_cm;
return true; return true;

11
libraries/AP_Common/Location.h
View File

@ -55,11 +55,14 @@ public:
// - above-origin and origin is not set // - above-origin and origin is not set
bool change_alt_frame(AltFrame desired_frame); bool change_alt_frame(AltFrame desired_frame);
// get position as a vector (in cm) from origin (x,y only or x,y,z) // get position as a vector (in cm) from origin (x,y only or
// return false on failure to get the vector which can only // x,y,z) return false on failure to get the vector which can only
// happen if the EKF origin has not been set yet // happen if the EKF origin has not been set yet x, y and z are in
// x, y and z are in centimetres // 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; 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; bool get_vector_from_origin_NEU(Vector3f &vec_neu) const WARN_IF_UNUSED;
// return horizontal distance in meters between two locations // return horizontal distance in meters between two locations

5
libraries/AP_Common/tests/test_location.cpp
View File

@ -280,8 +280,9 @@ TEST(Location, Tests)
EXPECT_EQ(0, test_location4.loiter_xtrack); EXPECT_EQ(0, test_location4.loiter_xtrack);
EXPECT_TRUE(test_location4.initialised()); EXPECT_TRUE(test_location4.initialised());
const Location test_location_empty{test_vect, Location::AltFrame::ABOVE_HOME}; // can't create a Location using a vector here as there's no origin for the vector to be relative to:
EXPECT_FALSE(test_location_empty.get_vector_from_origin_NEU(test_vec3)); // 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) TEST(Location, Distance)