Plane: implement VTOL landing for AFS termination

this allows for vertical landing as an AFS_TERM_ACTION
This commit is contained in:
Andrew Tridgell 2019-08-21 16:01:17 +10:00
parent 4a6fdc00c9
commit 7751352a86
4 changed files with 16 additions and 4 deletions

View File

@ -16,6 +16,12 @@ AP_AdvancedFailsafe_Plane::AP_AdvancedFailsafe_Plane(AP_Mission &_mission) :
*/ */
void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) 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); plane.g2.servo_channels.disable_passthrough(true);
if (_terminate_action == TERMINATE_ACTION_LAND) { if (_terminate_action == TERMINATE_ACTION_LAND) {

View File

@ -86,8 +86,10 @@ void Plane::failsafe_check(void)
#if ADVANCED_FAILSAFE == ENABLED #if ADVANCED_FAILSAFE == ENABLED
if (afs.should_crash_vehicle()) { if (afs.should_crash_vehicle()) {
afs.terminate_vehicle(); afs.terminate_vehicle();
if (!afs.terminating_vehicle_via_landing()) {
return; return;
} }
}
#endif #endif
// setup secondary output channels that do have // setup secondary output channels that do have

View File

@ -1653,7 +1653,7 @@ void QuadPlane::update(void)
} }
#if ADVANCED_FAILSAFE == ENABLED #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->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
motors->output(); motors->output();
return; return;
@ -1845,7 +1845,9 @@ void QuadPlane::motors_output(bool run_rate_controller)
} }
#if ADVANCED_FAILSAFE == ENABLED #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 #else
if (!hal.util->get_soft_armed() || SRV_Channels::get_emergency_stop()) { if (!hal.util->get_soft_armed() || SRV_Channels::get_emergency_stop()) {
#endif #endif

View File

@ -672,8 +672,10 @@ void Plane::set_servos(void)
#if ADVANCED_FAILSAFE == ENABLED #if ADVANCED_FAILSAFE == ENABLED
if (afs.should_crash_vehicle()) { if (afs.should_crash_vehicle()) {
afs.terminate_vehicle(); afs.terminate_vehicle();
if (!afs.terminating_vehicle_via_landing()) {
return; return;
} }
}
#endif #endif
// do any transition updates for quadplane // do any transition updates for quadplane