Plane: added 4 new parameters to control takeoff

this gives flexible control for taildragger takeoff

ArduPlane/ArduPlane.pde

@@ -511,7 +511,6 @@ static struct {
     locked_course_err : 0
 };
 
-
 ////////////////////////////////////////////////////////////////////////////////
 // flight mode specific
 ////////////////////////////////////////////////////////////////////////////////
@@ -527,11 +526,16 @@ static struct {
 
     // Minimum pitch to hold during takeoff command execution.  Hundredths of a degree
     int16_t takeoff_pitch_cd;
+
+    // the highest airspeed we have reached since entering AUTO. Used
+    // to control ground takeoff
+    float highest_airspeed;
 } auto_state = {
     takeoff_complete : true,
     land_complete : false,
     takeoff_altitude_cm : 0,
-    takeoff_pitch_cd : 0
+    takeoff_pitch_cd : 0,
+    highest_airspeed : 0
 };
 
 // true if we are in an auto-throttle mode, which means
@@ -1106,25 +1110,8 @@ static void handle_auto_mode(void)
 
     switch(nav_cmd_id) {
     case MAV_CMD_NAV_TAKEOFF:
-        if (steer_state.hold_course_cd == -1) {
-            // we don't yet have a heading to hold - just level
-            // the wings until we get up enough speed to get a GPS heading
-            nav_roll_cd = 0;
-        } else {
-            calc_nav_roll();
-            // during takeoff use the level flight roll limit to
-            // prevent large course corrections
-            nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
-        }
-        
-        if (airspeed.use()) {
-            calc_nav_pitch();
-            if (nav_pitch_cd < auto_state.takeoff_pitch_cd)
-                nav_pitch_cd = auto_state.takeoff_pitch_cd;
-        } else {
-            nav_pitch_cd = ((gps.ground_speed()*100) / (float)g.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd;
-            nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd);
-        }
+        takeoff_calc_roll();
+        takeoff_calc_pitch();
         
         // max throttle for takeoff
         channel_throttle->servo_out = aparm.throttle_max;

ArduPlane/Attitude.pde

@@ -14,6 +14,9 @@ static float get_speed_scaler(void)
 {
     float aspeed, speed_scaler;
     if (ahrs.airspeed_estimate(&aspeed)) {
+        if (aspeed > auto_state.highest_airspeed) {
+            auto_state.highest_airspeed = aspeed;
+        }
         if (aspeed > 0) {
             speed_scaler = g.scaling_speed / aspeed;
         } else {
@@ -94,6 +97,13 @@ static void stabilize_roll(float speed_scaler)
  */
 static void stabilize_pitch(float speed_scaler)
 {
+    int8_t force_elevator = takeoff_tail_hold();
+    if (force_elevator != 0) {
+        // we are holding the tail down during takeoff. Just covert
+        // from a percentage to a -4500..4500 centidegree angle
+        channel_pitch->servo_out = 45*force_elevator;
+        return;
+    }
     int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->servo_out * g.kff_throttle_to_pitch;
     bool disable_integrator = false;
     if (control_mode == STABILIZE && channel_pitch->control_in != 0) {
@@ -464,10 +474,14 @@ static void calc_nav_roll()
 *****************************************/
 static void throttle_slew_limit(int16_t last_throttle)
 {
+    uint8_t slewrate = aparm.throttle_slewrate;
+    if (control_mode==AUTO && auto_state.takeoff_complete == false) {
+        slewrate = g.takeoff_throttle_slewrate;
+    }
     // if slew limit rate is set to zero then do not slew limit
     if (aparm.throttle_slewrate) {                   
         // limit throttle change by the given percentage per second
-        float temp = aparm.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
+        float temp = slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
         // allow a minimum change of 1 PWM per cycle
         if (temp < 1) {
             temp = 1;

ArduPlane/Parameters.h

@@ -110,6 +110,10 @@ public:
         k_param_serial0_baud,
         k_param_serial1_baud,
         k_param_serial2_baud,
+        k_param_takeoff_tdrag_elevator,
+        k_param_takeoff_tdrag_speed1,
+        k_param_takeoff_rotate_speed,
+        k_param_takeoff_throttle_slewrate,
 
         // 100: Arming parameters
         k_param_arming = 100,
@@ -429,6 +433,10 @@ public:
     AP_Float takeoff_throttle_min_speed;
     AP_Float takeoff_throttle_min_accel;
     AP_Int8 takeoff_throttle_delay;
+    AP_Int8 takeoff_tdrag_elevator;
+    AP_Float takeoff_tdrag_speed1;
+    AP_Float takeoff_rotate_speed;
+    AP_Int8 takeoff_throttle_slewrate;
     AP_Int8 level_roll_limit;
     AP_Int8 flapin_channel;
     AP_Int8 flaperon_output;

ArduPlane/Parameters.pde

@@ -142,6 +142,42 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: User
     GSCALAR(takeoff_throttle_delay,     "TKOFF_THR_DELAY",  2),
 
+    // @Param: TKOFF_TDRAG_ELEV
+    // @DisplayName: Takeoff tail dragger elevator
+    // @Description: This parameter sets the amount of elevator to apply during the initial stage of a takeoff. It is used to hold the tail wheel of a taildragger on the ground during the initial takeoff stage to give maximum steering. This option should be conbined with the TKOFF_TDRAG_SPD1 option and the GROUND_STEER_ALT option along with tuning of the ground steering controller. A value of zero means to bypass the initial "tail hold" stage of takeoff. Set to zero for hand and catapult launch. For tail-draggers you should normally set this to 100, meaning full up elevator during the initial stage of takeoff. For most tricycle undercarriage aircraft a value of zero will work well, but for some tricycle aircraft a negative value will apply down elevator which will hold the nose wheel firmly on the ground during initial acceleration.
+    // @Units: Percent
+    // @Range: -100 100
+    // @Increment: 1
+    // @User: User
+    GSCALAR(takeoff_tdrag_elevator,     "TKOFF_TDRAG_ELEV",  0),
+
+    // @Param: TKOFF_TDRAG_SPD1
+    // @DisplayName: Takeoff tail dragger speed1
+    // @Description: This parameter sets the airspeed at which to stop holding the tail down and transition to rudder control of steering on the ground. When TKOFF_TDRAG_SPD1 is reached the pitch of the aircraft will be held level until TKOFF_ROTATE_SPD is reached, at which point the takeoff pitch specified in the mission will be used to "rotate" the pitch for takeoff climb. Set TKOFF_TDRAG_SPD1 to zero to go straight to rotation. This should be set to zero for hand launch and catapult launch. It should also be set to zero for tricycle undercarriages. For tail dragger aircraft it should be set just below the stall speed.
+    // @Units: m/s
+    // @Range: 0 30
+    // @Increment: 0.1
+    // @User: User
+    GSCALAR(takeoff_tdrag_speed1,     "TKOFF_TDRAG_SPD1",  0),
+
+    // @Param: TKOFF_ROTATE_SPD
+    // @DisplayName: Takeoff rotate speed
+    // @Description: This parameter sets the airspeed at which the aircraft will "rotate", setting climb pitch specified in the mission. If TKOFF_ROTATE_SPD is zero then the climb pitch will be used as soon as takeoff is started. For hand launch and catapult launches a TKOFF_ROTATE_SPD of zero should be set. For all ground launches TKOFF_ROTATE_SPD should be set above the stall speed, usually by about 2 to 3 meters/second.
+    // @Units: m/s
+    // @Range: 0 30
+    // @Increment: 0.1
+    // @User: User
+    GSCALAR(takeoff_rotate_speed,     "TKOFF_ROTATE_SPD",  0),
+
+    // @Param: TKOFF_THR_SLEW
+    // @DisplayName: Takeoff throttle slew rate
+    // @Description: This parameter sets the slew rate for the throttle during auto takeoff. When this is zero the THR_SLEWRATE parameter is used during takeoff. For rolling takeoffs it can be a good idea to set a lower slewrate for takeoff to give a slower acceleration which can improve ground steering control. The value is a percentage throttle change per second, so a value of 20 means to advance the throttle over 5 seconds on takeoff.
+    // @Units: percent
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: User
+    GSCALAR(takeoff_throttle_slewrate, "TKOFF_THR_SLEW",  0),
+
     // @Param: LEVEL_ROLL_LIMIT
     // @DisplayName: Level flight roll limit
     // @Description: This controls the maximum bank angle in degrees during flight modes where level flight is desired, such as in the final stages of landing, and during auto takeoff. This should be a small angle (such as 5 degrees) to prevent a wing hitting the runway during takeoff or landing. Setting this to zero will completely disable heading hold on auto takeoff and final landing approach.

ArduPlane/system.pde

@@ -336,6 +336,7 @@ static void set_mode(enum FlightMode mode)
     case AUTO:
         auto_throttle_mode = true;
         next_WP_loc = prev_WP_loc = current_loc;
+        auto_state.highest_airspeed = 0;
         // start or resume the mission, based on MIS_AUTORESET
         mission.start_or_resume();
         break;

ArduPlane/takeoff.pde

@@ -81,3 +81,67 @@ no_launch:
     return false;
 }
 
+/*
+  calculate desired bank angle during takeoff, setting nav_roll_cd
+ */
+static void takeoff_calc_roll(void)
+{
+    if (steer_state.hold_course_cd == -1) {
+        // we don't yet have a heading to hold - just level
+        // the wings until we get up enough speed to get a GPS heading
+        nav_roll_cd = 0;
+        return;
+    }
+
+    calc_nav_roll();
+
+    // during takeoff use the level flight roll limit to
+    // prevent large course corrections
+    nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
+}
+
+        
+/*
+  calculate desired pitch angle during takeoff, setting nav_pitch_cd
+ */
+static void takeoff_calc_pitch(void)
+{
+    if (auto_state.highest_airspeed < g.takeoff_rotate_speed) {
+        // we have not reached rotate speed, use a target pitch of
+        // zero, holding the plane level until we reach rotate speed
+        nav_pitch_cd = 0;
+        return;
+    }
+
+    if (airspeed.use()) {
+        calc_nav_pitch();
+        if (nav_pitch_cd < auto_state.takeoff_pitch_cd) {
+            nav_pitch_cd = auto_state.takeoff_pitch_cd;
+        }
+    } else {
+        nav_pitch_cd = ((gps.ground_speed()*100) / (float)g.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd;
+        nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd);
+    }
+}
+
+/*
+  return a tail hold percentage during initial takeoff for a tail dragger
+ */
+static int8_t takeoff_tail_hold(void)
+{
+    if (control_mode != AUTO || auto_state.takeoff_complete) {
+        // not in takeoff
+        return 0;
+    }
+    if (g.takeoff_tdrag_elevator == 0) {
+        // no takeoff elevator set
+        return 0;
+    }
+    if (auto_state.highest_airspeed >= g.takeoff_tdrag_speed1) {
+        // we've passed speed1. We now raise the tail and aim for
+        // level pitch. Return 0 meaning no fixed elevator setting
+        return 0;
+    }
+    // we are holding the tail down
+    return g.takeoff_tdrag_elevator;
+}