|
|
|
@ -28,12 +28,12 @@ void AP_AdvancedFailsafe_Copter::terminate_vehicle(void)
@@ -28,12 +28,12 @@ void AP_AdvancedFailsafe_Copter::terminate_vehicle(void)
|
|
|
|
|
copter.arming.disarm(); |
|
|
|
|
|
|
|
|
|
// and set all aux channels
|
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_heli_rsc, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_heli_tail_rsc, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_ignition, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_heli_rsc, SRV_Channel::Limit::TRIM); |
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_heli_tail_rsc, SRV_Channel::Limit::TRIM); |
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::TRIM); |
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_ignition, SRV_Channel::Limit::TRIM); |
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::Limit::TRIM); |
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::Limit::TRIM); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
SRV_Channels::output_ch_all(); |
|
|
|
@ -42,12 +42,12 @@ void AP_AdvancedFailsafe_Copter::terminate_vehicle(void)
@@ -42,12 +42,12 @@ void AP_AdvancedFailsafe_Copter::terminate_vehicle(void)
|
|
|
|
|
void AP_AdvancedFailsafe_Copter::setup_IO_failsafe(void) |
|
|
|
|
{ |
|
|
|
|
// setup failsafe for all aux channels
|
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_heli_rsc, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_heli_tail_rsc, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_ignition, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); |
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_heli_rsc, SRV_Channel::Limit::TRIM); |
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_heli_tail_rsc, SRV_Channel::Limit::TRIM); |
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::TRIM); |
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_ignition, SRV_Channel::Limit::TRIM); |
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::Limit::TRIM); |
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::Limit::TRIM); |
|
|
|
|
|
|
|
|
|
#if FRAME_CONFIG != HELI_FRAME |
|
|
|
|
// setup AP_Motors outputs for failsafe
|
|
|
|
|