From fd0cb0beedb10aa84eed97c430d692935fd2494b Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Tue, 10 Mar 2015 21:20:07 -0400 Subject: [PATCH] Copter: Create check_duplicate_auxsw() method to streamline pre-arm check code --- ArduCopter/motors.pde | 2 +- ArduCopter/switches.pde | 22 ++++++++++++++++++++++ 2 files changed, 23 insertions(+), 1 deletion(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 200c621572..7db56952d2 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -404,7 +404,7 @@ static bool pre_arm_checks(bool display_failure) if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) { // ensure ch7 and ch8 have different functions - if ((g.ch7_option != 0 || g.ch8_option != 0) && g.ch7_option == g.ch8_option) { + if (check_duplicate_auxsw()) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Duplicate Aux Switch Options")); } diff --git a/ArduCopter/switches.pde b/ArduCopter/switches.pde index 20886d909e..ef0c431c01 100644 --- a/ArduCopter/switches.pde +++ b/ArduCopter/switches.pde @@ -66,6 +66,28 @@ static bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check){ return ret; } +// check_duplicate_auxsw - Check to see if any Aux Switch Functions are duplicated +static bool check_duplicate_auxsw(void){ + + bool ret = ((g.ch7_option != AUXSW_DO_NOTHING) && (g.ch7_option == g.ch8_option || + g.ch7_option == g.ch9_option || g.ch7_option == g.ch10_option || + g.ch7_option == g.ch11_option || g.ch7_option == g.ch12_option)); + + ret = ret || ((g.ch8_option != AUXSW_DO_NOTHING) && (g.ch8_option == g.ch9_option || + g.ch8_option == g.ch10_option || g.ch8_option == g.ch11_option || + g.ch8_option == g.ch12_option)); + + ret = ret || ((g.ch9_option != AUXSW_DO_NOTHING) && (g.ch9_option == g.ch10_option || + g.ch9_option == g.ch11_option || g.ch9_option == g.ch12_option)); + + ret = ret || ((g.ch10_option != AUXSW_DO_NOTHING) && (g.ch10_option == g.ch11_option || + g.ch10_option == g.ch12_option)); + + ret = ret || ((g.ch11_option != AUXSW_DO_NOTHING) && (g.ch11_option == g.ch12_option)); + + return ret; +} + static void reset_control_switch() { control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1;