diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index 8458d74003..d84430e5d3 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -72,14 +72,19 @@ void Copter::loiter_run() wp_nav.loiter_soften_for_landing(); } +#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 + // Loiter State Machine Determination if (!motors.armed() || !motors.get_interlock()) { loiter_state = Loiter_MotorStopped; - } else if (!ap.auto_armed) { - loiter_state = Loiter_NotAutoArmed; - } else if (takeoff_state.running || (ap.land_complete && (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()))){ + } else if (takeoff_state.running || takeoff_triggered) { loiter_state = Loiter_Takeoff; - } else if (ap.land_complete){ + } else if (!ap.auto_armed || ap.land_complete) { loiter_state = Loiter_Landed; } else { loiter_state = Loiter_Flying; @@ -92,41 +97,24 @@ void Copter::loiter_run() motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); #if FRAME_CONFIG == HELI_FRAME - // helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); - // force descent rate and call position controller pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); - pos_control.update_z_controller(); #else wp_nav.init_loiter_target(); - // multicopters do not stabilize roll/pitch/yaw when motors are stopped - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover()); -#endif - break; - - case Loiter_NotAutoArmed: - - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - wp_nav.init_loiter_target(); -#if FRAME_CONFIG == HELI_FRAME - // Helicopters always stabilize roll/pitch/yaw - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); - attitude_control.set_throttle_out(0,false,g.throttle_filt); -#else - // Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); + 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.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover()); + wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); + pos_control.update_z_controller(); break; case Loiter_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 @@ -138,9 +126,6 @@ void Copter::loiter_run() // get takeoff adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); @@ -154,19 +139,18 @@ void Copter::loiter_run() break; case Loiter_Landed: - - wp_nav.init_loiter_target(); - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); - // 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); - // if throttle zero reset attitude and exit immediately - if (ap.throttle_zero) { + // 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); } - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover()); + wp_nav.init_loiter_target(); + attitude_control.reset_rate_controller_I_terms(); + attitude_control.set_yaw_target_to_current_heading(); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); + pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control.update_z_controller(); break; case Loiter_Flying: diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 34141316bf..fb8715ed89 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -233,7 +233,6 @@ enum AltHoldModeState { // Loiter states enum LoiterModeState { Loiter_MotorStopped, - Loiter_NotAutoArmed, Loiter_Takeoff, Loiter_Flying, Loiter_Landed