diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde index c78975d747..a1e388a49c 100644 --- a/ArduCopter/control_althold.pde +++ b/ArduCopter/control_althold.pde @@ -26,8 +26,8 @@ static void althold_run() float target_climb_rate; float takeoff_climb_rate = 0.0f; - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || !motors.get_interlock()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(g.rc_3.control_in)-throttle_average); return; diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 182ddd47ea..23bf030dde 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -109,8 +109,8 @@ static void auto_takeoff_start(float final_alt_above_home) // called by auto_run at 100hz or more static void auto_takeoff_run() { - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || !motors.get_interlock()) { // initialise wpnav targets wp_nav.shift_wp_origin_to_current_pos(); // reset attitude control targets @@ -156,8 +156,8 @@ static void auto_wp_start(const Vector3f& destination) // called by auto_run at 100hz or more static void auto_wp_run() { - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || !motors.get_interlock()) { // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); @@ -212,8 +212,8 @@ static void auto_spline_start(const Vector3f& destination, bool stopped_at_start // called by auto_run at 100hz or more static void auto_spline_run() { - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || !motors.get_interlock()) { // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); @@ -281,7 +281,7 @@ static void auto_land_run() int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; - // if not auto armed set throttle to zero and exit immediately + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // set target to current position @@ -447,8 +447,8 @@ bool auto_loiter_start() // called by auto_run at 100hz or more void auto_loiter_run() { - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed || ap.land_complete) { + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index 46e7067562..722fc36a04 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -260,9 +260,9 @@ static void autotune_run() float target_yaw_rate; int16_t target_climb_rate; - // if not auto armed set throttle to zero and exit immediately + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately // this should not actually be possible because of the autotune_init() checks - if (!ap.auto_armed) { + if (!ap.auto_armed || !motors.get_interlock()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(g.rc_3.control_in)-throttle_average); return; diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde index 7fb31e4d6b..34acc19e07 100644 --- a/ArduCopter/control_circle.pde +++ b/ArduCopter/control_circle.pde @@ -32,8 +32,8 @@ static void circle_run() float target_yaw_rate = 0; float target_climb_rate = 0; - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed || ap.land_complete) { + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { // To-Do: add some initialisation of position controllers attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); diff --git a/ArduCopter/control_drift.pde b/ArduCopter/control_drift.pde index 9e127cf686..fc3d85ed1f 100644 --- a/ArduCopter/control_drift.pde +++ b/ArduCopter/control_drift.pde @@ -46,8 +46,8 @@ static void drift_run() float target_yaw_rate; int16_t pilot_throttle_scaled; - // if not armed or landed and throttle at zero, set throttle to zero and exit immediately - if(!motors.armed() || (ap.land_complete && ap.throttle_zero)) { + // if not armed or motor interlock not enabled or landed and throttle at zero, set throttle to zero and exit immediately + if(!motors.armed() || !motors.get_interlock() || (ap.land_complete && ap.throttle_zero)) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index 0804542b85..fa1f7c295a 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -158,8 +158,8 @@ static void guided_set_destination_posvel(const Vector3f& destination, const Vec // should be called at 100hz or more static void guided_run() { - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { + // if not auto armed or motors not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || !motors.get_interlock()) { // To-Do: reset waypoint controller? attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true @@ -195,8 +195,8 @@ static void guided_run() // called by guided_run at 100hz or more static void guided_takeoff_run() { - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { + // if not auto armed or motors interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || !motors.get_interlock()) { // initialise wpnav targets wp_nav.shift_wp_origin_to_current_pos(); // reset attitude control targets diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index eca5b8a5bb..774846a2a1 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -50,8 +50,8 @@ static void land_gps_run() int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; - // if not auto armed or landed set throttle to zero and exit immediately - if(!ap.auto_armed || ap.land_complete) { + // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); wp_nav.init_loiter_target(); @@ -123,8 +123,8 @@ static void land_nogps_run() float target_roll = 0.0f, target_pitch = 0.0f; float target_yaw_rate = 0; - // if not auto armed or landed set throttle to zero and exit immediately - if(!ap.auto_armed || ap.land_complete) { + // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 13d7a69773..dd26ae07ec 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -33,8 +33,8 @@ static void loiter_run() float target_climb_rate = 0; float takeoff_climb_rate = 0.0f; - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || !motors.get_interlock()) { wp_nav.init_loiter_target(); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(g.rc_3.control_in)-throttle_average); diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index 321d37f512..5f6c81a74e 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -154,8 +154,8 @@ static void poshold_run() float vel_fw, vel_right; // vehicle's current velocity in body-frame forward and right directions const Vector3f& vel = inertial_nav.get_velocity(); - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || !motors.get_interlock()) { wp_nav.init_loiter_target(); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(g.rc_3.control_in)-throttle_average); diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 258f7e70e3..a082ae5923 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -131,8 +131,8 @@ static void rtl_return_start() // called by rtl_run at 100hz or more static void rtl_climb_return_run() { - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || !motors.get_interlock()) { // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // To-Do: re-initialise wpnav targets @@ -187,8 +187,8 @@ static void rtl_loiterathome_start() // called by rtl_run at 100hz or more static void rtl_loiterathome_run() { - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || !motors.get_interlock()) { // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // To-Do: re-initialise wpnav targets @@ -257,8 +257,8 @@ static void rtl_descent_run() int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || !motors.get_interlock()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // set target to current position wp_nav.init_loiter_target(); @@ -319,8 +319,8 @@ static void rtl_land_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed || ap.land_complete) { + // 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()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // set target to current position wp_nav.init_loiter_target();