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

Author: peterbarker

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;

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

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)