dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
bresch 2021-06-03 15:21:08 +02:00 committed by Beat Küng
parent 316e0dfeb5
commit e53d2907d7
2 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -435,6 +435,11 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
_trajectory[2].setMaxAccel(9.81f); _trajectory[2].setMaxAccel(9.81f);
_trajectory[2].setMaxJerk(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 } else if (_velocity_setpoint(2) < 0.f) { // up
float z_accel_constraint = _param_mpc_acc_up_max.get(); float z_accel_constraint = _param_mpc_acc_up_max.get();
float z_vel_constraint = _param_mpc_z_vel_max_up.get(); float z_vel_constraint = _param_mpc_z_vel_max_up.get();

2
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -429,7 +429,7 @@ void MulticopterPositionControl::Run()
_control.setVelocityLimits( _control.setVelocityLimits(
math::constrain(speed_horizontal, 0.f, _param_mpc_xy_vel_max.get()), 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::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); _control.setInputSetpoint(_setpoint);