Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 5 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+3 -2
View File
@@ -355,7 +355,8 @@ void MulticopterPositionControl::Run()
_control.setTiltLimit(math::radians(_param_mpc_tiltmax_air.get())); _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 : const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down :
_param_mpc_z_vel_max_dn.get(); _param_mpc_z_vel_max_dn.get();
const float speed_horizontal = PX4_ISFINITE(_vehicle_constraints.speed_xy) ? _vehicle_constraints.speed_xy : const float speed_horizontal = PX4_ISFINITE(_vehicle_constraints.speed_xy) ? _vehicle_constraints.speed_xy :
@@ -368,7 +369,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::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())); math::constrain(speed_down, 0.f, _param_mpc_z_vel_max_dn.get()));
_control.setInputSetpoint(_setpoint); _control.setInputSetpoint(_setpoint);