From 1914053db20dcf64596680ffaf4b417ea10fe984 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 15 Sep 2022 16:22:47 +0200 Subject: [PATCH] ekf2: synchronize z_valid and v_z_valid mc_pos_control doesn't handle all the cases at the moment --- src/modules/ekf2/EKF2.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 525bf3f698..dc49593078 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1096,11 +1096,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.ay = vel_deriv(1); lpos.az = vel_deriv(2); - // TODO: better status reporting lpos.xy_valid = _ekf.local_position_is_valid(); - lpos.z_valid = _ekf.isLocalVerticalPositionValid(); lpos.v_xy_valid = _ekf.local_position_is_valid(); - lpos.v_z_valid = _ekf.isLocalVerticalVelocityValid(); + + // TODO: some modules (e.g.: mc_pos_control) don't handle v_z_valid != z_valid properly + lpos.z_valid = _ekf.isLocalVerticalPositionValid() || _ekf.isLocalVerticalVelocityValid(); + lpos.v_z_valid = _ekf.isLocalVerticalVelocityValid() || _ekf.isLocalVerticalPositionValid(); // Position of local NED origin in GPS / WGS84 frame if (_ekf.global_origin_valid()) {