From 981fd98564a943933b93552d3a27f1ed48263614 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 12 Jul 2018 22:29:20 +0100 Subject: [PATCH] position_estimator_inav: simplify validation check --- .../position_estimator_inav_main.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 8f27185337..4b69f6d436 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -792,14 +792,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) static float last_vision_y = 0.0f; static float last_vision_z = 0.0f; - vision_xy_valid = (!PX4_ISFINITE(visual_odom.pose_covariance[0]) ? sqrtf(fmaxf(visual_odom.pose_covariance[0], - visual_odom.pose_covariance[6])) > ep_max_std_dev : true) ? false : true; - vision_z_valid = (!PX4_ISFINITE(visual_odom.pose_covariance[0]) ? visual_odom.pose_covariance[11] > ep_max_std_dev : - true) ? false : true; - vision_vxy_valid = (!PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? sqrtf(fmaxf(visual_odom.velocity_covariance[0], - visual_odom.velocity_covariance[6])) > ev_max_std_dev : true) ? false : true; - vision_vz_valid = (!PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? visual_odom.velocity_covariance[11] > - ep_max_std_dev : true) ? false : true; + vision_xy_valid = !PX4_ISFINITE(visual_odom.pose_covariance[0]) ? sqrtf(fmaxf(visual_odom.pose_covariance[0], + visual_odom.pose_covariance[6])) > ep_max_std_dev : true; + vision_z_valid = !PX4_ISFINITE(visual_odom.pose_covariance[0]) ? visual_odom.pose_covariance[11] > ep_max_std_dev : + true; + vision_vxy_valid = !PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? sqrtf(fmaxf(visual_odom.velocity_covariance[0], + visual_odom.velocity_covariance[6])) > ev_max_std_dev : true; + vision_vz_valid = !PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? visual_odom.velocity_covariance[11] > + ep_max_std_dev : true; /* reset position estimate on first vision update */ if (vision_xy_valid) {