From 28b89b024f76868793064b23ba7625a9c616276c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 3 Mar 2021 18:02:28 +0100 Subject: [PATCH] MulticopterPositionControl: fix Takeoff ramp use - guard against NAN speed_up limit as input to ramp see #14339 - allow negative speed_up limit for ramp to work --- src/modules/mc_pos_control/MulticopterPositionControl.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 5be2d92ea7..95c2eb696e 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -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);