diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index 9f70263a61..8eec59d8be 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -139,13 +139,6 @@ void set_pre_arm_rc_check(bool b) } } -void set_motor_interlock(bool b) -{ - if(ap.motor_interlock != b) { - ap.motor_interlock = b; - } -} - void set_using_interlock(bool b) { if(ap.using_interlock != b) { diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 48c2f289de..9358443a3d 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -357,9 +357,8 @@ static union { uint8_t system_time_set : 1; // 28 // true if the system time has been set from the GPS uint8_t gps_base_pos_set : 1; // 29 // true when the gps base position has been set (used for RTK gps only) enum HomeState home_state : 2; // 30,31 // home status (unset, set, locked) - uint8_t motor_interlock : 1; // 32 // motor interlock status, final control for motors on/off - uint8_t using_interlock : 1; // 33 // aux switch motor interlock function is in use - uint8_t motor_estop : 1; // 34 // motor estop switch, shuts off motors when enabled + uint8_t using_interlock : 1; // 32 // aux switch motor interlock function is in use + uint8_t motor_estop : 1; // 33 // motor estop switch, shuts off motors when enabled }; uint32_t value; } ap; diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index ea4aa4ab18..5cf1962511 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -84,7 +84,7 @@ static void auto_disarm_check() // allow auto disarm in manual flight modes or Loiter/AltHold if we're landed // always allow auto disarm if using interlock switch or motors are E-stopped - if (mode_has_manual_throttle(control_mode) || ap.land_complete || (ap.using_interlock && !ap.motor_interlock) || ap.motor_estop) { + if (mode_has_manual_throttle(control_mode) || ap.land_complete || (ap.using_interlock && !motors.get_interlock()) || ap.motor_estop) { auto_disarming_counter++; // use a shorter delay if using throttle interlock switch or E-stop, because it is less @@ -173,8 +173,8 @@ static bool init_arm_motors(bool arming_from_gcs) // check if we are using motor interlock control on an aux switch set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); - // if we are using motor interlock switch and it's disabled, fail to arm - if (ap.using_interlock && ap.motor_interlock){ + // if we are using motor interlock switch and it's enabled, fail to arm + if (ap.using_interlock && motors.get_interlock()){ gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Interlock Enabled")); AP_Notify::flags.armed = false; return false; @@ -245,7 +245,7 @@ static bool pre_arm_checks(bool display_failure) // otherwise exit immediately. This check to be repeated, // as state can change at any time. set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); - if (ap.using_interlock && ap.motor_interlock){ + if (ap.using_interlock && motors.get_interlock()){ if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Motor Interlock Enabled")); } @@ -741,9 +741,6 @@ static void init_disarm_motors() gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); #endif - // always force Motor Interlock to false - set_motor_interlock(false); - // send disarm command to motors motors.armed(false); @@ -785,11 +782,7 @@ static void motors_output() if (ap.motor_test) { motor_test_output(); } else { - if (ap.using_interlock){ - // pass in motor interlock status to motors class - // true means motors run, false motors don't run - motors.set_interlock(ap.motor_interlock); - } else { + if (!ap.using_interlock){ // if not using interlock switch, set according to E-Stop status // where E-Stop is forced false during arming if E-Stop switch // is not used. Interlock enabled means motors run, so we must diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 64f3cefec3..f0d3042f2a 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -184,7 +184,7 @@ static void set_throttle_zero_flag(int16_t throttle_control) // if not using throttle interlock and non-zero throttle and not E-stopped, // or using motor interlock and it's enabled, then motors are running, // and we are flying. Immediately set as non-zero - if ((!ap.using_interlock && (throttle_control > 0) && !ap.motor_estop) || (ap.using_interlock && ap.motor_interlock)) { + if ((!ap.using_interlock && (throttle_control > 0) && !ap.motor_estop) || (ap.using_interlock && motors.get_interlock())) { last_nonzero_throttle_ms = tnow_ms; ap.throttle_zero = false; } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) { diff --git a/ArduCopter/switches.pde b/ArduCopter/switches.pde index 99b8ca13e1..de0c3e235e 100644 --- a/ArduCopter/switches.pde +++ b/ArduCopter/switches.pde @@ -556,7 +556,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_MOTOR_INTERLOCK: // Turn on when above LOW, because channel will also be used for speed // control signal in tradheli - set_motor_interlock(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); + motors.set_interlock(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); break; }