ArduPlane: use Location::AltFrame for guided_state target frame references enabling ABOVE_TERRAIN

ArduPlane/GCS_Mavlink.cpp

@@ -932,48 +932,25 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
             return MAV_RESULT_DENIED;
         }
 
-         // the requested alt data might be relative or absolute
-        float new_target_alt = packet.z * 100;
-        float new_target_alt_rel = packet.z * 100 + plane.home.alt;
-
-         // only global/relative/terrain frames are supported
-        switch(packet.frame) {
-            case MAV_FRAME_GLOBAL_RELATIVE_ALT: {
-                if   (is_equal(plane.guided_state.target_alt,new_target_alt_rel) ) { // compare two floats as near-enough
-                    // no need to process any new packet/s with the same ALT any further, if we are already doing it.
-                    return MAV_RESULT_ACCEPTED;
-                }
-                plane.guided_state.target_alt = new_target_alt_rel;
-                break;
-            }
-            case MAV_FRAME_GLOBAL: {
-                if   (is_equal(plane.guided_state.target_alt,new_target_alt) ) {  // compare two floats as near-enough
-                    // no need to process any new packet/s with the same ALT any further, if we are already doing it.
-                    return MAV_RESULT_ACCEPTED;
-                }
-                plane.guided_state.target_alt = new_target_alt;
-                break;
-            }
-            default:
-                //  MAV_RESULT_DENIED  means Command is invalid (is supported but has invalid parameters).
-                return MAV_RESULT_DENIED;
+        Location::AltFrame new_target_alt_frame;
+        if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.frame, new_target_alt_frame)) {
+            return MAV_RESULT_DENIED;
         }
+        // keep a copy of what came in via MAVLink - this is needed for logging, but not for anything else
+        plane.guided_state.target_mav_frame = packet.frame;
 
-        plane.guided_state.target_alt_frame = packet.frame;
-        plane.guided_state.last_target_alt = plane.current_loc.alt; // FIXME: Reference frame is not corrected for here
+        const int32_t new_target_alt_cm = packet.z * 100;
+        plane.guided_state.target_location.set_alt_cm(new_target_alt_cm, new_target_alt_frame); 
         plane.guided_state.target_alt_time_ms = AP_HAL::millis();
 
+        // param3 contains the desired vertical velocity (not acceleration)
         if (is_zero(packet.param3)) {
-            // the user wanted /maximum acceleration, pick a large value as close enough
-            plane.guided_state.target_alt_accel = 1000.0;
+            // the user wanted /maximum altitude change rate, pick a large value as close enough
+            plane.guided_state.target_alt_rate = 1000.0;
         } else {
-            plane.guided_state.target_alt_accel = fabsf(packet.param3);
+            plane.guided_state.target_alt_rate = fabsf(packet.param3);
         }
 
-         // assign an acceleration direction
-        if (plane.guided_state.target_alt < plane.current_loc.alt) {
-            plane.guided_state.target_alt_accel *= -1.0f;
-        }
         return MAV_RESULT_ACCEPTED;
     }
 

ArduPlane/Log.cpp

@@ -128,10 +128,11 @@ struct PACKED log_OFG_Guided {
     float target_airspeed_cm;
     float target_airspeed_accel;
     float target_alt;
-    float target_alt_accel;
-    uint8_t target_alt_frame;
+    float target_alt_rate;
+    uint8_t target_mav_frame;   // received MavLink frame
     float target_heading;
     float target_heading_limit;
+    uint8_t target_alt_frame;   // internal AltFrame
 };
 
 // Write a OFG Guided packet.
@@ -142,11 +143,12 @@ void Plane::Log_Write_OFG_Guided()
         time_us                : AP_HAL::micros64(),
         target_airspeed_cm     : (float)guided_state.target_airspeed_cm*(float)0.01,
         target_airspeed_accel  : guided_state.target_airspeed_accel,
-        target_alt             : guided_state.target_alt,
-        target_alt_accel       : guided_state.target_alt_accel,
-        target_alt_frame       : guided_state.target_alt_frame,
+        target_alt             : guided_state.target_location.alt * 0.01,
+        target_alt_rate        : guided_state.target_alt_rate,
+        target_mav_frame       : guided_state.target_mav_frame,
         target_heading         : guided_state.target_heading,
-        target_heading_limit   : guided_state.target_heading_accel_limit
+        target_heading_limit   : guided_state.target_heading_accel_limit,
+        target_alt_frame       : static_cast<uint8_t>(guided_state.target_location.get_alt_frame()),
     };
     logger.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -274,7 +276,7 @@ void Plane::Log_Write_Guided(void)
         logger.Write_PID(LOG_PIDG_MSG, g2.guidedHeading.get_pid_info());
     }
 
-    if ( is_positive(guided_state.target_alt) || is_positive(guided_state.target_airspeed_cm) ) {
+    if ( guided_state.target_location.alt != -1 || is_positive(guided_state.target_airspeed_cm) ) {
         Log_Write_OFG_Guided();
     }
 #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
@@ -490,12 +492,13 @@ const struct LogStructure Plane::log_structure[] = {
 // @Field: Arsp:  target airspeed cm
 // @Field: ArspA:  target airspeed accel
 // @Field: Alt:  target alt
-// @Field: AltA: target alt accel
-// @Field: AltF: target alt frame
+// @Field: AltA: target alt velocity (rate of change)
+// @Field: AltF: target alt frame (MAVLink)
 // @Field: Hdg:  target heading
 // @Field: HdgA: target heading lim
+// @Field: AltL: target alt frame (Location)
     { LOG_OFG_MSG, sizeof(log_OFG_Guided),     
-      "OFG", "QffffBff",    "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA", "s-------", "F-------" , true }, 
+      "OFG", "QffffBffB",    "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA,AltL", "snnmo-d--", "F--------" , true }, 
 #endif
 };
 

ArduPlane/Plane.h

@@ -577,11 +577,10 @@ class Plane : public AP_Vehicle {
         uint32_t target_airspeed_time_ms;
 
         // altitude adjustments
-        float target_alt = -1;   // don't default to zero here, as zero is a valid alt.
-        uint32_t last_target_alt = 0;
-        float target_alt_accel;
+        Location target_location;
+        float target_alt_rate;
         uint32_t target_alt_time_ms = 0;
-        uint8_t target_alt_frame = 0;
+        uint8_t target_mav_frame = -1;
 
         // heading track
         float target_heading = -4; // don't default to zero or -1 here, as both are valid headings in radians

ArduPlane/mode.cpp

@@ -58,9 +58,8 @@ bool Mode::enter()
     plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range
     plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
     plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.
-    plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane.
     plane.guided_state.target_alt_time_ms = 0;
-    plane.guided_state.last_target_alt = 0;
+    plane.guided_state.target_location.set_alt_cm(-1, Location::AltFrame::ABSOLUTE); 
 #endif
 
 #if AP_CAMERA_ENABLED

ArduPlane/mode_guided.cpp

@@ -129,24 +129,29 @@ void ModeGuided::set_radius_and_direction(const float radius, const bool directi
 void ModeGuided::update_target_altitude()
 {
 #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
-    if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero.
+    // target altitude can be negative (e.g. flying below home altitude from the top of a mountain)
+    if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_location.alt != -1 )) { // target_alt now defaults to -1, and _time_ms defaults to zero.
         // offboard altitude demanded
         uint32_t now = AP_HAL::millis();
         float delta = 1e-3f * (now - plane.guided_state.target_alt_time_ms);
         plane.guided_state.target_alt_time_ms = now;
         // determine delta accurately as a float
-        float delta_amt_f = delta * plane.guided_state.target_alt_accel;
+        float delta_amt_f = delta * plane.guided_state.target_alt_rate;
         // then scale x100 to match last_target_alt and convert to a signed int32_t as it may be negative
         int32_t delta_amt_i = (int32_t)(100.0 * delta_amt_f); 
-        Location temp {};
-        temp.alt = plane.guided_state.last_target_alt + delta_amt_i; // ...to avoid floats here, 
-        if (is_positive(plane.guided_state.target_alt_accel)) {
-            temp.alt = MIN(plane.guided_state.target_alt, temp.alt);
-        } else {
-            temp.alt = MAX(plane.guided_state.target_alt, temp.alt);
+        // To calculate the required velocity (up or down), we need to target and current altitudes in the target frame
+        const Location::AltFrame target_frame = plane.guided_state.target_location.get_alt_frame();
+        int32_t target_alt_previous_cm;
+        if (plane.current_loc.initialised() && plane.guided_state.target_location.initialised() && 
+            plane.current_loc.get_alt_cm(target_frame, target_alt_previous_cm)) {
+            // create a new interim target location that that takes current_location and moves delta_amt_i in the right direction
+            int32_t temp_alt_cm = constrain_int32(plane.guided_state.target_location.alt, target_alt_previous_cm - delta_amt_i,  target_alt_previous_cm + delta_amt_i);
+            Location temp_location = plane.guided_state.target_location;
+            temp_location.set_alt_cm(temp_alt_cm, target_frame);
+
+            // incrementally step the altitude towards the target            
+            plane.set_target_altitude_location(temp_location);
         }
-        plane.guided_state.last_target_alt = temp.alt;
-        plane.set_target_altitude_location(temp);
     } else 
 #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
         {

Tools/autotest/arduplane.py

@@ -5533,6 +5533,7 @@ def RunMissionScript(self):
 
     def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self):
         '''test handling of MAV_CMD_GUIDED_CHANGE_ALTITUDE'''
+        self.start_subtest("set home relative altitude")
         self.takeoff(30, relative=True)
         self.change_mode('GUIDED')
         for alt in 50, 70:
@@ -5544,6 +5545,7 @@ def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self):
             self.wait_altitude(alt-1, alt+1, timeout=30, relative=True)
 
         # test for #24535
+        self.start_subtest("switch to loiter and resume guided maintains home relative altitude")
         self.change_mode('LOITER')
         self.delay_sim_time(5)
         self.change_mode('GUIDED')
@@ -5554,6 +5556,55 @@ def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self):
             timeout=30,
             relative=True,
         )
+        # test that this works if switching between RELATIVE (HOME) and GLOBAL(AMSL)
+        self.start_subtest("set global/AMSL altitude, switch to loiter and resume guided")
+        alt = 625
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_GUIDED_CHANGE_ALTITUDE,
+            p7=alt,
+            frame=mavutil.mavlink.MAV_FRAME_GLOBAL,
+        )
+        self.wait_altitude(alt-3, alt+3, timeout=30, relative=False)
+        self.change_mode('LOITER')
+        self.delay_sim_time(5)
+        self.change_mode('GUIDED')
+        self.wait_altitude(
+            alt-5,  # NOTE: reuse of alt from above CHANGE_ALTITUDE
+            alt+5,
+            minimum_duration=10,
+            timeout=30,
+            relative=False,
+        )
+        # test that this works if switching between RELATIVE (HOME) and terrain
+        self.start_subtest("set terrain altitude, switch to loiter and resume guided")
+        self.change_mode('GUIDED')
+        alt = 100
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_GUIDED_CHANGE_ALTITUDE,
+            p7=alt,
+            frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
+        )
+        self.wait_altitude(
+            alt-5,  # NOTE: reuse of alt from abovE
+            alt+5,  # use a 5m buffer as the plane needs to go up and down a bit to maintain terrain distance
+            minimum_duration=10,
+            timeout=30,
+            relative=False,
+            altitude_source="TERRAIN_REPORT.current_height"
+        )
+        self.change_mode('LOITER')
+        self.delay_sim_time(5)
+        self.change_mode('GUIDED')
+        self.wait_altitude(
+            alt-5,  # NOTE: reuse of alt from abovE
+            alt+5,  # use a 5m buffer as the plane needs to go up and down a bit to maintain terrain distance
+            minimum_duration=10,
+            timeout=30,
+            relative=False,
+            altitude_source="TERRAIN_REPORT.current_height"
+        )
+
+        self.delay_sim_time(5)
         self.fly_home_land_and_disarm()
 
     def _MAV_CMD_PREFLIGHT_CALIBRATION(self, command):

Tools/autotest/vehicle_test_suite.py

@@ -7176,6 +7176,10 @@ def get_altitude(self, relative=False, timeout=30, altitude_source=None):
                 altitude_source = "GLOBAL_POSITION_INT.relative_alt"
             else:
                 altitude_source = "GLOBAL_POSITION_INT.alt"
+        if altitude_source == "TERRAIN_REPORT.current_height":
+            terrain = self.assert_receive_message('TERRAIN_REPORT')
+            return terrain.current_height
+
         (msg, field) = altitude_source.split('.')
         msg = self.poll_message(msg, quiet=True)
         divisor = 1000.0  # mm is pretty common in mavlink