diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 8fbee726a3..cf2a6f55a4 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -16,6 +16,12 @@ AP_AdvancedFailsafe_Plane::AP_AdvancedFailsafe_Plane(AP_Mission &_mission) : */ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) { + if (plane.quadplane.available() && _terminate_action == TERMINATE_ACTION_LAND) { + // perform a VTOL landing + plane.set_mode(plane.mode_qland, MODE_REASON_FENCE_BREACH); + return; + } + plane.g2.servo_channels.disable_passthrough(true); if (_terminate_action == TERMINATE_ACTION_LAND) { diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index 9ff449ac9a..d89e32e1dd 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -86,7 +86,9 @@ void Plane::failsafe_check(void) #if ADVANCED_FAILSAFE == ENABLED if (afs.should_crash_vehicle()) { afs.terminate_vehicle(); - return; + if (!afs.terminating_vehicle_via_landing()) { + return; + } } #endif diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 2b964d0153..87a52880a4 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1653,7 +1653,7 @@ void QuadPlane::update(void) } #if ADVANCED_FAILSAFE == ENABLED - if (plane.afs.should_crash_vehicle()) { + if (plane.afs.should_crash_vehicle() && !plane.afs.terminating_vehicle_via_landing()) { motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); motors->output(); return; @@ -1845,7 +1845,9 @@ void QuadPlane::motors_output(bool run_rate_controller) } #if ADVANCED_FAILSAFE == ENABLED - if (!hal.util->get_soft_armed() || plane.afs.should_crash_vehicle() || SRV_Channels::get_emergency_stop()) { + if (!hal.util->get_soft_armed() || + (plane.afs.should_crash_vehicle() && !plane.afs.terminating_vehicle_via_landing()) || + SRV_Channels::get_emergency_stop()) { #else if (!hal.util->get_soft_armed() || SRV_Channels::get_emergency_stop()) { #endif diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 981c9b6d1f..28eb02a5a7 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -672,7 +672,9 @@ void Plane::set_servos(void) #if ADVANCED_FAILSAFE == ENABLED if (afs.should_crash_vehicle()) { afs.terminate_vehicle(); - return; + if (!afs.terminating_vehicle_via_landing()) { + return; + } } #endif