Copter: sport mode restructured to match althold and feedback from mid-stick

This modifies sport mode to be structured like althold and loiter flight modes so that ongoing maintenance of the modes is hopefully easier.
Also changes throttle feedback to occur from mid-stick

ArduCopter/control_sport.cpp

@@ -24,30 +24,21 @@ bool Copter::sport_init(bool ignore_checks)
 // should be called at 100hz or more
 void Copter::sport_run()
 {
-    float target_roll_rate, target_pitch_rate, target_yaw_rate;
+    SportModeState sport_state;
-    float target_climb_rate = 0;
     float takeoff_climb_rate = 0.0f;
 
     // initialize vertical speed and acceleration
     pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
     pos_control.set_accel_z(g.pilot_accel_z);
 
-    // if not armed or throttle at zero, set throttle to zero and exit immediately
-    if (!motors.armed() || ap.throttle_zero || !motors.get_interlock()) {
-        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
-        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
-        pos_control.relax_alt_hold_controllers(0.0f);
-        return;
-    }
-
     // apply SIMPLE mode transform
     update_simple_mode();
 
     // get pilot's desired roll and pitch rates
 
     // calculate rate requests
-    target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p;
+    float target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p;
-    target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p;
+    float target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p;
 
     int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
     target_roll_rate -= constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
@@ -69,41 +60,91 @@ void Copter::sport_run()
     }
 
     // get pilot's desired yaw rate
-    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
 
     // get pilot desired climb rate
-    target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
+    float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
     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
+#if FRAME_CONFIG == HELI_FRAME
-    takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
+    // helicopters are held on the ground until rotor speed runup has finished
+    bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete());
+#else
+    bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
+#endif
 
-    // check for take-off
+    // State Machine Determination
-    if (ap.land_complete && (takeoff_state.running || (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()))) {
+    if (!motors.armed() || !motors.get_interlock()) {
-        if (!takeoff_state.running) {
+        sport_state = Sport_MotorStopped;
-            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
+    } else if (takeoff_state.running || takeoff_triggered) {
-        }
+        sport_state = Sport_Takeoff;
-
+    } else if (!ap.auto_armed || ap.land_complete) {
-        // indicate we are taking off
+        sport_state = Sport_Landed;
-        set_land_complete(false);
+    } else {
-        // clear i term when we're taking off
+        sport_state = Sport_Flying;
-        set_throttle_takeoff();
     }
 
-    // reset target lean angles and heading while landed
+    // State Machine
-    if (ap.land_complete) {
+    switch (sport_state) {
-        if (ap.throttle_zero) {
+
-            motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
+    case Sport_MotorStopped:
-        }else{
+
-            motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+        motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
-        }
+        attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
-        // move throttle to between minimum and non-takeoff-throttle to keep us on the ground
+#if FRAME_CONFIG == HELI_FRAME
-        attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
+        // force descent rate and call position controller
-        pos_control.relax_alt_hold_controllers(0.0f);
+        pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
-    }else{
+#else
+        attitude_control.relax_attitude_controllers();
+        attitude_control.reset_rate_controller_I_terms();
+        attitude_control.set_yaw_target_to_current_heading();
+        pos_control.relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
+#endif
+        pos_control.update_z_controller();
+        break;
+
+    case Sport_Takeoff:
         // set motors to full range
         motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
+        // initiate take-off
+        if (!takeoff_state.running) {
+            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
+            // indicate we are taking off
+            set_land_complete(false);
+            // clear i terms
+            set_throttle_takeoff();
+        }
+
+        // get take-off adjusted pilot and takeoff climb rates
+        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
+
+        // call attitude controller
+        attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
+
+        // call position controller
+        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
+        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
+        pos_control.update_z_controller();
+        break;
+
+    case Sport_Landed:
+        // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
+        if (target_climb_rate < 0.0f) {
+            motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
+        } else {
+            motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+        }
+
+        attitude_control.reset_rate_controller_I_terms();
+        attitude_control.set_yaw_target_to_current_heading();
+        attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
+        pos_control.relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
+        pos_control.update_z_controller();
+        break;
+
+    case Sport_Flying:
+        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
         // call attitude controller
         attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
 
@@ -115,7 +156,7 @@ void Copter::sport_run()
 
         // call position controller
         pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
-        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         pos_control.update_z_controller();
+        break;
     }
 }

ArduCopter/defines.h

@@ -238,6 +238,14 @@ enum LoiterModeState {
     Loiter_Landed
 };
 
+// Sport states
+enum SportModeState {
+    Sport_MotorStopped,
+    Sport_Takeoff,
+    Sport_Flying,
+    Sport_Landed
+};
+
 // Flip states
 enum FlipState {
     Flip_Start,