fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: improve failsafe handling

Browse Source 
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.
This commit is contained in:
Matthias Grob 2019-10-08 20:50:17 +02:00
parent 068f56d66f
commit 001da78089
1 changed files with 21 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -963,23 +963,36 @@ 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;
}
}