From 17cbcd1bf6d2c909207ecc8362b102b473c24ee5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 5 Apr 2013 15:35:26 +1100 Subject: [PATCH] Plane: fixed failsafe with VTAIL_OUTPUT --- ArduPlane/failsafe.pde | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/ArduPlane/failsafe.pde b/ArduPlane/failsafe.pde index 0a3a766da9..d758c0335c 100644 --- a/ArduPlane/failsafe.pde +++ b/ArduPlane/failsafe.pde @@ -39,13 +39,20 @@ void failsafe_check(uint32_t tnow) // pass RC inputs to outputs every 20ms last_timestamp = tnow; hal.rcin->clear_overrides(); - uint8_t start_ch = 0; - if (demoing_servos) { - start_ch = 1; + g.channel_roll.radio_out = hal.rcin->read(CH_1); + g.channel_pitch.radio_out = hal.rcin->read(CH_2); + g.channel_throttle.radio_out = hal.rcin->read(CH_3); + g.channel_rudder.radio_out = hal.rcin->read(CH_4); + if (g.vtail_output != VTAIL_DISABLED) { + vtail_output_mixing(); } - for (uint8_t ch=start_ch; ch<4; ch++) { - servo_write(ch, hal.rcin->read(ch)); + if (!demoing_servos) { + servo_write(CH_1, g.channel_roll.radio_out); } + servo_write(CH_2, g.channel_pitch.radio_out); + servo_write(CH_3, g.channel_throttle.radio_out); + servo_write(CH_4, g.channel_rudder.radio_out); + 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); RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input, true);