Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
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
// 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;

11
libraries/AP_Common/Location.h
View File

@ -55,11 +55,14 @@ public:
// - 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

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_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)