diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index d0bc7e08d5..06d75866e0 100644 --- a/ArduCopter/control_sport.cpp +++ b/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; - float target_climb_rate = 0; + SportModeState sport_state; 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; - target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p; + float target_roll_rate = channel_roll->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 - takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); +#if FRAME_CONFIG == HELI_FRAME + // 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 - if (ap.land_complete && (takeoff_state.running || (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()))) { - 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 term when we're taking off - set_throttle_takeoff(); + // State Machine Determination + if (!motors.armed() || !motors.get_interlock()) { + sport_state = Sport_MotorStopped; + } else if (takeoff_state.running || takeoff_triggered) { + sport_state = Sport_Takeoff; + } else if (!ap.auto_armed || ap.land_complete) { + sport_state = Sport_Landed; + } else { + sport_state = Sport_Flying; } - // reset target lean angles and heading while landed - if (ap.land_complete) { - if (ap.throttle_zero) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - }else{ - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - } - // move throttle to between minimum and non-takeoff-throttle to keep us on the ground - attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt); - pos_control.relax_alt_hold_controllers(0.0f); - }else{ + // State Machine + switch (sport_state) { + + case Sport_MotorStopped: + + 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); +#if FRAME_CONFIG == HELI_FRAME + // force descent rate and call position controller + pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); +#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; } } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index fb8715ed89..2cc411d7ec 100644 --- a/ArduCopter/defines.h +++ b/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,