From 1afeb053988df87cbaadb56bfeb50d841bef1dd4 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Tue, 10 Mar 2015 16:53:30 -0400 Subject: [PATCH] Copter: Create check_if_auxsw_mode_used() to check for function usage --- ArduCopter/landing_gear.pde | 2 +- ArduCopter/switches.pde | 14 +++++++++++--- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/ArduCopter/landing_gear.pde b/ArduCopter/landing_gear.pde index 589ebb3385..057f7631e0 100644 --- a/ArduCopter/landing_gear.pde +++ b/ArduCopter/landing_gear.pde @@ -4,7 +4,7 @@ static void landinggear_update(){ // If landing gear control is active, run update function. - if (g.ch7_option == AUXSW_LANDING_GEAR || g.ch8_option == AUXSW_LANDING_GEAR ){ + if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){ // last status (deployed or retracted) used to check for changes static bool last_deploy_status; diff --git a/ArduCopter/switches.pde b/ArduCopter/switches.pde index 83b607a475..20886d909e 100644 --- a/ArduCopter/switches.pde +++ b/ArduCopter/switches.pde @@ -36,9 +36,9 @@ static void read_control_switch() } } - if(g.ch7_option != AUXSW_SIMPLE_MODE && g.ch8_option != AUXSW_SIMPLE_MODE && g.ch7_option != AUXSW_SUPERSIMPLE_MODE && g.ch8_option != AUXSW_SUPERSIMPLE_MODE) { - // set Simple mode using stored paramters from Mission planner - // rather than by the control switch + if(!check_if_auxsw_mode_used(AUXSW_SIMPLE_MODE) && !check_if_auxsw_mode_used(AUXSW_SUPERSIMPLE_MODE)) { + // if none of the Aux Switches are set to Simple or Super Simple Mode then + // set Simple Mode using stored parameters from EEPROM if (BIT_IS_SET(g.super_simple, switch_position)) { set_simple_mode(2); }else{ @@ -58,6 +58,14 @@ static void read_control_switch() control_switch_state.last_switch_position = switch_position; } +// check_if_auxsw_mode_used - Check to see if any of the Aux Switches are set to a given mode. +static bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check){ + bool ret = g.ch7_option == auxsw_mode_check || g.ch8_option == auxsw_mode_check || g.ch9_option == auxsw_mode_check + || g.ch10_option == auxsw_mode_check || g.ch11_option == auxsw_mode_check || g.ch12_option == auxsw_mode_check; + + return ret; +} + static void reset_control_switch() { control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1;