From 4fc7348582db78ad906a368383c2d42e9fdacdf5 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 6 Jan 2022 19:07:01 +0100 Subject: [PATCH] mpc: leave room for altitude error control during descent and descent Since the same parameter is used to generate the trajectory and to saturate the controller, there is no additional space for the output of the position controller once it is filled with the feedforward, letting the altitude grow uncontrolled. --- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index d7ad0ed872..1c0717c630 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -830,6 +830,11 @@ void FlightTaskAuto::_updateTrajConstraints() _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get()); } + + // Stretch the constraints of the velocity controller to leave some room for an additional + // correction required by the altitude/vertical position controller + _constraints.speed_down = math::max(_constraints.speed_down, 1.2f * _param_mpc_z_v_auto_dn.get());; + _constraints.speed_up = math::max(_constraints.speed_up, 1.2f * _param_mpc_z_v_auto_up.get());; } bool FlightTaskAuto::_highEnoughForLandingGear()