Plane: split up altitude handling

this creates altitude.pde for altitude handling, getting ready to add
terrain following

ArduPlane/ArduPlane.pde

@@ -655,12 +655,16 @@ static struct Location guided_WP_loc;
 static struct AP_Mission::Mission_Command auto_rtl_command;
 
 ////////////////////////////////////////////////////////////////////////////////
-// Altitude / Climb rate control
+// Altitude control
-////////////////////////////////////////////////////////////////////////////////
+static struct {
-// The current desired altitude.  Altitude is linearly ramped between waypoints.  Centimeters
+    // target altitude above sea level in cm. Used for barometric
-static int32_t target_altitude_cm;
+    // altitude navigation
-// Altitude difference between previous and current waypoint.  Centimeters
+    int32_t amsl_cm;
-static int32_t offset_altitude_cm;
+
+    // Altitude difference between previous and current waypoint in
+    // centimeters. Used for glide slope handling
+    int32_t offset_cm;
+} target_altitude;
 
 ////////////////////////////////////////////////////////////////////////////////
 // INS variables
@@ -733,7 +737,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { update_compass,         5,   1200 },
     { read_airspeed,          5,   1200 },
     { update_alt,             5,   3400 },
-    { calc_altitude_error,    5,   1000 },
+    { adjust_altitude_target, 5,   1000 },
     { obc_fs_check,           5,   1000 },
     { gcs_update,             1,   1700 },
     { gcs_data_stream_send,   1,   3000 },
@@ -1413,7 +1417,7 @@ static void update_alt()
             update_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
         }
 
-        SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100), 
+        SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
                                                  target_airspeed_cm,
                                                  flight_stage,
                                                  auto_state.takeoff_pitch_cd,

ArduPlane/altitude.pde

@@ -0,0 +1,240 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  altitude handling routines
+ */
+
+/*
+  adjust altitude target depending on mode
+ */
+static void adjust_altitude_target()
+{
+    if (control_mode == FLY_BY_WIRE_B ||
+        control_mode == CRUISE) {
+        return;
+    }
+    if (nav_controller->reached_loiter_target() || (wp_distance <= 30) || (wp_totalDistance<=30)) {
+        // once we reach a loiter target then lock to the final
+        // altitude target
+        set_target_altitude_location(next_WP_loc);
+    } else if (target_altitude.offset_cm != 0) {
+        // control climb/descent rate
+        set_target_altitude_proportion(next_WP_loc, (float)(wp_distance-30) / (float)(wp_totalDistance-30));
+
+        // stay within the range of the start and end locations in altitude
+        constrain_target_altitude_location(next_WP_loc, prev_WP_loc);
+    } else if (mission.get_current_do_cmd().id != MAV_CMD_CONDITION_CHANGE_ALT) {
+        set_target_altitude_location(next_WP_loc);
+    }
+
+    altitude_error_cm = calc_altitude_error_cm();
+}
+
+/*
+  setup for a gradual glide slope to the next waypoint, if appropriate
+ */
+static void setup_glide_slope(void)
+{
+    // establish the distance we are travelling to the next waypoint,
+    // for calculating out rate of change of altitude
+    wp_totalDistance        = get_distance(current_loc, next_WP_loc);
+    wp_distance             = wp_totalDistance;
+
+    /*
+      work out if we will gradually change altitude, or try to get to
+      the new altitude as quickly as possible.
+     */
+    switch (control_mode) {
+    case RTL:
+    case GUIDED:
+        /* glide down slowly if above target altitude, but ascend more
+           rapidly if below it. See
+           https://github.com/diydrones/ardupilot/issues/39
+        */
+        if (above_location(next_WP_loc)) {
+            set_offset_altitude_location(next_WP_loc);
+        } else {
+            reset_offset_altitude();
+        }
+        break;
+
+    case AUTO:
+        // we only do glide slide handling in AUTO when above 40m or
+        // when descending. The 40 meter threshold is arbitrary, and
+        // is basically to prevent situations where we try to slowly
+        // gain height at low altitudes, potentially hitting
+        // obstacles.
+        if (relative_altitude() > 40 || !above_location(next_WP_loc)) {
+            set_offset_altitude_location(next_WP_loc);
+        } else {
+            reset_offset_altitude();
+        }
+        break;
+    default:
+        reset_offset_altitude();
+        break;
+    }
+}
+
+/*
+  return RTL altitude as AMSL altitude
+ */
+static int32_t get_RTL_altitude()
+{
+    if (g.RTL_altitude_cm < 0) {
+        return current_loc.alt;
+    }
+    return g.RTL_altitude_cm + home.alt;
+}
+
+
+/*
+  return relative altitude in meters (relative to home)
+ */
+static float relative_altitude(void)
+{
+    return (current_loc.alt - home.alt) * 0.01f;
+}
+
+/*
+  return relative altitude in centimeters, absolute value
+ */
+static int32_t relative_altitude_abs_cm(void)
+{
+    return labs(current_loc.alt - home.alt);
+}
+
+
+/*
+  set the target altitude to the current altitude. This is used when 
+  setting up for altitude hold, such as when releasing elevator in
+  CRUISE mode.
+ */
+static void set_target_altitude_current(void)
+{
+    target_altitude.amsl_cm = current_loc.alt;
+}
+
+/*
+  set the target altitude to the current altitude, with ALT_OFFSET adjustment
+ */
+static void set_target_altitude_current_adjusted(void)
+{
+    target_altitude.amsl_cm = adjusted_altitude_cm();
+}
+
+/*
+  set target altitude based on a location structure
+ */
+static void set_target_altitude_location(const Location &loc)
+{
+    target_altitude.amsl_cm = loc.alt;
+}
+
+/*
+  return relative to home target altitude in centimeters. Used for
+  altitude control libraries
+ */
+static int32_t relative_target_altitude_cm(void)
+{
+    return target_altitude.amsl_cm - home.alt + (int32_t(g.alt_offset)*100);
+}
+
+/*
+  change the current target altitude by an amount in centimeters. Used
+  to cope with changes due to elevator in CRUISE or FBWB
+ */
+static void change_target_altitude(int32_t change_cm)
+{
+    target_altitude.amsl_cm += change_cm;
+}
+
+/*
+  change target altitude by a proportion of the target altitude offset
+  (difference in height to next WP from previous WP). proportion
+  should be between 0 and 1. 
+
+  When proportion is zero we have reached the destination. When
+  proportion is 1 we are at the starting waypoint.
+
+  Note that target_altitude is setup initially based on the
+  destination waypoint
+ */
+static void set_target_altitude_proportion(const Location &loc, float proportion)
+{
+    set_target_altitude_location(loc);
+    proportion = constrain_float(proportion, 0.0f, 1.0f);
+    change_target_altitude(-target_altitude.offset_cm*proportion);
+}
+
+/*
+  constrain target altitude to be between two locations. Used to
+  ensure we stay within two waypoints in altitude
+ */
+static void constrain_target_altitude_location(const Location &loc1, const Location &loc2)
+{
+    if (loc1.alt > loc2.alt) {
+        target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc2.alt, loc1.alt);
+    } else {
+        target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc1.alt, loc2.alt);
+    }
+}
+
+/*
+  return error between target altitude and current altitude
+ */
+static int32_t calc_altitude_error_cm(void)
+{
+    return target_altitude.amsl_cm - adjusted_altitude_cm();
+}
+
+/*
+  check for FBWB_min_altitude_cm violation
+ */
+static void check_minimum_altitude(void)
+{
+    if (g.FBWB_min_altitude_cm != 0 && target_altitude.amsl_cm < home.alt + g.FBWB_min_altitude_cm) {
+        target_altitude.amsl_cm = home.alt + g.FBWB_min_altitude_cm;
+    }
+}
+
+/*
+  reset the altitude offset used for glide slopes
+ */
+static void reset_offset_altitude(void)
+{
+    target_altitude.offset_cm = 0;
+}
+
+/*
+  reset the altitude offset used for glide slopes, based on difference
+  between altitude at a destination and current altitude. If
+  destination is above the current altitude then the result is
+  positive.
+ */
+static void set_offset_altitude_location(const Location &loc)
+{
+    target_altitude.offset_cm = loc.alt - current_loc.alt;
+}
+
+/*
+  are we above the altitude given by a location?
+ */
+static bool above_location(const Location &loc)
+{
+    return current_loc.alt > loc.alt;
+}

ArduPlane/commands.pde

@@ -3,15 +3,6 @@
  *  logic for dealing with the current command in the mission and home location
  */
 
-static int32_t read_alt_to_hold()
-{
-    if (g.RTL_altitude_cm < 0) {
-        return current_loc.alt;
-    }
-    return g.RTL_altitude_cm + home.alt;
-}
-
-
 /*
  *  set_next_WP - sets the target location the vehicle should fly to
  */
@@ -56,12 +47,13 @@ static void set_next_WP(const struct Location& loc)
 
     // used to control FBW and limit the rate of climb
     // -----------------------------------------------
-    target_altitude_cm = current_loc.alt;
+    set_target_altitude_current();
 
     // zero out our loiter vals to watch for missed waypoints
     loiter_angle_reset();
 
     setup_glide_slope();
+    setup_turn_angle();
 
     loiter_angle_reset();
 }
@@ -84,9 +76,10 @@ static void set_guided_WP(void)
 
     // used to control FBW and limit the rate of climb
     // -----------------------------------------------
-    target_altitude_cm = current_loc.alt;
+    set_target_altitude_current();
 
     setup_glide_slope();
+    setup_turn_angle();
 
     loiter_angle_reset();
 }

ArduPlane/commands_logic.pde

@@ -252,7 +252,7 @@ static void do_RTL(void)
 {
     control_mode    = RTL;
     prev_WP_loc = current_loc;
-    next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold());
+    next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
 
     if (g.loiter_radius < 0) {
         loiter.direction = -1;
@@ -261,6 +261,7 @@ static void do_RTL(void)
     }
 
     setup_glide_slope();
+    setup_turn_angle();
 
     if (should_log(MASK_LOG_MODE))
         Log_Write_Mode(control_mode);
@@ -504,9 +505,10 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd)
     if (condition_value < adjusted_altitude_cm()) {
         condition_rate = -condition_rate;
     }
-    target_altitude_cm = adjusted_altitude_cm() + (condition_rate / 10);    // condition_rate is climb rate in cm/s.  We divide by 10 because this function is called at 10hz
+    set_target_altitude_current_adjusted();
+    change_target_altitude(condition_rate/10);
     next_WP_loc.alt = condition_value;                                      // For future nav calculations
-    offset_altitude_cm = 0;                                                 // For future nav calculations
+    reset_offset_altitude();
 }
 
 static void do_within_distance(const AP_Mission::Mission_Command& cmd)
@@ -534,7 +536,9 @@ static bool verify_change_alt()
         condition_value = 0;
         return true;
     }
-    target_altitude_cm += condition_rate / 10;  // condition_rate is climb rate in cm/s.  We divide by 10 because this function is called at 10hz
+    // condition_rate is climb rate in cm/s.  
+    // We divide by 10 because this function is called at 10hz
+    change_target_altitude(condition_rate/10);
     return false;
 }
 
@@ -632,9 +636,10 @@ static void exit_mission_callback()
         gcs_send_text_fmt(PSTR("Returning to Home"));
         memset(&auto_rtl_command, 0, sizeof(auto_rtl_command));
         auto_rtl_command.content.location = 
-            rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold());
+            rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
         auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM;
         setup_glide_slope();
+        setup_turn_angle();
         start_command(auto_rtl_command);
     }
 }

ArduPlane/geofence.pde

@@ -341,7 +341,7 @@ static void geofence_check(bool altitude_check_only)
     case FENCE_ACTION_GUIDED:
     case FENCE_ACTION_GUIDED_THR_PASS:
         if (g.fence_ret_rally != 0) { //return to a rally point
-            guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold());
+            guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
 
         } else { //return to fence return point, not a rally point
             if (g.fence_retalt > 0) {

ArduPlane/navigation.pde

@@ -123,33 +123,6 @@ static void calc_gndspeed_undershoot()
     }
 }
 
-static void calc_altitude_error()
-{
-    if (control_mode == FLY_BY_WIRE_B ||
-        control_mode == CRUISE) {
-        return;
-    }
-    if (nav_controller->reached_loiter_target() || (wp_distance <= 30) || (wp_totalDistance<=30)) {
-        // once we reach a loiter target then lock to the final
-        // altitude target
-        target_altitude_cm = next_WP_loc.alt;
-    } else if (offset_altitude_cm != 0) {
-        // control climb/descent rate
-        target_altitude_cm = next_WP_loc.alt - (offset_altitude_cm*((float)(wp_distance-30) / (float)(wp_totalDistance-30)));
-
-        // stay within the range of the start and end locations in altitude
-        if (prev_WP_loc.alt > next_WP_loc.alt) {
-            target_altitude_cm = constrain_int32(target_altitude_cm, next_WP_loc.alt, prev_WP_loc.alt);
-        } else {
-            target_altitude_cm = constrain_int32(target_altitude_cm, prev_WP_loc.alt, next_WP_loc.alt);
-        }
-    } else if (mission.get_current_do_cmd().id != MAV_CMD_CONDITION_CHANGE_ALT) {
-        target_altitude_cm = next_WP_loc.alt;
-    }
-
-    altitude_error_cm       = target_altitude_cm - adjusted_altitude_cm();
-}
-
 static void update_loiter()
 {
     nav_controller->update_loiter(next_WP_loc, abs(g.loiter_radius), loiter.direction);
@@ -205,19 +178,18 @@ static void update_fbwb_speed_height(void)
         elevator_input = -elevator_input;
     }
     
-    target_altitude_cm += g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f;
+    change_target_altitude(g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f);
     
     if (elevator_input == 0.0f && last_elevator_input != 0.0f) {
         // the user has just released the elevator, lock in
         // the current altitude
-        target_altitude_cm = current_loc.alt;
+        set_target_altitude_current();
     }
 
     // check for FBWB altitude limit
-    if (g.FBWB_min_altitude_cm != 0 && target_altitude_cm < home.alt + g.FBWB_min_altitude_cm) {
+    check_minimum_altitude();
-        target_altitude_cm = home.alt + g.FBWB_min_altitude_cm;
+
-    }
+    altitude_error_cm = calc_altitude_error_cm();
-    altitude_error_cm = target_altitude_cm - adjusted_altitude_cm();
     
     last_elevator_input = elevator_input;
     
@@ -225,56 +197,6 @@ static void update_fbwb_speed_height(void)
     calc_nav_pitch();
 }
 
-/*
-  setup for a gradual glide slope to the next waypoint, if appropriate
- */
-static void setup_glide_slope(void)
-{
-    // establish the distance we are travelling to the next waypoint,
-    // for calculating out rate of change of altitude
-    wp_totalDistance        = get_distance(current_loc, next_WP_loc);
-    wp_distance             = wp_totalDistance;
-
-    /*
-      work out if we will gradually change altitude, or try to get to
-      the new altitude as quickly as possible.
-     */
-    switch (control_mode) {
-    case RTL:
-    case GUIDED:
-        /* glide down slowly if above target altitude, but ascend more
-           rapidly if below it. See
-           https://github.com/diydrones/ardupilot/issues/39
-        */
-        if (current_loc.alt > next_WP_loc.alt) {
-            offset_altitude_cm = next_WP_loc.alt - current_loc.alt;
-        } else {
-            offset_altitude_cm = 0;
-        }
-        break;
-
-    case AUTO:
-        // we only do glide slide handling in AUTO when above 40m or
-        // when descending. The 40 meter threshold is arbitrary, and
-        // is basically to prevent situations where we try to slowly
-        // gain height at low altitudes, potentially hitting
-        // obstacles.
-        if (relative_altitude() > 40 || next_WP_loc.alt < current_loc.alt) {
-            offset_altitude_cm = next_WP_loc.alt - current_loc.alt;
-        } else {
-            offset_altitude_cm = 0;        
-        }
-        break;
-    default:
-        offset_altitude_cm = 0;        
-        break;
-    }
-
-
-    // calculate turn angle for next leg
-    setup_turn_angle();
-}
-
 /*
   calculate the turn angle for the next leg of the mission
  */
@@ -293,19 +215,3 @@ static void setup_turn_angle(void)
     }
 }    
 
-/*
-  return relative altitude in meters (relative to home)
- */
-static float relative_altitude(void)
-{
-    return (current_loc.alt - home.alt) * 0.01f;
-}
-
-/*
-  return relative altitude in centimeters, absolute value
- */
-static int32_t relative_altitude_abs_cm(void)
-{
-    return labs(current_loc.alt - home.alt);
-}
-

ArduPlane/system.pde

@@ -322,12 +322,12 @@ static void set_mode(enum FlightMode mode)
         auto_throttle_mode = true;
         cruise_state.locked_heading = false;
         cruise_state.lock_timer_ms = 0;
-        target_altitude_cm = current_loc.alt;
+        set_target_altitude_current();
         break;
 
     case FLY_BY_WIRE_B:
         auto_throttle_mode = true;
-        target_altitude_cm = current_loc.alt;
+        set_target_altitude_current();
         break;
 
     case CIRCLE: