diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 9628316035..053d347aa0 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -70,6 +70,7 @@ public: k_param_flap_2_speed, k_param_num_resets, k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change + k_param_reset_switch_chan, // 110: Telemetry control @@ -320,6 +321,7 @@ public: AP_Int16 num_resets; AP_Int16 log_bitmask; AP_Int16 log_last_filenumber; // *** Deprecated - remove with next eeprom number change + AP_Int8 reset_switch_chan; AP_Int16 airspeed_cruise; AP_Int16 min_gndspeed; AP_Int16 pitch_trim; @@ -435,6 +437,7 @@ public: num_resets (0, k_param_num_resets, PSTR("SYS_NUM_RESETS")), log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")), log_last_filenumber (0, k_param_log_last_filenumber, PSTR("LOG_LASTFILE")), + reset_switch_chan (0, k_param_reset_switch_chan, PSTR("RST_SWITCH_CH")), airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")), min_gndspeed (MIN_GNDSPEED_CM, k_param_min_gndspeed, PSTR("MIN_GNDSPD_CM")), pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")), diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 08fc1a2819..89b5e41e14 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -816,3 +816,11 @@ #ifndef FENCE_TRIGGERED_PIN # define FENCE_TRIGGERED_PIN -1 #endif + +// if RESET_SWITCH_CH is not zero, then this is the PWM value on +// that channel where we reset the control mode to the current switch +// position (to for example return to switched mode after failsafe or +// fence breach) +#ifndef RESET_SWITCH_CHAN_PWM +# define RESET_SWITCH_CHAN_PWM 1750 +#endif diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde index 26010e3cd3..c5b31df963 100644 --- a/ArduPlane/control_modes.pde +++ b/ArduPlane/control_modes.pde @@ -3,7 +3,16 @@ static void read_control_switch() { byte switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition){ + + // we look for changes in the switch position. If the + // RST_SWITCH_CH parameter is set, then it is a switch that can be + // used to force re-reading of the control switch. This is useful + // when returning to the previous mode after a failsafe or fence + // breach. This channel is best used on a momentary switch (such + // as a spring loaded trainer switch). + if (oldSwitchPosition != switchPosition || + (g.reset_switch_chan != 0 && + APM_RC.InputCh(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) { set_mode(flight_modes[switchPosition]);