diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index 7143a551ba..0ae0f5b416 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -91,7 +91,7 @@ WindEstimator::update(uint64_t time_now) return; } - float dt = (float)(time_now - _time_last_update) * 1e-6f; + const float dt = (float)(time_now - _time_last_update) * 1e-6f; _time_last_update = time_now; matrix::Matrix3f Qk; @@ -127,7 +127,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const const bool meas_is_rejected = check_if_meas_is_rejected(_tas_innov, _tas_innov_var, _tas_gate); - if (_tas_innov_var < 0.0f) { + if (_tas_innov_var < FLT_EPSILON) { // re init filter in case of a negative variance, and trigger early return to not fuse measurement _initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var); return; @@ -172,7 +172,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const const bool meas_is_rejected = check_if_meas_is_rejected(_beta_innov, _beta_innov_var, _beta_gate); - if (_beta_innov_var < 0.0f) { + if (_beta_innov_var < FLT_EPSILON) { // re init filter in case of a negative variance, and trigger early return to not fuse measurement _initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi()); return; @@ -219,7 +219,7 @@ WindEstimator::run_sanity_checks() // attain symmetry for (unsigned row = 0; row < 3; row++) { for (unsigned column = 0; column < row; column++) { - float tmp = (_P(row, column) + _P(column, row)) / 2; + const float tmp = (_P(row, column) + _P(column, row)) / 2.f; _P(row, column) = tmp; _P(column, row) = tmp; }