fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 20:34:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: set uninitialized vel/pos/hgt test ratios to NAN

Browse Source 
- ensure valid min is numerically larger than 0 (float32)
This commit is contained in:
Daniel Agar 2021-06-29 10:17:11 -04:00
parent 3beb5bcbe0
commit 33bf4e0303
1 changed files with 26 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
EKF/ekf_helper.cpp
View File

@ -945,30 +945,47 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
mag = sqrtf(math::max(_yaw_test_ratio, _mag_test_ratio.max()));
// return the largest velocity and position innovation test ratio
vel = NAN;
pos = NAN;
if (_control_status.flags.gps) {
vel = sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1)));
pos = sqrtf(_gps_pos_test_ratio(0));
float gps_vel = sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1)));
vel = math::max(gps_vel, FLT_MIN);
float gps_pos = sqrtf(_gps_pos_test_ratio(0));
pos = math::max(gps_pos, FLT_MIN);
}
if (_control_status.flags.ev_vel) {
vel = math::max(vel, sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1))));
pos = math::max(pos, sqrtf(_ev_pos_test_ratio(0)));
float ev_vel = sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1)));
vel = math::max(math::max(vel, ev_vel), FLT_MIN);
}
if (_control_status.flags.ev_pos) {
float ev_pos = sqrtf(_ev_pos_test_ratio(0));
pos = math::max(math::max(pos, ev_pos), FLT_MIN);
}
if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
vel = sqrtf(_optflow_test_ratio);
float of_vel = sqrtf(_optflow_test_ratio);
vel = math::max(of_vel, FLT_MIN);
}
// return the vertical position innovation test ratio
if (_control_status.flags.baro_hgt) {
hgt = sqrtf(_baro_hgt_test_ratio(1));
hgt = math::max(sqrtf(_baro_hgt_test_ratio(1)), FLT_MIN);
} else if (_control_status.flags.gps_hgt) {
hgt = sqrtf(_gps_pos_test_ratio(1));
hgt = math::max(sqrtf(_gps_pos_test_ratio(1)), FLT_MIN);
} else if (_control_status.flags.rng_hgt) {
hgt = sqrtf(_rng_hgt_test_ratio(1));
hgt = math::max(sqrtf(_rng_hgt_test_ratio(1)), FLT_MIN);
} else if (_control_status.flags.ev_hgt) {
hgt = sqrtf(_ev_pos_test_ratio(1));
hgt = math::max(sqrtf(_ev_pos_test_ratio(1)), FLT_MIN);
} else {
hgt = NAN;
}
// return the airspeed fusion innovation test ratio