From 4d831707463628ec61af99bd0caac29fe08492f9 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 11 Jan 2020 21:40:23 +0100 Subject: [PATCH] mc_pos_control: do not ignore EKF vz with terrain following This caused bad altitude control performance when enabling terrain following. It even leads to complete vertical control instability in case dist_bottom is inaccurate. Relying on the estimator states is the way to go instead of silently using one altitude source as state. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) 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 197a59a677..a94608318e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -462,13 +462,7 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) _vel_y_deriv.update(0.0f); } - if (_param_mpc_alt_mode.get() && _local_pos.dist_bottom_valid && PX4_ISFINITE(_local_pos.dist_bottom_rate)) { - // terrain following - _states.velocity(2) = -_local_pos.dist_bottom_rate; - _states.acceleration(2) = _vel_z_deriv.update(-_states.velocity(2)); - - } else if (PX4_ISFINITE(_local_pos.vz)) { - + 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)) {