fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

MulticopterPositionControl: fix Takeoff ramp use

Browse Source 
- guard against NAN speed_up limit as input to ramp see #14339
- allow negative speed_up limit for ramp to work
This commit is contained in:
Matthias Grob 2021-03-03 18:02:28 +01:00
parent 0eb327743e
commit 28b89b024f
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -355,7 +355,8 @@ void MulticopterPositionControl::Run()
_control.setTiltLimit(math::radians(_param_mpc_tiltmax_air.get()));
}
const float speed_up = _takeoff.updateRamp(dt, _vehicle_constraints.speed_up);
const float speed_up = _takeoff.updateRamp(dt,
PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get());
const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down :
_param_mpc_z_vel_max_dn.get();
const float speed_horizontal = PX4_ISFINITE(_vehicle_constraints.speed_xy) ? _vehicle_constraints.speed_xy :
@ -368,7 +369,7 @@ void MulticopterPositionControl::Run()
_control.setVelocityLimits(
math::constrain(speed_horizontal, 0.f, _param_mpc_xy_vel_max.get()),
math::constrain(speed_up, 0.f, _param_mpc_z_vel_max_up.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()));
_control.setInputSetpoint(_setpoint);