From aa96fa6d9e873afb8905ebb4046b258ba91214bb Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 3 Jun 2020 11:25:03 +0200 Subject: [PATCH] Random style fixes in the code --- EKF/estimator_interface.cpp | 1 + EKF/imu_down_sampler.cpp | 1 + EKF/terrain_estimator.cpp | 2 +- EKF/vel_pos_fusion.cpp | 1 + 4 files changed, 4 insertions(+), 1 deletion(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 1af98abd81..cedb0a2402 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -367,6 +367,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample& flow) // when there are other aiding sources available const float flow_rate_magnitude = flow.flow_xy_rad.norm() / delta_time; flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate); + } else { // protect against overflow caused by division with very small delta_time delta_time = delta_time_min; diff --git a/EKF/imu_down_sampler.cpp b/EKF/imu_down_sampler.cpp index f0414c4d5e..92cb11980d 100644 --- a/EKF/imu_down_sampler.cpp +++ b/EKF/imu_down_sampler.cpp @@ -45,6 +45,7 @@ bool ImuDownSampler::update(const imuSample &imu_sample_new) { return true; } else { + return false; } } diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index e635a21271..4686a6c4df 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -179,7 +179,7 @@ void Ekf::fuseFlowForTerrain() // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed // correct for gyro bias errors in the data used to do the motion compensation // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. - const Vector2f opt_flow_rate = Vector2f{_flow_compensated_XY_rad} / _flow_sample_delayed.dt + Vector2f{_flow_gyro_bias}; + const Vector2f opt_flow_rate = Vector2f(_flow_compensated_XY_rad) / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias); // get latest estimated orientation const float q0 = _state.quat_nominal(0); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index ea6ca5192b..4aa4b05a04 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -130,6 +130,7 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate fuseVelPosHeight(innov(2), innov_var(2), 5); return true; + } else { _innov_check_fail_status.flags.reject_ver_pos = true; return false;