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)
{
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) {

View File

@ -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

View File

@ -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

View File

@ -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