Plane: never allow terrain_alt=1 and relative_alt=0

the Location object does not handle this combination. Throw an
internal error and try to cope as best we can

Author: tridge

ArduPlane/GCS_MAVLink_Plane.cpp

@@ -758,12 +758,15 @@ 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;
+    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();
 }
 
@@ -873,10 +876,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
         plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
 #endif
 
-        // 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.cpp

@@ -891,9 +891,8 @@ bool Plane::set_target_location(const Location &target_loc)
         return false;
     }
     // add home alt if needed
-    if (loc.relative_alt && !loc.terrain_alt) {
-        loc.alt += plane.home.alt;
-        loc.relative_alt = 0;
+    if (!loc.terrain_alt) {
+        loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
     }
     plane.set_guided_WP(loc);
     return true;
@@ -941,6 +940,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/Plane.h

@@ -903,7 +903,7 @@ class Plane : public AP_Vehicle {
     void change_target_altitude(int32_t change_cm);
     void set_target_altitude_proportion(const Location &loc, float proportion);
 #if AP_TERRAIN_AVAILABLE
-    void set_target_altitude_proportion_terrain(float prev_theight);
+    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);
@@ -917,6 +917,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,12 +34,12 @@ 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) {
+        if (next_WP_loc.terrain_alt && !next_WP_loc.relative_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
+              have terrain_alt=1, but could (due to a bug!) 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;
         }
@@ -235,10 +235,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;
     }
@@ -322,24 +318,45 @@ void Plane::set_target_altitude_proportion(const Location &loc, float proportion
     }
 }
 
+#if AP_TERRAIN_AVAILABLE
 /*
-  change target altitude along a path between two terrain altitudes
-  in prev_WP_loc and next_WP_loc using plane.auto_state.wp_proportion
-  next_WP_loc must be using terrain_alt
-
-  prev_theight must be the height of the terrain in meters at
-  prev_WP_loc
+  change target altitude along a path between two locations
+  (prev_WP_loc and next_WP_loc) where the second location is a terrain
+  altitude
  */
-void Plane::set_target_altitude_proportion_terrain(float prev_theight)
+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);
-    float prev_WP_height_abs = prev_WP_loc.alt*0.01;
-    float prev_WP_height_terrain = prev_WP_height_abs - prev_theight;
     Location loc = next_WP_loc;
-    loc.alt = linear_interpolate(prev_WP_height_terrain*100, next_WP_loc.alt,
-                                 p, 0, 1);
+    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
@@ -927,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

@@ -340,6 +340,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

@@ -186,7 +186,6 @@ bool Mode::is_vtol_man_throttle() const
 void Mode::update_target_altitude()
 {
     Location target_location;
-    float prev_theight;
 
     if (plane.landing.is_flaring()) {
         // during a landing flare, use TECS_LAND_SINK as a target sink
@@ -211,13 +210,9 @@ void Mode::update_target_altitude()
         plane.set_target_altitude_location(plane.next_WP_loc);
 #if AP_TERRAIN_AVAILABLE
     } else if (plane.next_WP_loc.terrain_alt &&
-               plane.next_WP_loc.relative_alt &&
-               !plane.prev_WP_loc.relative_alt &&
-               !plane.prev_WP_loc.terrain_alt &&
-               plane.terrain.height_amsl(plane.prev_WP_loc, prev_theight)) {
-        // special case for target as terrain relative, and
-        // prev_WP_loc absolute
-        plane.set_target_altitude_proportion_terrain(prev_theight);
+               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)) {

ArduPlane/mode_autoland.cpp

@@ -177,7 +177,7 @@ bool ModeAutoLand::_enter()
 #if AP_TERRAIN_AVAILABLE
     // Update loiter location to be relative terrain if enabled
     if (plane.terrain_enabled_in_current_mode()) {
-        cmd_loiter.content.location.terrain_alt = 1;
+        cmd_loiter.content.location.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_TERRAIN);
     };
 #endif
     // land WP at home

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