Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: ensure gyro biases are numbers 

avoid errors during compass mot
This commit is contained in:
Andy Piper 2023-03-11 17:01:55 +00:00 committed by Randy Mackay
parent 708124183d
commit fc7f182214
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

@ -230,6 +230,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);
} }