Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

position_estimator_inav: fix covariance check logic

Browse Source
This commit is contained in:
TSC21
2018-11-19 17:51:45 +00:00
committed by Lorenz Meier
parent 20b3adebf4
commit adc81aaab5
1 changed files with 20 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+20 -17
View File
@@ -792,18 +792,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
static float last_vision_y = 0.0f; static float last_vision_y = 0.0f;
static float last_vision_z = 0.0f; static float last_vision_z = 0.0f;
vision_xy_valid = PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf( vision_xy_valid = PX4_ISFINITE(visual_odom.x)
visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE], && (PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf(
visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Y_VARIANCE])) > ep_max_std_dev : true; visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE],
vision_z_valid = PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Y_VARIANCE])) <= ep_max_std_dev : true);
visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Z_VARIANCE] > ep_max_std_dev : vision_z_valid = PX4_ISFINITE(visual_odom.z)
true; && (PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ?
vision_vxy_valid = PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? sqrtf( visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Z_VARIANCE] <= ep_max_std_dev : true);
vision_vxy_valid = PX4_ISFINITE(visual_odom.vx)
&& PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? sqrtf(
fmaxf(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE], fmaxf(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE],
visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VY_VARIANCE])) > ev_max_std_dev : true; visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VY_VARIANCE])) <= ev_max_std_dev : true;
vision_vz_valid = PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? vision_vz_valid = PX4_ISFINITE(visual_odom.vz)
visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VZ_VARIANCE] > && (PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ?
ep_max_std_dev : true; visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VZ_VARIANCE] <= ep_max_std_dev : true);
/* reset position estimate on first vision update */ /* reset position estimate on first vision update */
if (vision_xy_valid) { if (vision_xy_valid) {
@@ -917,12 +919,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) { if (updated) {
orb_copy(ORB_ID(vehicle_mocap_odometry), mocap_position_sub, &mocap); orb_copy(ORB_ID(vehicle_mocap_odometry), mocap_position_sub, &mocap);
mocap_xy_valid = (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf( mocap_xy_valid = PX4_ISFINITE(mocap.x)
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE], && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf(
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Y_VARIANCE])) > ep_max_std_dev : true) ? false : true; mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE],
mocap_z_valid = (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Y_VARIANCE])) <= ep_max_std_dev : true);
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Z_VARIANCE] > ep_max_std_dev : true) ? false : mocap_z_valid = PX4_ISFINITE(mocap.z)
true; && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ?
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Z_VARIANCE] <= ep_max_std_dev : true);
if (!params.disable_mocap) { if (!params.disable_mocap) {
/* reset position estimate on first mocap update */ /* reset position estimate on first mocap update */