dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MultiCopterPositionControl: hotfix emergency failsafe 

that prevents the vehicle from crashing with invalid setpoints or
states.

This broke with #16869 when the scheduling of the position control module
and the setpoint generation got independent. The failsafe mechanism assumed
the setpoint is overwirtten by the possibly infeasible input on every loop
iteration which is not the case anymore. As a result the failsafe reset its
histeresis based on the failsafe setpoint from the last loop iteration.

Keeping the failsafe_setpoint separate solves this issue. Note that
these setpoints to the bare minimum to keep the vehicle safely in the air
and do not suffer from sideffects ignoring the EKF reset.
This commit is contained in:
Matthias Grob 2021-03-05 16:38:38 +01:00 committed by Daniel Agar
parent fddcb73802
commit 114e85d260
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -407,12 +407,14 @@ void MulticopterPositionControl::Run()
_last_warn = time_stamp_now;
}
failsafe(time_stamp_now, _setpoint, states, !was_in_failsafe);
vehicle_local_position_setpoint_s failsafe_setpoint{};
failsafe(time_stamp_now, failsafe_setpoint, states, !was_in_failsafe);
// reset constraints
_vehicle_constraints = {0, NAN, NAN, NAN, false, {}};
_control.setInputSetpoint(_setpoint);
_control.setInputSetpoint(failsafe_setpoint);
_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
_control.update(dt);
}