diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index c9ad9b7b12..f9142f4893 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -435,6 +435,11 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints() _trajectory[2].setMaxAccel(9.81f); _trajectory[2].setMaxJerk(9.81f); + // If the current velocity is beyond the usual constraints, tell + // the controller to exceptionally increase its saturations to avoid + // cutting out the feedforward + _constraints.speed_down = math::max(fabsf(_trajectory[2].getCurrentVelocity()), _param_mpc_z_vel_max_dn.get()); + } else if (_velocity_setpoint(2) < 0.f) { // up float z_accel_constraint = _param_mpc_acc_up_max.get(); float z_vel_constraint = _param_mpc_z_vel_max_up.get(); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 4597ce8f8b..c06b868dae 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -429,7 +429,7 @@ void MulticopterPositionControl::Run() _control.setVelocityLimits( math::constrain(speed_horizontal, 0.f, _param_mpc_xy_vel_max.get()), math::min(speed_up, _param_mpc_z_vel_max_up.get()), // takeoff ramp starts with negative velocity limit - math::constrain(speed_down, 0.f, _param_mpc_z_vel_max_dn.get())); + math::max(speed_down, 0.f)); _control.setInputSetpoint(_setpoint);