Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: ensure gyro biases are numbers

Browse Source 
avoid errors during compass mot
This commit is contained in:
Andy Piper 2023-03-11 17:01:55 +00:00 committed by Peter Barker
parent 8db14c2bbf
commit 0408ff0ab1
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -229,6 +229,11 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx)
if (spinRate_squared < sq(0.175f)) { if (spinRate_squared < sq(0.175f)) {
AHRS[mdl_idx].gyro_bias -= tilt_error_gyro_correction * (EKFGSF_gyroBiasGain * angle_dt); AHRS[mdl_idx].gyro_bias -= tilt_error_gyro_correction * (EKFGSF_gyroBiasGain * angle_dt);
// sanity check
if (AHRS[mdl_idx].gyro_bias.is_nan()) {
AHRS[mdl_idx].gyro_bias.zero();
}
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
AHRS[mdl_idx].gyro_bias[i] = constrain_ftype(AHRS[mdl_idx].gyro_bias[i], -gyro_bias_limit, gyro_bias_limit); AHRS[mdl_idx].gyro_bias[i] = constrain_ftype(AHRS[mdl_idx].gyro_bias[i], -gyro_bias_limit, gyro_bias_limit);
} }