From 3543071eb9a386534ea65d4fbdcc70797cc6e749 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 6 Feb 2019 16:44:12 +0100 Subject: [PATCH] Position Controller - Saturate the controllers using static parameters. Handle saturations in the flight tasks only --- src/modules/mc_pos_control/PositionControl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index c1c4bc62dc..eaf4affb02 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -221,11 +221,11 @@ void PositionControl::_positionController() // Constrain horizontal velocity by prioritizing the velocity component along the // the desired position setpoint over the feed-forward term. const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position), - Vector2f(_vel_sp - vel_sp_position), _constraints.speed_xy); + Vector2f(_vel_sp - vel_sp_position), MPC_XY_VEL_MAX.get()); _vel_sp(0) = vel_sp_xy(0); _vel_sp(1) = vel_sp_xy(1); // Constrain velocity in z-direction. - _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down); + _vel_sp(2) = math::constrain(_vel_sp(2), -MPC_Z_VEL_MAX_UP.get(), MPC_Z_VEL_MAX_DN.get()); } void PositionControl::_velocityController(const float &dt)