From 114e85d260d4acb157e191fd8cfc1f58c9c73e4e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 5 Mar 2021 16:38:38 +0100 Subject: [PATCH] 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. --- src/modules/mc_pos_control/MulticopterPositionControl.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index fcb62e6bac..c3a40737da 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -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); }