From 001da78089475621d5721bcb698f8f946f67f627 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 8 Oct 2019 20:50:17 +0200 Subject: [PATCH] mc_pos_control: improve failsafe handling See issue #12307 Since commander should still handle all failsafes we should only run into this case as last resort to not crash. If all failsafe actions are disabled but data is missing e.g. RC loss action disabled but flying in manual and no RC this can be tested. --- .../mc_pos_control/mc_pos_control_main.cpp | 29 ++++++++++++++----- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index fe51658941..abf84c7181 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -963,21 +963,34 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint } else { reset_setpoint_to_nan(setpoint); - if (PX4_ISFINITE(_states.velocity(2))) { - // We have a valid velocity in D-direction. - // descend downwards with landspeed. - setpoint.vz = _param_mpc_land_speed.get(); - setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; + if (PX4_ISFINITE(_states.velocity(0)) && PX4_ISFINITE(_states.velocity(1))) { + // don't move along xy + setpoint.vx = setpoint.vy = 0.0f; if (warn) { - PX4_WARN("Failsafe: Descend with land-speed."); + PX4_WARN("Failsafe: stop and wait"); } } else { - // Use the failsafe from the PositionController. + // descend with land speed since we can't stop + setpoint.thrust[0] = setpoint.thrust[1] = 0.f; + setpoint.vz = _param_mpc_land_speed.get(); + if (warn) { - PX4_WARN("Failsafe: Descend with just attitude control."); + PX4_WARN("Failsafe: blind hover"); + } + } + + if (PX4_ISFINITE(_states.velocity(2))) { + // don't move along z if we can stop in all dimensions + if (!PX4_ISFINITE(setpoint.vz)) { + setpoint.vz = 0.f; } + + } else { + // emergency descend with a bit below hover thrust + setpoint.vz = NAN; + setpoint.thrust[2] = _param_mpc_thr_hover.get() * .8f; } _in_failsafe = true;