Plane: fixed terrain guided target intermediate alt handling (4.6 backport)

ArduPlane/ArduPlane.cpp

@@ -886,15 +886,15 @@ bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
 bool Plane::set_target_location(const Location &target_loc)
 {
     Location loc{target_loc};
+    fix_terrain_WP(loc, __LINE__);
 
     if (plane.control_mode != &plane.mode_guided) {
         // only accept position updates when in GUIDED mode
         return false;
     }
-    // add home alt if needed
-    if (loc.relative_alt) {
-        loc.alt += plane.home.alt;
-        loc.relative_alt = 0;
+    // convert to absolute
+    if (!loc.terrain_alt) {
+        loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
     }
     plane.set_guided_WP(loc);
     return true;
@@ -942,6 +942,8 @@ bool Plane::update_target_location(const Location &old_loc, const Location &new_
     }
     next_WP_loc = new_loc;
 
+    fix_terrain_WP(next_WP_loc, __LINE__);
+
 #if HAL_QUADPLANE_ENABLED
     if (control_mode == &mode_qland || control_mode == &mode_qloiter) {
         mode_qloiter.last_target_loc_set_ms = AP_HAL::millis();

ArduPlane/GCS_Mavlink.cpp

@@ -748,12 +748,17 @@ bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd)
  */
 void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
 {
-    plane.next_WP_loc.alt = cmd.content.location.alt;
-    if (cmd.content.location.relative_alt && !cmd.content.location.terrain_alt) {
-        plane.next_WP_loc.alt += plane.home.alt;
+    plane.fix_terrain_WP(cmd.content.location, __LINE__);
+
+    if (cmd.content.location.terrain_alt) {
+        plane.next_WP_loc.set_alt_cm(cmd.content.location.alt, Location::AltFrame::ABOVE_TERRAIN);
+    } else {
+        // convert to absolute alt
+        float abs_alt_m;
+        if (cmd.content.location.get_alt_m(Location::AltFrame::ABSOLUTE, abs_alt_m)) {
+            plane.next_WP_loc.set_alt_m(abs_alt_m, Location::AltFrame::ABSOLUTE);
+        }
     }
-    plane.next_WP_loc.relative_alt = false;
-    plane.next_WP_loc.terrain_alt = cmd.content.location.terrain_alt;
     plane.reset_offset_altitude();
 }
 
@@ -839,6 +844,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
     if (!location_from_command_t(packet, requested_position)) {
         return MAV_RESULT_DENIED;
     }
+    plane.fix_terrain_WP(requested_position, __LINE__);
 
     if (isnan(packet.param4) || is_zero(packet.param4)) {
         requested_position.loiter_ccw = 0;
@@ -864,10 +870,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
         (plane.control_mode == &plane.mode_guided)) {
         plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND);
 
-        // add home alt if needed
-        if (requested_position.relative_alt && !requested_position.terrain_alt) {
-            requested_position.alt += plane.home.alt;
-            requested_position.relative_alt = 0;
+        // convert to absolute alt
+        if (!requested_position.terrain_alt) {
+            requested_position.change_alt_frame(Location::AltFrame::ABSOLUTE);
         }
 
         plane.set_guided_WP(requested_position);

ArduPlane/Plane.h

@@ -897,6 +897,9 @@ class Plane : public AP_Vehicle {
     int32_t relative_target_altitude_cm(void);
     void change_target_altitude(int32_t change_cm);
     void set_target_altitude_proportion(const Location &loc, float proportion);
+#if AP_TERRAIN_AVAILABLE
+    bool set_target_altitude_proportion_terrain(void);
+#endif
     void constrain_target_altitude_location(const Location &loc1, const Location &loc2);
     int32_t calc_altitude_error_cm(void);
     void check_fbwb_altitude(void);
@@ -909,6 +912,7 @@ class Plane : public AP_Vehicle {
     float mission_alt_offset(void);
     float height_above_target(void);
     float lookahead_adjustment(void);
+    void fix_terrain_WP(Location &loc, uint32_t linenum);
 #if AP_RANGEFINDER_ENABLED
     float rangefinder_correction(void);
     void rangefinder_height_update(void);

ArduPlane/altitude.cpp

@@ -34,15 +34,8 @@ void Plane::check_home_alt_change(void)
     if (home_alt_cm != auto_state.last_home_alt_cm && hal.util->get_soft_armed()) {
         // cope with home altitude changing
         const int32_t alt_change_cm = home_alt_cm - auto_state.last_home_alt_cm;
-        if (next_WP_loc.terrain_alt) {
-            /*
-              next_WP_loc for terrain alt WP are quite strange. They
-              have terrain_alt=1, but also have relative_alt=0 and
-              have been calculated to be relative to home. We need to
-              adjust for the change in home alt
-             */
-            next_WP_loc.alt += alt_change_cm;
-        }
+        fix_terrain_WP(next_WP_loc, __LINE__);
+
         // reset TECS to force the field elevation estimate to reset
         TECS_controller.offset_altitude(alt_change_cm * 0.01f);
     }
@@ -244,10 +237,6 @@ void Plane::set_target_altitude_location(const Location &loc)
     if (loc.terrain_alt && terrain.height_above_terrain(height, true)) {
         target_altitude.terrain_following = true;
         target_altitude.terrain_alt_cm = loc.alt;
-        if (!loc.relative_alt) {
-            // it has home added, remove it
-            target_altitude.terrain_alt_cm -= home.alt;
-        }
     } else {
         target_altitude.terrain_following = false;
     }
@@ -300,6 +289,7 @@ void Plane::change_target_altitude(int32_t change_cm)
     }
 #endif
 }
+
 /*
   change target altitude by a proportion of the target altitude offset
   (difference in height to next WP from previous WP). proportion
@@ -329,6 +319,46 @@ void Plane::set_target_altitude_proportion(const Location &loc, float proportion
     }
 }
 
+#if AP_TERRAIN_AVAILABLE
+/*
+  change target altitude along a path between two locations
+  (prev_WP_loc and next_WP_loc) where the second location is a terrain
+  altitude
+ */
+bool Plane::set_target_altitude_proportion_terrain(void)
+{
+    if (!next_WP_loc.terrain_alt ||
+        !next_WP_loc.relative_alt) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        return false;
+    }
+    /*
+      we first need to get the height of the terrain at prev_WP_loc
+     */
+    float prev_WP_height_terrain;
+    if (!plane.prev_WP_loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN,
+                                     prev_WP_height_terrain)) {
+        return false;
+    }
+    // and next_WP_loc alt as terrain
+    float next_WP_height_terrain;
+    if (!plane.next_WP_loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN,
+                                     next_WP_height_terrain)) {
+        return false;
+    }
+    const float p = constrain_float(plane.auto_state.wp_proportion, 0, 1);
+    Location loc = next_WP_loc;
+    const auto alt = linear_interpolate(prev_WP_height_terrain, next_WP_height_terrain,
+                                        p, 0, 1);
+
+    loc.set_alt_m(alt, Location::AltFrame::ABOVE_TERRAIN);
+
+    set_target_altitude_location(loc);
+
+    return true;
+}
+#endif // AP_TERRAIN_AVAILABLE
+
 /*
   constrain target altitude to be between two locations. Used to
   ensure we stay within two waypoints in altitude
@@ -914,3 +944,24 @@ float Plane::get_landing_height(bool &rangefinder_active)
 
     return height;
 }
+
+/*
+  if a terrain location doesn't have the relative_alt flag set
+  then fix the alt and trigger a flow of control error
+ */
+void Plane::fix_terrain_WP(Location &loc, uint32_t linenum)
+{
+    if (loc.terrain_alt && !loc.relative_alt) {
+        AP::internalerror().error(AP_InternalError::error_t::flow_of_control, linenum);
+        /*
+          we definately have a bug, now we need to guess what was
+          really meant. The lack of the relative_alt flag notionally
+          means that home.alt has been added to loc.alt, so remove it,
+          but only if it doesn't lead to a negative terrain altitude
+         */
+        if (loc.alt - home.alt > -500) {
+            loc.alt -= home.alt;
+        }
+        loc.relative_alt = true;
+    }
+}

ArduPlane/commands.cpp

@@ -33,16 +33,15 @@ void Plane::set_next_WP(const Location &loc)
         next_WP_loc.lng = current_loc.lng;
         // additionally treat zero altitude as current altitude
         if (next_WP_loc.alt == 0) {
-            next_WP_loc.alt = current_loc.alt;
-            next_WP_loc.relative_alt = false;
-            next_WP_loc.terrain_alt = false;
+            next_WP_loc.set_alt_cm(current_loc.alt, Location::AltFrame::ABSOLUTE);
         }
     }
 
+    fix_terrain_WP(next_WP_loc, __LINE__);
+
     // convert relative alt to absolute alt
-    if (next_WP_loc.relative_alt) {
-        next_WP_loc.relative_alt = false;
-        next_WP_loc.alt += home.alt;
+    if (!next_WP_loc.terrain_alt) {
+        next_WP_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
     }
 
     // are we already past the waypoint? This happens when we jump
@@ -85,6 +84,8 @@ void Plane::set_guided_WP(const Location &loc)
     // ---------------------
     next_WP_loc = loc;
 
+    fix_terrain_WP(next_WP_loc, __LINE__);
+
     // used to control FBW and limit the rate of climb
     // -----------------------------------------------
     set_target_altitude_current();

ArduPlane/commands_logic.cpp

@@ -337,6 +337,9 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm)
     auto_state.crosstrack = false;
     prev_WP_loc = current_loc;
     next_WP_loc = calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm);
+
+    fix_terrain_WP(next_WP_loc, __LINE__);
+
     setup_terrain_target_alt(next_WP_loc);
     set_target_altitude_location(next_WP_loc);
 

ArduPlane/mode.cpp

@@ -199,7 +199,13 @@ void Mode::update_target_altitude()
         // once we reach a loiter target then lock to the final
         // altitude target
         plane.set_target_altitude_location(plane.next_WP_loc);
-    } else if (plane.target_altitude.offset_cm != 0 && 
+#if AP_TERRAIN_AVAILABLE
+    } else if (plane.next_WP_loc.terrain_alt &&
+               plane.set_target_altitude_proportion_terrain()) {
+        // special case for target as terrain relative handled inside
+        // set_target_altitude_proportion_terrain
+#endif
+    } else if (plane.target_altitude.offset_cm != 0 &&
                !plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc)) {
         // control climb/descent rate
         plane.set_target_altitude_proportion(plane.next_WP_loc, 1.0f-plane.auto_state.wp_proportion);

ArduPlane/mode_guided.cpp

@@ -108,10 +108,10 @@ void ModeGuided::navigate()
 
 bool ModeGuided::handle_guided_request(Location target_loc)
 {
+    plane.fix_terrain_WP(target_loc, __LINE__);
     // add home alt if needed
-    if (target_loc.relative_alt && !target_loc.terrain_alt) {
-        target_loc.alt += plane.home.alt;
-        target_loc.relative_alt = 0;
+    if (!target_loc.terrain_alt) {
+        target_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
     }
 
     plane.set_guided_WP(target_loc);

ArduPlane/quadplane.cpp

@@ -2814,13 +2814,6 @@ void QuadPlane::vtol_position_controller(void)
                                                          0, 1);
                 }
             }
-#if AP_TERRAIN_AVAILABLE
-            float terrain_altitude_offset;
-            if (plane.next_WP_loc.terrain_alt && plane.terrain.height_terrain_difference_home(terrain_altitude_offset, true)) {
-                // Climb if current terrain is above home, target_altitude_cm is reltive to home
-                target_altitude_cm += MAX(terrain_altitude_offset*100,0);
-            }
-#endif
             float zero = 0;
             float target_z = target_altitude_cm;
             pos_control->input_pos_vel_accel_z(target_z, zero, 0);