diff --git a/ArduCopter/afs_copter.cpp b/ArduCopter/afs_copter.cpp index 27330a6513..c88467918e 100644 --- a/ArduCopter/afs_copter.cpp +++ b/ArduCopter/afs_copter.cpp @@ -17,20 +17,24 @@ AP_AdvancedFailsafe_Copter::AP_AdvancedFailsafe_Copter(AP_Mission &_mission, AP_ */ void AP_AdvancedFailsafe_Copter::terminate_vehicle(void) { - // stop motors - copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); - copter.motors->output(); + if (_terminate_action == TERMINATE_ACTION_LAND) { + copter.set_mode(LAND, MODE_REASON_TERMINATE); + } else { + // stop motors + copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + copter.motors->output(); - // disarm as well - copter.init_disarm_motors(); + // disarm as well + copter.init_disarm_motors(); - // 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); + // 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::output_ch_all(); } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index fce0854001..04aa1510dd 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -124,6 +124,7 @@ enum mode_reason_t { MODE_REASON_AVOIDANCE, MODE_REASON_AVOIDANCE_RECOVERY, MODE_REASON_THROW_COMPLETE, + MODE_REASON_TERMINATE, }; // Tuning enumeration