From 1b29a1af46e693f147098697eded1cf2b86b1ea4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 16 Nov 2015 10:58:15 +0900 Subject: [PATCH] Copter: consolidate setting of using-iterlock state --- ArduCopter/AP_State.cpp | 12 ++++++++---- ArduCopter/ArduCopter.cpp | 10 +++------- ArduCopter/Copter.h | 2 +- ArduCopter/heli.cpp | 3 --- ArduCopter/motors.cpp | 1 - ArduCopter/switches.cpp | 5 ----- ArduCopter/system.cpp | 3 +++ 7 files changed, 15 insertions(+), 21 deletions(-) diff --git a/ArduCopter/AP_State.cpp b/ArduCopter/AP_State.cpp index 89afcef27d..df7f6b08fc 100644 --- a/ArduCopter/AP_State.cpp +++ b/ArduCopter/AP_State.cpp @@ -114,11 +114,15 @@ void Copter::set_pre_arm_rc_check(bool b) } } -void Copter::set_using_interlock(bool b) +void Copter::update_using_interlock() { - if(ap.using_interlock != b) { - ap.using_interlock = b; - } +#if FRAME_CONFIG == HELI_FRAME + // helicopters are always using motor interlock + ap.using_interlock = true; +#else + // check if we are using motor interlock control on an aux switch + ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK); +#endif } void Copter::set_motor_emergency_stop(bool b) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 1aa4daa3c4..93c6c410c0 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -480,16 +480,12 @@ void Copter::one_hz_loop() // make it possible to change ahrs orientation at runtime during initial config ahrs.set_orientation(); -#if FRAME_CONFIG == HELI_FRAME - // helicopters are always using motor interlock - set_using_interlock(true); -#else + update_using_interlock(); + +#if FRAME_CONFIG != HELI_FRAME // check the user hasn't updated the frame orientation motors.set_frame_orientation(g.frame_orientation); - // check if we are using motor interlock control on an aux switch - set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); - // set all throttle channel settings motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max); // set hover throttle diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 972f8c0c6e..132ff08be0 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -576,7 +576,7 @@ private: void set_land_complete_maybe(bool b); void set_pre_arm_check(bool b); void set_pre_arm_rc_check(bool b); - void set_using_interlock(bool b); + void update_using_interlock(); void set_motor_emergency_stop(bool b); float get_smoothing_gain(); void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max); diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 20f3e57dc2..e0eaac1a52 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -16,9 +16,6 @@ static int8_t heli_dynamic_flight_counter; // heli_init - perform any special initialisation required for the tradheli void Copter::heli_init() { - // helicopters are always using motor interlock - set_using_interlock(true); - /* automatically set H_RSC_MIN and H_RSC_MAX from RC8_MIN and RC8_MAX so that when users upgrade from tradheli version 3.3 to diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 3fdcd43d94..d180d741fd 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -231,7 +231,6 @@ bool Copter::pre_arm_checks(bool display_failure) // if it is, switch needs to be in disabled position to arm // 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 && motors.get_interlock()){ if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled"); diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 38adc8865f..18c3e6e099 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -244,14 +244,9 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) case AUXSW_RELAY: case AUXSW_LANDING_GEAR: case AUXSW_MOTOR_ESTOP: - do_aux_switch_function(ch_option, ch_flag); - break; - case AUXSW_MOTOR_INTERLOCK: - set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); do_aux_switch_function(ch_option, ch_flag); break; - } } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index cefc23f11e..58dee570d1 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -162,6 +162,9 @@ void Copter::init_ardupilot() log_init(); #endif + // update motor interlock state + update_using_interlock(); + #if FRAME_CONFIG == HELI_FRAME // trad heli specific initialisation heli_init();