Browse Source

Copter: Create check_duplicate_auxsw() method to streamline pre-arm check code

master
Robert Lefebvre 10 years ago committed by Randy Mackay
parent
commit
fd0cb0beed
  1. 2
      ArduCopter/motors.pde
  2. 22
      ArduCopter/switches.pde

2
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)) { if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
// ensure ch7 and ch8 have different functions // 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) { if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Duplicate Aux Switch Options")); gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Duplicate Aux Switch Options"));
} }

22
ArduCopter/switches.pde

@ -66,6 +66,28 @@ static bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check){
return ret; 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() static void reset_control_switch()
{ {
control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1; control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1;

Loading…
Cancel
Save