From a6cc972b5c14903903dca5ee939ace3baa719c1a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 11 Jan 2020 21:43:58 +0100 Subject: [PATCH] mc_pos_control: do not use invalid vz estimate --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 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 a94608318e..c5aa244aca 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -462,7 +462,7 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) _vel_y_deriv.update(0.0f); } - if (PX4_ISFINITE(_local_pos.vz)) { + if (PX4_ISFINITE(_local_pos.vz) && _local_pos.v_z_valid) { _states.velocity(2) = _local_pos.vz; if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) {