From 1fcfd394dd61eaf4f7ed8c58fbf98a865c898a12 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Thu, 26 Dec 2019 16:20:11 +0100 Subject: [PATCH] Various small non-functional improvements --- EKF/control.cpp | 48 +++++++++---------------------------- EKF/estimator_interface.cpp | 2 +- EKF/vel_pos_fusion.cpp | 8 +++---- 3 files changed, 16 insertions(+), 42 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 93eda4a393..401abc79b2 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -789,10 +789,9 @@ void Ekf::controlHeightSensorTimeouts() // check if height has been inertial deadreckoning for too long bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > (uint64_t)5e6); - // reset the vertical position and velocity states if (hgt_fusion_timeout || continuous_bad_accel_hgt) { - // boolean that indicates we will do a height reset - bool reset_height = false; + + bool request_height_reset = false; // handle the case where we are using baro for height if (_control_status.flags.baro_hgt) { @@ -816,8 +815,7 @@ void Ekf::controlHeightSensorTimeouts() setControlGPSHeight(); - // request a reset - reset_height = true; + request_height_reset = true; ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to GPS"); } else if (baro_data_available) { @@ -826,15 +824,9 @@ void Ekf::controlHeightSensorTimeouts() setControlBaroHeight(); - // request a reset - reset_height = true; + request_height_reset = true; ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to baro"); - } else { - // we have nothing we can reset to - // deny a reset - reset_height = false; - } } @@ -861,21 +853,15 @@ void Ekf::controlHeightSensorTimeouts() setControlBaroHeight(); - // request a reset - reset_height = true; + request_height_reset = true; ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to baro"); } else if (!_gps_hgt_intermittent) { setControlGPSHeight(); - // request a reset - reset_height = true; + request_height_reset = true; ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to GPS"); - } else { - // we have nothing to reset to - reset_height = false; - } } @@ -890,8 +876,7 @@ void Ekf::controlHeightSensorTimeouts() setControlRangeHeight(); - // request a reset - reset_height = true; + request_height_reset = true; ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to rng hgt"); } else if (baro_data_available) { @@ -900,14 +885,9 @@ void Ekf::controlHeightSensorTimeouts() setControlBaroHeight(); - // request a reset - reset_height = true; + request_height_reset = true; ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to baro"); - } else { - // we have nothing to reset to - reset_height = false; - } } @@ -924,8 +904,7 @@ void Ekf::controlHeightSensorTimeouts() if (ev_data_available) { setControlEVHeight(); - // request a reset - reset_height = true; + request_height_reset = true; ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to ev hgt"); } else if (baro_data_available) { @@ -934,19 +913,14 @@ void Ekf::controlHeightSensorTimeouts() setControlBaroHeight(); - // request a reset - reset_height = true; + request_height_reset = true; ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to baro"); - } else { - // we have nothing to reset to - reset_height = false; - } } // Reset vertical position and velocity states to the last measurement - if (reset_height) { + if (request_height_reset) { resetHeight(); // Reset the timout timer _time_last_hgt_fuse = _time_last_imu; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 6a580f78d6..8f324d5719 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -367,7 +367,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // check if enough integration time and fail if integration time is less than 50% // of min arrival interval because too much data is being lost float delta_time = 1e-6f * (float)flow->dt; // in seconds - float delta_time_min = 0.5f * 1e-6f * (float)_min_obs_interval_us; + const float delta_time_min = 0.5e-6f * (float)_min_obs_interval_us; bool delta_time_good = delta_time >= delta_time_min; bool flow_magnitude_good = true; diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 8def30d2db..b96cf72d8f 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -51,8 +51,8 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_ga { innov_var(0) = P(4,4) + obs_var(0); innov_var(1) = P(5,5) + obs_var(1); - test_ratio(0) = fmaxf( sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)), - sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); + test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)), + sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); const bool innov_check_pass = (test_ratio(0) <= 1.0f); if (innov_check_pass) @@ -95,8 +95,8 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga { innov_var(0) = P(7,7) + obs_var(0); innov_var(1) = P(8,8) + obs_var(1); - test_ratio(0) = fmaxf( sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)), - sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); + test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)), + sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); const bool innov_check_pass = (test_ratio(0) <= 1.0f) || !_control_status.flags.tilt_align; if (innov_check_pass) {