From cce426dcb8a1e2d984a4c305ae984f5b25356a8b Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 13 Jan 2016 16:11:50 +0900 Subject: [PATCH] Copter: rtl uses throttle in 0 to 1 range and sets motor spool state --- ArduCopter/control_rtl.cpp | 36 ++++++++++++++++++++++++++++-------- 1 file changed, 28 insertions(+), 8 deletions(-) diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index 7056afa259..2ace437e42 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -114,12 +114,14 @@ void Copter::rtl_return_start() void Copter::rtl_climb_return_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if(!ap.auto_armed || !motors.get_interlock()) { + if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // 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 // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif @@ -137,6 +139,9 @@ void Copter::rtl_climb_return_run() } } + // set motors to full range + motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED); + // run waypoint controller wp_nav.update_wpnav(); @@ -176,12 +181,14 @@ void Copter::rtl_loiterathome_start() void Copter::rtl_loiterathome_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if(!ap.auto_armed || !motors.get_interlock()) { + if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // 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 // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif @@ -199,6 +206,9 @@ void Copter::rtl_loiterathome_run() } } + // set motors to full range + motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED); + // run waypoint controller wp_nav.update_wpnav(); @@ -252,12 +262,14 @@ void Copter::rtl_descent_run() float target_yaw_rate = 0; // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if(!ap.auto_armed || !motors.get_interlock()) { + if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // 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 // set target to current position @@ -288,6 +300,9 @@ void Copter::rtl_descent_run() target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } + // set motors to full range + motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED); + // process roll, pitch inputs wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); @@ -328,12 +343,14 @@ void Copter::rtl_land_run() int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately - if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { + if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // 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 // set target to current position @@ -384,6 +401,9 @@ void Copter::rtl_land_run() target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } + // set motors to full range + motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED); + // process pilot's roll and pitch input wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);