Plane: use a glide slope in RTL when descending

when above the target altitude in RTL come down slowly, when below
climb rapidly

This fixes issue #39

ArduPlane/commands.pde

@@ -184,20 +184,10 @@ static void set_next_WP(const struct Location *wp)
     // -----------------------------------------------
     target_altitude_cm = current_loc.alt;
 
-    if (prev_WP.id != MAV_CMD_NAV_TAKEOFF && 
-        prev_WP.alt != home.alt && 
-        (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND)) {
-        offset_altitude_cm = next_WP.alt - prev_WP.alt;
-    } else {
-        offset_altitude_cm = 0;        
-    }
-
     // zero out our loiter vals to watch for missed waypoints
     loiter_angle_reset();
 
-    // this is handy for the groundstation
-    wp_totalDistance        = get_distance(&current_loc, &next_WP);
-    wp_distance             = wp_totalDistance;
+    setup_glide_slope();
 
     loiter_angle_reset();
 }
@@ -221,11 +211,8 @@ static void set_guided_WP(void)
     // used to control FBW and limit the rate of climb
     // -----------------------------------------------
     target_altitude_cm = current_loc.alt;
-    offset_altitude_cm = next_WP.alt - prev_WP.alt;
 
-    // this is handy for the groundstation
-    wp_totalDistance        = get_distance(&current_loc, &next_WP);
-    wp_distance             = wp_totalDistance;
+    setup_glide_slope();
 
     loiter_angle_reset();
 }

ArduPlane/commands_logic.pde

@@ -232,7 +232,8 @@ static bool verify_condition_command()          // Returns true if command compl
 static void do_RTL(void)
 {
     control_mode    = RTL;
-    next_WP                 = home;
+    prev_WP = current_loc;
+    next_WP = home;
 
     if (g.loiter_radius < 0) {
         loiter.direction = -1;
@@ -245,6 +246,8 @@ static void do_RTL(void)
     // -------------------------
     next_WP.alt = read_alt_to_hold();
 
+    setup_glide_slope();
+
     if (g.log_bitmask & MASK_LOG_MODE)
         Log_Write_Mode(control_mode);
 }

ArduPlane/navigation.pde

@@ -132,12 +132,12 @@ static void calc_altitude_error()
     if (control_mode == FLY_BY_WIRE_B) {
         return;
     }
-    if (control_mode == AUTO && offset_altitude_cm != 0) {
-        // limit climb rates
+    if (offset_altitude_cm != 0) {
+        // control climb/descent rate
         target_altitude_cm = next_WP.alt - (offset_altitude_cm*((float)(wp_distance-30) / (float)(wp_totalDistance-30)));
 
         // stay within a certain range
-        if(prev_WP.alt > next_WP.alt) {
+        if (prev_WP.alt > next_WP.alt) {
             target_altitude_cm = constrain_int32(target_altitude_cm, next_WP.alt, prev_WP.alt);
         }else{
             target_altitude_cm = constrain_int32(target_altitude_cm, prev_WP.alt, next_WP.alt);
@@ -154,3 +154,42 @@ static void update_loiter()
     nav_controller->update_loiter(next_WP, abs(g.loiter_radius), loiter.direction);
 }
 
+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);
+    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.alt) {
+            offset_altitude_cm = next_WP.alt - current_loc.alt;            
+        } else {
+            offset_altitude_cm = 0;
+        }
+        break;
+
+    case AUTO:
+        if (prev_WP.id != MAV_CMD_NAV_TAKEOFF && 
+            prev_WP.alt != home.alt && 
+            (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND)) {
+            offset_altitude_cm = next_WP.alt - prev_WP.alt;
+        } else {
+            offset_altitude_cm = 0;        
+        }
+        break;
+    default:
+        offset_altitude_cm = 0;        
+        break;
+    }
+}