From e53d2907d76fd4a5e5bd8584b42d058fbaf6cbd1 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 3 Jun 2021 15:21:08 +0200 Subject: [PATCH] AutoSmoothVel: adjust controller constraints in emergency braking When engaging auto mode at high vertical speed, we don't want to cut the velocity trajectory setpoint in order to create a smooth deceleration. --- .../tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp | 5 +++++ src/modules/mc_pos_control/MulticopterPositionControl.cpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) 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);