diff --git a/ArduPlane/failsafe.pde b/ArduPlane/failsafe.pde index 1aa2048152..c5640d7e64 100644 --- a/ArduPlane/failsafe.pde +++ b/ArduPlane/failsafe.pde @@ -46,6 +46,7 @@ void failsafe_check(uint32_t tnow) for (uint8_t ch=start_ch; ch<4; ch++) { APM_RC.OutputCh(ch, APM_RC.InputCh(ch)); } - RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual); + RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true); + RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, true); } } diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 8b086e876c..cc4d2c25bc 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -1,3 +1,4 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* * RC_Channel.cpp - Radio library for Arduino * Code by Jason Short. DIYDrones.com @@ -324,6 +325,12 @@ RC_Channel::output() _apm_rc->OutputCh(_ch_out, radio_out); } +void +RC_Channel::input() +{ + radio_in = _apm_rc->InputCh(_ch_out); +} + void RC_Channel::enable_out() { diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 61ead5027b..e457df9301 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -88,6 +88,7 @@ public: static void set_apm_rc(APM_RC_Class * apm_rc); void output(); + void input(); void enable_out(); static APM_RC_Class * _apm_rc; diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index efb9ffe447..a19a3ad779 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -172,11 +172,17 @@ RC_Channel_aux::set_radio_to_trim(RC_Channel_aux::Aux_servo_function_t function) copy radio_in to radio_out for a given function */ void -RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::Aux_servo_function_t function) +RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::Aux_servo_function_t function, bool do_input_output) { for (uint8_t i = 0; i < 7; i++) { if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { + if (do_input_output) { + _aux_channels[i]->input(); + } _aux_channels[i]->radio_out = _aux_channels[i]->radio_in; + if (do_input_output) { + _aux_channels[i]->output(); + } } } } diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 46651ad162..4bf7bcc944 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -67,7 +67,7 @@ public: static void set_radio_to_trim(Aux_servo_function_t function); // copy radio_in to radio_out - static void copy_radio_in_out(Aux_servo_function_t function); + static void copy_radio_in_out(Aux_servo_function_t function, bool do_input_output=false); // set servo_out static void set_servo_out(Aux_servo_function_t function, int16_t value);