From efe312d4a66402b31787d3240438a54e7534203c Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 12 Jul 2018 20:45:59 +0100 Subject: [PATCH] style and format clean --- src/modules/ekf2/ekf2_main.cpp | 10 ++++++---- .../BlockLocalPositionEstimator.cpp | 2 ++ .../local_position_estimator/sensors/mocap.cpp | 1 + .../sensors/vision.cpp | 3 ++- .../position_estimator_inav_main.cpp | 18 ++++++++++++------ 5 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 1a488b3f41..f47924e7cc 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1178,13 +1178,15 @@ void Ekf2::run() ev_data.posErr = _ev_pos_noise.get(); ev_data.hgtErr = _ev_pos_noise.get(); } + // orientation measurement error from parameters if (!PX4_ISFINITE(ev_odom.pose_covariance[0])) { - ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmax(ev_odom.pose_covariance[18], ev_odom.pose_covariance[20])))); + ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmax(ev_odom.pose_covariance[18], + ev_odom.pose_covariance[20])))); + + } else { + ev_data.angErr = _ev_ang_noise.get(); } - else { - ev_data.angErr = _ev_ang_noise.get(); - } // only set data if all positions are valid if (sqrtf(ev_odom.pose_covariance[0]) < ep_max_std_dev && sqrtf(ev_odom.pose_covariance[6]) < ep_max_std_dev) { diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index d14777b048..4990200c07 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -667,10 +667,12 @@ void BlockLocalPositionEstimator::publishEstimatorStatus() _pub_est_status.get().covariances[13] = _P(X_bx, X_bx); _pub_est_status.get().covariances[14] = _P(X_by, X_by); _pub_est_status.get().covariances[15] = _P(X_bz, X_bz); + // mag - not determined for (size_t i = 16; i <= 21; i++) { _pub_est_status.get().covariances[i] = NAN; } + // replacing the hor wind cov with terrain altitude covariance _pub_est_status.get().covariances[22] = _P(X_tz, X_tz); _pub_est_status.get().covariances[23] = NAN; diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index 83855f1119..a72b3b1d40 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -66,6 +66,7 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector &y) _mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[11]); _mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV; _mocap_z_valid = _mocap_epv <= EP_MAX_STD_DEV; + } else { // if we don't have covariances, assume every reading _mocap_xy_valid = true; diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index cdc84280e2..ea43192e30 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -71,6 +71,7 @@ int BlockLocalPositionEstimator::visionMeasure(Vector &y) _vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[11]); _vision_xy_valid = _vision_eph <= EP_MAX_STD_DEV; _vision_z_valid = _vision_epv <= EP_MAX_STD_DEV; + } else { // if we don't have covariances, assume every reading _vision_xy_valid = true; @@ -138,7 +139,7 @@ void BlockLocalPositionEstimator::visionCorrect() if (vision_delay < 0.0f) { vision_delay = 0.0f; } // use auto-calculated delay from measurement if parameter is set to zero - if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist) < 0){ return; } + if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist) < 0) { return; } Vector x0 = _xDelay.get(i_hist); 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 8090fcb92d..8f27185337 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -792,10 +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) ? 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; /* reset position estimate on first vision update */ if (vision_xy_valid) { @@ -909,8 +913,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_mocap_odometry), mocap_position_sub, &mocap); - mocap_xy_valid = (!PX4_ISFINITE(mocap.pose_covariance[0]) ? sqrtf(fmaxf(mocap.pose_covariance[0], mocap.pose_covariance[6])) > ep_max_std_dev : true) ? false : true; - mocap_z_valid = (!PX4_ISFINITE(mocap.pose_covariance[0]) ? mocap.pose_covariance[11] > ep_max_std_dev : true) ? false : true; + mocap_xy_valid = (!PX4_ISFINITE(mocap.pose_covariance[0]) ? sqrtf(fmaxf(mocap.pose_covariance[0], + mocap.pose_covariance[6])) > ep_max_std_dev : true) ? false : true; + mocap_z_valid = (!PX4_ISFINITE(mocap.pose_covariance[0]) ? mocap.pose_covariance[11] > ep_max_std_dev : true) ? false : + true; if (!params.disable_mocap) { /* reset position estimate on first mocap update */