diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index d4f9b65bfb..d8535a16c0 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -106,13 +106,6 @@ AP_HAL::BetterStream* cliSerial; const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; -//////////////////////////////////////////////////////////////////////////////// -// Outback Challenge Failsafe Support -//////////////////////////////////////////////////////////////////////////////// -#if OBC_FAILSAFE == ENABLED -APM_OBC obc; -#endif - //////////////////////////////////////////////////////////////////////////////// // the rate we run the main loop at //////////////////////////////////////////////////////////////////////////////// @@ -561,6 +554,13 @@ AP_Mission mission(ahrs, &exit_mission_callback, MISSION_START_BYTE, MISSION_END_BYTE); +//////////////////////////////////////////////////////////////////////////////// +// Outback Challenge Failsafe Support +//////////////////////////////////////////////////////////////////////////////// +#if OBC_FAILSAFE == ENABLED +APM_OBC obc(mission, barometer, gps); +#endif + //////////////////////////////////////////////////////////////////////////////// // Waypoint distances //////////////////////////////////////////////////////////////////////////////// @@ -909,9 +909,7 @@ static void obc_fs_check(void) { #if OBC_FAILSAFE == ENABLED // perform OBC failsafe checks - obc.check(OBC_MODE(control_mode), - failsafe.last_heartbeat_ms, - gps, mission); + obc.check(OBC_MODE(control_mode), failsafe.last_heartbeat_ms); #endif } diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 860f6a2c05..8b91de823b 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -814,19 +814,6 @@ static void set_servos(void) channel_pitch->radio_out = elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); } -#if OBC_FAILSAFE == ENABLED - // this is to allow the failsafe module to deliberately crash - // the plane. Only used in extreme circumstances to meet the - // OBC rules - if (obc.crash_plane()) { - channel_roll->servo_out = -4500; - channel_pitch->servo_out = -4500; - channel_rudder->servo_out = -4500; - channel_throttle->servo_out = 0; - } -#endif - - // push out the PWM values if (g.mix_mode == 0) { channel_roll->calc_pwm(); @@ -916,6 +903,29 @@ static void set_servos(void) channel_rudder->radio_out = channel_rudder->radio_in; } +#if OBC_FAILSAFE == ENABLED + // this is to allow the failsafe module to deliberately crash + // the plane. Only used in extreme circumstances to meet the + // OBC rules + if (obc.crash_plane()) { + channel_roll->servo_out = -4500; + channel_pitch->servo_out = -4500; + channel_rudder->servo_out = -4500; + channel_throttle->servo_out = 0; + channel_roll->calc_pwm(); + channel_pitch->calc_pwm(); + channel_rudder->calc_pwm(); + channel_throttle->calc_pwm(); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, 100); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, 100); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, 4500); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, 4500); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, 4500); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator_with_input, 4500); + } +#endif + + if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) { flaperon_update(auto_flap_percent); }