From 841914462d23983b932ec9a1c9dc5a0dc88b8313 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 7 Jun 2021 16:49:37 +0200 Subject: [PATCH] MC pos control: use vz<->z_deriv blending in pure vel mode only This logic was introduced to avoid large altitude drifts in velocity mode (altitude not controlled) due to potential velocity estimate bias in case of IMU problems (see PR #8445). After some refactorings, the logic started to be used in altitude-controlled modes as well, which is not needed. --- src/modules/mc_pos_control/MulticopterPositionControl.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index d3286423c9..e55476a707 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -404,9 +404,11 @@ void MulticopterPositionControl::Run() _control.setInputSetpoint(_setpoint); // update states - if (PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON) + if (!PX4_ISFINITE(_setpoint.z) + && PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON) && PX4_ISFINITE(local_pos.z_deriv) && local_pos.z_valid && local_pos.v_z_valid) { - // A change in velocity is demanded. Set velocity to the derivative of position + // A change in velocity is demanded and the altitude is not controlled. + // Set velocity to the derivative of position // because it has less bias but blend it in across the landing speed range // < MPC_LAND_SPEED: ramp up using altitude derivative without a step // >= MPC_LAND_SPEED: use altitude derivative