Copter: take-off uses PosControl's add_takeoff method

Also balances pilot and takeoff climb rates
Also removes takeoff's direct use of poscontrol target

Combined effort of Randy and Leonard

ArduCopter/control_althold.pde

@@ -24,6 +24,7 @@ static void althold_run()
     float target_roll, target_pitch;
     float target_yaw_rate;
     float target_climb_rate;
+    float takeoff_climb_rate = 0.0f;
 
     // if not auto armed set throttle to zero and exit immediately
     if(!ap.auto_armed) {
@@ -44,7 +45,10 @@ static void althold_run()
 
     // get pilot desired climb rate
     target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
-    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-takeoff_get_speed(), g.pilot_velocity_z_max-takeoff_get_speed());
+    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+
+    // get takeoff adjusted pilot and takeoff climb rates
+    takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
 
     // check for take-off
     if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) {
@@ -75,7 +79,7 @@ static void althold_run()
 
         // call position controller
         pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
-        takeoff_increment_alt_target(G_Dt);
+        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         pos_control.update_z_controller();
     }
 }

ArduCopter/control_loiter.pde

@@ -31,6 +31,7 @@ static void loiter_run()
 {
     float target_yaw_rate = 0;
     float target_climb_rate = 0;
+    float takeoff_climb_rate = 0.0f;
 
     // if not auto armed set throttle to zero and exit immediately
     if(!ap.auto_armed) {
@@ -53,7 +54,10 @@ static void loiter_run()
 
         // get pilot desired climb rate
         target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
-        target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-takeoff_get_speed(), g.pilot_velocity_z_max-takeoff_get_speed());
+        target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+
+        // get takeoff adjusted pilot and takeoff climb rates
+        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
 
         // check for take-off
         if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) {
@@ -99,7 +103,7 @@ static void loiter_run()
 
         // update altitude target and call position controller
         pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
-        takeoff_increment_alt_target(G_Dt);
+        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         pos_control.update_z_controller();
     }
 }

ArduCopter/control_poshold.pde

@@ -146,7 +146,8 @@ static void poshold_run()
 {
     float target_roll, target_pitch;  // pilot's roll and pitch angle inputs
     float target_yaw_rate = 0;          // pilot desired yaw rate in centi-degrees/sec
-    int16_t target_climb_rate = 0;      // pilot desired climb rate in centimeters/sec
+    float target_climb_rate = 0;      // pilot desired climb rate in centimeters/sec
+    float takeoff_climb_rate = 0.0f;    // takeoff induced climb rate
     float brake_to_loiter_mix;          // mix of brake and loiter controls.  0 = fully brake controls, 1 = fully loiter controls
     float controller_to_pilot_roll_mix; // mix of controller and pilot controls.  0 = fully last controller controls, 1 = fully pilot controls
     float controller_to_pilot_pitch_mix;    // mix of controller and pilot controls.  0 = fully last controller controls, 1 = fully pilot controls
@@ -171,7 +172,10 @@ static void poshold_run()
 
         // get pilot desired climb rate (for alt-hold mode and take-off)
         target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
-        target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-takeoff_get_speed(), g.pilot_velocity_z_max-takeoff_get_speed());
+        target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+
+        // get takeoff adjusted pilot and takeoff climb rates
+        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
 
         // check for take-off
         if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) {
@@ -526,7 +530,7 @@ static void poshold_run()
         }
         // update altitude target and call position controller
         pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
-        takeoff_increment_alt_target(G_Dt);
+        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         pos_control.update_z_controller();
     }
 }

ArduCopter/control_sport.pde

@@ -23,6 +23,7 @@ static void sport_run()
 {
     float target_roll_rate, target_pitch_rate, target_yaw_rate;
     float target_climb_rate = 0;
+    float takeoff_climb_rate = 0.0f;
 
     // if not armed or throttle at zero, set throttle to zero and exit immediately
     if(!motors.armed() || g.rc_3.control_in <= 0) {
@@ -64,7 +65,10 @@ static void sport_run()
 
     // get pilot desired climb rate
     target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
-    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-takeoff_get_speed(), g.pilot_velocity_z_max-takeoff_get_speed());
+    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+
+    // get takeoff adjusted pilot and takeoff climb rates
+    takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
 
     // check for take-off
     if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) {
@@ -96,7 +100,7 @@ static void sport_run()
 
         // call position controller
         pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
-        takeoff_increment_alt_target(G_Dt);
+        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         pos_control.update_z_controller();
     }
 }

ArduCopter/flight_mode.pde

@@ -129,8 +129,6 @@ static bool set_mode(uint8_t mode)
 // called at 100hz or more
 static void update_flight_mode()
 {
-    takeoff_timer_update();
-
 #if AP_AHRS_NAVEKF_AVAILABLE
     // Update EKF speed limit - used to limit speed when we are using optical flow
     ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);

ArduCopter/takeoff.pde

@@ -36,7 +36,7 @@ static bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
             case ALT_HOLD:
             case SPORT:
                 set_auto_armed(true);
-                takeoff_timer_start(pv_alt_above_origin(takeoff_alt_cm)-pos_control.get_pos_target().z);
+                takeoff_timer_start(takeoff_alt_cm);
                 return true;
         }
     }
@@ -68,27 +68,51 @@ static void takeoff_stop()
     takeoff_state.start_ms = 0;
 }
 
-// update takeoff timer and stop the takeoff after time_ms has passed
-static void takeoff_timer_update()
+// returns pilot and takeoff climb rates
+//  pilot_climb_rate is both an input and an output
+//  takeoff_climb_rate is only an output
+//  has side-effect of turning takeoff off when timeout as expired
+static void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate)
 {
-    if (millis()-takeoff_state.start_ms >= takeoff_state.time_ms) {
-        takeoff_state.running = false;
+    // return pilot_climb_rate if take-off inactive
+    if (!takeoff_state.running) {
+        takeoff_climb_rate = 0.0f;
+        return;
+    }
+
+    // check if timeout as expired
+    if ((millis()-takeoff_state.start_ms) >= takeoff_state.time_ms) {
+        takeoff_stop();
+        takeoff_climb_rate = 0.0f;
+        return;
+    }
+
+    // if takeoff climb rate is zero return
+    if (takeoff_state.speed <= 0.0f) {
+        takeoff_climb_rate = 0.0f;
+        return;
+    }
+
+    // default take-off climb rate to maximum speed
+    takeoff_climb_rate = takeoff_state.speed;
+
+    // if pilot's commands descent
+    if (pilot_climb_rate < 0.0f) {
+        // if overall climb rate is still positive, move to take-off climb rate
+        if (takeoff_climb_rate + pilot_climb_rate > 0.0f) {
+            takeoff_climb_rate = takeoff_climb_rate + pilot_climb_rate;
+            pilot_climb_rate = 0;
+        } else {
+            // if overall is negative, move to pilot climb rate
+            pilot_climb_rate = pilot_climb_rate + takeoff_climb_rate;
+            takeoff_climb_rate = 0.0f;
+        }
+    } else { // pilot commands climb
+        // pilot climb rate is zero until it surpasses the take-off climb rate
+        if (pilot_climb_rate > takeoff_climb_rate) {
+            pilot_climb_rate = pilot_climb_rate - takeoff_climb_rate;
+        } else {
+            pilot_climb_rate = 0.0f;
+        }
     }
 }
-
-// increase altitude target as part of takeoff
-//   dt - time interval (in seconds) that this function is called
-static void takeoff_increment_alt_target(float dt)
-{
-    if (takeoff_state.running) {
-        Vector3f pos_target = pos_control.get_pos_target();
-        pos_target.z += takeoff_state.speed*dt;
-        pos_control.set_pos_target(pos_target);
-    }
-}
-
-// get take off speed in cm/s
-static float takeoff_get_speed()
-{
-    return takeoff_state.running?takeoff_state.speed:0.0f;
-}