dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

WindEstimator: add consts, fix float comparison to 0 and use consistently floats in division 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-01-17 14:21:08 +01:00 committed by Daniel Agar
parent 15335b194a
commit a6d14796e4
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/lib/wind_estimator/WindEstimator.cpp
View File

@ -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;
}