From f11a65a74fccb07ebc274810831e52dc744cbcda Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 27 Jun 2018 09:50:20 +0200 Subject: [PATCH] mc_pos_control: fix overwrite for state velocity --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d795be0f30..9f2bd74887 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -440,6 +440,8 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) } else if (PX4_ISFINITE(_local_pos.vz)) { + _states.velocity(2) = _local_pos.vz; + if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) { // A change in velocity is demanded. Set velocity to the derivative of position // because it has less bias but blend it in across the landing speed range @@ -447,7 +449,6 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) _states.velocity(2) = _local_pos.z_deriv * weighting + _local_pos.vz * (1.0f - weighting); } - _states.velocity(2) = _local_pos.vz; _states.acceleration(2) = _vel_z_deriv.update(-_states.velocity(2)); } else {