From f8d03b58d1e303d8cbea19e31a2ad34984b96859 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 18 Jan 2016 12:53:08 +0900 Subject: [PATCH] Copter: loiter uses 0 to 1 throttle and sets motor spool state --- ArduCopter/control_loiter.cpp | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index 736ac60cbd..a88a6163a7 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -95,7 +95,9 @@ void Copter::loiter_run() // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(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 disarmed +#else + motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED); + // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif // HELI_FRAME pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); @@ -115,6 +117,7 @@ void Copter::loiter_run() pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); pos_control.update_z_controller(); #else + motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SHUT_DOWN); 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); @@ -135,6 +138,9 @@ 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_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED); + // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); @@ -154,15 +160,25 @@ void Copter::loiter_run() // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt); -#else // multicopters do not stabilize roll/pitch/yaw when disarmed +#else + // if throttle zero reset attitude and exit immediately + if (ap.throttle_zero) { + motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED); + }else{ + motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED); + } + // multicopters do not stabilize roll/pitch/yaw when disarmed // move throttle to between minimum and non-takeoff-throttle to keep us on the ground - attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt); + attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt); #endif pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); break; case Loiter_Flying: + // set motors to full range + motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED); + // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);