Repository: ArduPilot/ardupilot
Pull request #29764: add and use a Location::copy_alt_from
Author: login: peterbarker
Labels: name: MergeOnCIPass
Created: 2025-04-12T04:27:22Z | Merged: 2025-04-15T11:15:42Z

removes external knowledge of internal Location shape


Diff:
diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp
index ec4cfd67c6208..ec8f8a3275005 100644
--- a/ArduCopter/commands.cpp
+++ b/ArduCopter/commands.cpp
@@ -25,7 +25,7 @@ void Copter::set_home_to_current_location_inflight() {
     Location temp_loc;
     Location ekf_origin;
     if (ahrs.get_location(temp_loc) && ahrs.get_origin(ekf_origin)) {
-        temp_loc.alt = ekf_origin.alt;
+        temp_loc.copy_alt_from(ekf_origin);
         if (!set_home(temp_loc, false)) {
             return;
         }
diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp
index ec0ee5cd9e379..71400c853d597 100644
--- a/libraries/AP_Common/Location.cpp
+++ b/libraries/AP_Common/Location.cpp
@@ -86,6 +86,14 @@ bool Location::change_alt_frame(AltFrame desired_frame)
     return true;
 }
 
+void Location::copy_alt_from(const Location &other)
+{
+    alt = other.alt;
+    relative_alt = other.relative_alt;
+    terrain_alt = other.terrain_alt;
+    origin_alt = other.origin_alt;
+}
+
 // get altitude frame
 Location::AltFrame Location::get_alt_frame() const
 {
diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h
index ab7c0732c5903..2d9d2886126df 100644
--- a/libraries/AP_Common/Location.h
+++ b/libraries/AP_Common/Location.h
@@ -60,6 +60,9 @@ class Location
     // - above-origin and origin is not set
     bool change_alt_frame(AltFrame desired_frame);
 
+    // copy altitude and its frame of other Location object:
+    void copy_alt_from(const Location &other);
+
     // 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
diff --git a/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp b/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp
index 7d93184dd9ebc..9ab3004625e2d 100644
--- a/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp
+++ b/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp
@@ -48,10 +48,12 @@ void AP_GPS_ExternalAHRS::handle_external(const AP_ExternalAHRS::gps_data_messag
     }
     state.num_sats = pkt.satellites_in_view;
 
-    Location loc = {};
-    loc.lat = pkt.latitude;
-    loc.lng = pkt.longitude;
-    loc.alt = pkt.msl_altitude;
+    const Location loc {
+        pkt.latitude,
+        pkt.longitude,
+        pkt.msl_altitude,
+        Location::AltFrame::ABSOLUTE
+    };
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     if (!loc.initialised() && state.status >= AP_GPS::GPS_Status::GPS_OK_FIX_2D) {
         AP_HAL::panic("Invalid location passed to AP_GPS_ExternalAHRS");
diff --git a/libraries/AP_GPS/AP_GPS_MSP.cpp b/libraries/AP_GPS/AP_GPS_MSP.cpp
index 32c40c5d70195..7b089cf53f2ec 100644
--- a/libraries/AP_GPS/AP_GPS_MSP.cpp
+++ b/libraries/AP_GPS/AP_GPS_MSP.cpp
@@ -43,13 +43,12 @@ void AP_GPS_MSP::handle_msp(const MSP::msp_gps_data_message_t &pkt)
     state.time_week_ms = pkt.ms_tow;
     state.status = (AP_GPS::GPS_Status)pkt.fix_type;
     state.num_sats = pkt.satellites_in_view;
-
-    Location loc = {};
-    loc.lat = pkt.latitude;
-    loc.lng = pkt.longitude;
-    loc.alt = pkt.msl_altitude;
-
-    state.location = loc;
+    state.location = {
+        pkt.latitude,
+        pkt.longitude,
+        pkt.msl_altitude,
+        Location::AltFrame::ABSOLUTE
+    };
     state.hdop = pkt.hdop;
     state.vdop = GPS_UNKNOWN_DOP;
 
diff --git a/libraries/SITL/SIM_AirSim.cpp b/libraries/SITL/SIM_AirSim.cpp
index 989da084200e5..8097c694061cd 100644
--- a/libraries/SITL/SIM_AirSim.cpp
+++ b/libraries/SITL/SIM_AirSim.cpp
@@ -307,9 +307,12 @@ void AirSim::recv_fdm(const sitl_input& input)
     gyro = state.imu.angular_velocity;
     velocity_ef = state.velocity.world_linear_velocity;
 
-    location.lat = state.gps.lat * 1.0e7;
-    location.lng = state.gps.lon * 1.0e7;
-    location.alt = state.gps.alt * 100.0f;
+    location = {
+        int32_t(state.gps.lat * 1.0e7),
+        int32_t(state.gps.lon * 1.0e7),
+        int32_t(state.gps.alt * 100.0f),
+        Location::AltFrame::ABSOLUTE
+    };
 
     position = origin.get_distance_NED_double(location);
 
diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index a039e82d8473b..c8c017e8140b1 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -672,10 +672,12 @@ void Aircraft::update_home()
         if (sitl == nullptr) {
             return;
         }
-        Location loc;
-        loc.lat = sitl->opos.lat.get() * 1.0e7;
-        loc.lng = sitl->opos.lng.get() * 1.0e7;
-        loc.alt = sitl->opos.alt.get() * 1.0e2;
+        const Location loc{
+            int32_t(sitl->opos.lat.get() * 1.0e7),
+            int32_t(sitl->opos.lng.get() * 1.0e7),
+            int32_t(sitl->opos.alt.get() * 1.0e2),
+            Location::AltFrame::ABSOLUTE
+        };
         set_start_location(loc, sitl->opos.hdg.get());
     }
 }
diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp
index 1e1cc7539f341..556f146a3a8d2 100644
--- a/libraries/SITL/SIM_JSBSim.cpp
+++ b/libraries/SITL/SIM_JSBSim.cpp
@@ -435,9 +435,12 @@ void JSBSim::recv_fdm(const struct sitl_input &input)
     gyro = Vector3f(p, q, r);
 
     velocity_ef = Vector3f(fdm.v_north, fdm.v_east, fdm.v_down) * FEET_TO_METERS;
-    location.lat = RAD_TO_DEG_DOUBLE * fdm.latitude * 1.0e7;
-    location.lng = RAD_TO_DEG_DOUBLE * fdm.longitude * 1.0e7;
-    location.alt = fdm.agl*100 + home.alt;
+    location = {
+        int32_t(RAD_TO_DEG_DOUBLE * fdm.latitude * 1.0e7),
+        int32_t(RAD_TO_DEG_DOUBLE * fdm.longitude * 1.0e7),
+        int32_t(fdm.agl*100 + home.alt),
+        Location::AltFrame::ABSOLUTE
+    };
     dcm.from_euler(fdm.phi, fdm.theta, fdm.psi);
     airspeed = fdm.vcas * KNOTS_TO_METERS_PER_SECOND;
     airspeed_pitot = airspeed;
diff --git a/libraries/SITL/SIM_Scrimmage.cpp b/libraries/SITL/SIM_Scrimmage.cpp
index 9af2b7999194c..d3c0f761fb9fc 100644
--- a/libraries/SITL/SIM_Scrimmage.cpp
+++ b/libraries/SITL/SIM_Scrimmage.cpp
@@ -99,11 +99,13 @@ void Scrimmage::recv_fdm(const struct sitl_input &input)
 
     velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);
 
-    location.lat = pkt.latitude * 1.0e7;
-    location.lng = pkt.longitude * 1.0e7;
-    location.alt = pkt.altitude * 1.0e2;
-    position.z = (home.alt - location.alt) * 1.0e-2;
-
+    location = {
+        int32_t(pkt.latitude * 1.0e7),
+        int32_t(pkt.longitude * 1.0e7),
+        int32_t(pkt.altitude * 1.0e2),
+        Location::AltFrame::ABSOLUTE
+    };
+    UNUSED_RESULT(home.get_height_above(location, position.z));
 
     // velocity relative to air mass, in earth frame TODO
     velocity_air_ef = velocity_ef - wind_ef;
diff --git a/libraries/SITL/SIM_SilentWings.cpp b/libraries/SITL/SIM_SilentWings.cpp
index 4ebba41e0c7f1..20bacadd3f642 100644
--- a/libraries/SITL/SIM_SilentWings.cpp
+++ b/libraries/SITL/SIM_SilentWings.cpp
@@ -192,11 +192,14 @@ void SilentWings::process_packet()
     wind_ef = dcm * (Vector3f(pkt.vx, pkt.vy, pkt.vz) - Vector3f(pkt.vx_wind, pkt.vy_wind, pkt.vz_wind));
     airspeed = pkt.v_eas;
     airspeed_pitot = pkt.v_eas;
-    Location curr_location;
-    curr_location.lat = pkt.position_latitude * 1.0e7;
-    curr_location.lng = pkt.position_longitude * 1.0e7;
-    curr_location.alt = pkt.altitude_msl * 100.0f;
-    ground_level = curr_location.alt * 0.01f - pkt.altitude_ground;
+    const float alt_abs_m = pkt.altitude_msl;
+    Location curr_location{
+        int32_t(pkt.position_latitude * 1.0e7),
+        int32_t(pkt.position_longitude * 1.0e7),
+        int32_t(alt_abs_m * 100.0f),
+        Location::AltFrame::ABSOLUTE
+    };
+    ground_level = alt_abs_m - pkt.altitude_ground;
     Vector3f posdelta = origin.get_distance_NED(curr_location);
     position.x = posdelta.x;
     position.y = posdelta.y;
@@ -209,16 +212,19 @@ void SilentWings::process_packet()
         printf("SilentWings home reset dist=%f alt=%.1f/%.1f\n",
                curr_location.get_distance(location), curr_location.alt*0.01f, location.alt*0.01f);
         // reset home location
-        home.lat = curr_location.lat;
-        home.lng = curr_location.lng;
-        origin.lat = home.lat;
-        origin.lng = home.lng;
         // Resetting altitude reference point in flight can throw off a bunch
         // of important calculations, so let the home altitude always be 0m MSL
-        home.alt = 0;
+        home = {
+            curr_location.lat,
+            curr_location.lng,
+            0,
+            Location::AltFrame::ABSOLUTE,
+        };
+        origin.lat = home.lat;
+        origin.lng = home.lng;
         position.x = 0;
         position.y = 0;
-        position.z = -curr_location.alt;
+        position.z = -alt_abs_m;;
         home_initialized = true;
         update_position();
     }
diff --git a/libraries/SITL/SIM_last_letter.cpp b/libraries/SITL/SIM_last_letter.cpp
index 8cb72cd0eca67..0c16a18484f14 100644
--- a/libraries/SITL/SIM_last_letter.cpp
+++ b/libraries/SITL/SIM_last_letter.cpp
@@ -102,9 +102,12 @@ void last_letter::recv_fdm(const struct sitl_input &input)
     accel_body = Vector3f(pkt.xAccel, pkt.yAccel, pkt.zAccel);
     gyro = Vector3f(pkt.rollRate, pkt.pitchRate, pkt.yawRate);
     velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);
-    location.lat = pkt.latitude * 1.0e7;
-    location.lng = pkt.longitude * 1.0e7;
-    location.alt = pkt.altitude*1.0e2;
+    location = {
+        int32_t(pkt.latitude * 1.0e7),
+        int32_t(pkt.longitude * 1.0e7),
+        int32_t(pkt.altitude*1.0e2),
+        Location::AltFrame::ABSOLUTE
+    };
     dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw);
 
     airspeed = pkt.airspeed;


Reviews:
user: login: IamPete1
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-04-14T12:03:42Z
user: login: tridge
state: CHANGES_REQUESTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-14T22:35:20Z
user: login: peterbarker
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-14T23:25:44Z
user: login: peterbarker
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-15T01:32:48Z
user: login: peterbarker
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-15T01:33:08Z

Review comments:
diff_hunk: @@ -86,6 +86,14 @@ bool Location::change_alt_frame(AltFrame desired_frame)
     return true;
 }
 
+void Location::copy_alt_from(const Location &other)
path: libraries/AP_Common/Location.cpp
user: login: IamPete1
body: Should we be copying terrain relative altitudes without locations? That is somehow loosing some of the information. I note there is only one user in any-case.
created_at: 2025-04-14T12:03:42Z
author_association: MEMBER
diff_hunk: @@ -99,11 +99,13 @@ void Scrimmage::recv_fdm(const struct sitl_input &input)
 
     velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);
 
-    location.lat = pkt.latitude * 1.0e7;
-    location.lng = pkt.longitude * 1.0e7;
-    location.alt = pkt.altitude * 1.0e2;
-    position.z = (home.alt - location.alt) * 1.0e-2;
path: libraries/SITL/SIM_Scrimmage.cpp
user: login: tridge
body: why remove position.z set?
created_at: 2025-04-14T22:30:39Z
author_association: CONTRIBUTOR
diff_hunk: @@ -209,16 +212,19 @@ void SilentWings::process_packet()
         printf("SilentWings home reset dist=%f alt=%.1f/%.1f\n",
                curr_location.get_distance(location), curr_location.alt*0.01f, location.alt*0.01f);
         // reset home location
-        home.lat = curr_location.lat;
-        home.lng = curr_location.lng;
-        origin.lat = home.lat;
-        origin.lng = home.lng;
         // Resetting altitude reference point in flight can throw off a bunch
         // of important calculations, so let the home altitude always be 0m MSL
-        home.alt = 0;
+        home = {
+            curr_location.lat,
+            curr_location.lng,
+            0,
+            Location::AltFrame::ABSOLUTE,
+        };
+        origin.lat = home.lat;
+        origin.lng = home.lng;
         position.x = 0;
         position.y = 0;
-        position.z = -curr_location.alt;
+        position.z = -alt_abs_m * 100;;
path: libraries/SITL/SIM_SilentWings.cpp
user: login: tridge
body: looks wrong, maybe existing bug? position vector is used in meters in other places
created_at: 2025-04-14T22:33:44Z
author_association: CONTRIBUTOR
diff_hunk: @@ -532,14 +536,18 @@ bool XPlane::receive_data(void)
     accel_earth.z += GRAVITY_MSS;
     
     // the position may slowly deviate due to float accuracy and longitude scaling
-    if (loc.get_distance(location) > 4 || abs(loc.alt - location.alt)*0.01f > 2.0f) {
+    ftype delta_height;
path: libraries/SITL/SIM_XPlane.cpp
user: login: tridge
body: needs to be test flown over a long distance in XPlane
created_at: 2025-04-14T22:35:09Z
author_association: CONTRIBUTOR
diff_hunk: @@ -86,6 +86,14 @@ bool Location::change_alt_frame(AltFrame desired_frame)
     return true;
 }
 
+void Location::copy_alt_from(const Location &other)
path: libraries/AP_Common/Location.cpp
user: login: peterbarker
body: Good call, added an internal error

created_at: 2025-04-14T23:15:38Z
author_association: CONTRIBUTOR
diff_hunk: @@ -99,11 +99,13 @@ void Scrimmage::recv_fdm(const struct sitl_input &input)
 
     velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);
 
-    location.lat = pkt.latitude * 1.0e7;
-    location.lng = pkt.longitude * 1.0e7;
-    location.alt = pkt.altitude * 1.0e2;
-    position.z = (home.alt - location.alt) * 1.0e-2;
path: libraries/SITL/SIM_Scrimmage.cpp
user: login: peterbarker
body: It's still there :-)

created_at: 2025-04-14T23:17:36Z
author_association: CONTRIBUTOR
diff_hunk: @@ -209,16 +212,19 @@ void SilentWings::process_packet()
         printf("SilentWings home reset dist=%f alt=%.1f/%.1f\n",
                curr_location.get_distance(location), curr_location.alt*0.01f, location.alt*0.01f);
         // reset home location
-        home.lat = curr_location.lat;
-        home.lng = curr_location.lng;
-        origin.lat = home.lat;
-        origin.lng = home.lng;
         // Resetting altitude reference point in flight can throw off a bunch
         // of important calculations, so let the home altitude always be 0m MSL
-        home.alt = 0;
+        home = {
+            curr_location.lat,
+            curr_location.lng,
+            0,
+            Location::AltFrame::ABSOLUTE,
+        };
+        origin.lat = home.lat;
+        origin.lng = home.lng;
         position.x = 0;
         position.y = 0;
-        position.z = -curr_location.alt;
+        position.z = -alt_abs_m * 100;;
path: libraries/SITL/SIM_SilentWings.cpp
user: login: peterbarker
body: Yes, I agree, it does look suspect!  Not here to fix bugs ~~, just create them~~
created_at: 2025-04-14T23:21:07Z
author_association: CONTRIBUTOR
diff_hunk: @@ -532,14 +536,18 @@ bool XPlane::receive_data(void)
     accel_earth.z += GRAVITY_MSS;
     
     // the position may slowly deviate due to float accuracy and longitude scaling
-    if (loc.get_distance(location) > 4 || abs(loc.alt - location.alt)*0.01f > 2.0f) {
+    ftype delta_height;
path: libraries/SITL/SIM_XPlane.cpp
user: login: peterbarker
body: Removed these XPlane patches.

created_at: 2025-04-14T23:23:28Z
author_association: CONTRIBUTOR
diff_hunk: @@ -209,16 +212,19 @@ void SilentWings::process_packet()
         printf("SilentWings home reset dist=%f alt=%.1f/%.1f\n",
                curr_location.get_distance(location), curr_location.alt*0.01f, location.alt*0.01f);
         // reset home location
-        home.lat = curr_location.lat;
-        home.lng = curr_location.lng;
-        origin.lat = home.lat;
-        origin.lng = home.lng;
         // Resetting altitude reference point in flight can throw off a bunch
         // of important calculations, so let the home altitude always be 0m MSL
-        home.alt = 0;
+        home = {
+            curr_location.lat,
+            curr_location.lng,
+            0,
+            Location::AltFrame::ABSOLUTE,
+        };
+        origin.lat = home.lat;
+        origin.lng = home.lng;
         position.x = 0;
         position.y = 0;
-        position.z = -curr_location.alt;
+        position.z = -alt_abs_m * 100;;
path: libraries/SITL/SIM_SilentWings.cpp
user: login: peterbarker
body: ... well, we did just end up changing this blind

created_at: 2025-04-15T01:32:48Z
author_association: CONTRIBUTOR
diff_hunk: @@ -86,6 +86,14 @@ bool Location::change_alt_frame(AltFrame desired_frame)
     return true;
 }
 
+void Location::copy_alt_from(const Location &other)
path: libraries/AP_Common/Location.cpp
user: login: peterbarker
body: ... which we then removed at DevCall :-)

created_at: 2025-04-15T01:33:08Z
author_association: CONTRIBUTOR

PR comments:
user: login: timtuxworth
created_at: 2025-04-13T04:39:04Z
body: Might it be better to create an Altitude class with a magnitude and frame? Then we would get things like copy() methods as standard and it would be much easier simpler to pass them around etc.
author_association: CONTRIBUTOR
user: login: peterbarker
created_at: 2025-04-13T11:48:04Z
body: > Might it be better to create an Altitude class with a magnitude and frame? Then we would get things like copy() methods as standard and it would be much easier simpler to pass them around etc.

That's an interesting concept - and it would actually match a function I was looking at writing the other day.

OTOH, it's actually *very* rare that we use altitudes without also having an associated lat/lng.  Maybe a dozen places in the code.

I'm reluctant to divert from our current push to eliminate access to the `.alt` field at this time.  It's just bug after bug after bug, and we *do* have clear both (a) intent and (b) a clear path to doing that at the moment.

author_association: CONTRIBUTOR
user: login: timtuxworth
created_at: 2025-04-13T12:43:58Z
body: > OTOH, it's actually _very_ rare that we use altitudes without also having an associated lat/lng. Maybe a dozen places in the code.

Fence min/max altitude ... ;-)
author_association: CONTRIBUTOR