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

ekf2: fix yaw estimator velocity accuracy 

- additionally require GPS speed accuracy is within EKF2_REQ_SACC
This commit is contained in:
Daniel Agar 2022-08-06 11:54:57 -04:00
parent d9d127a237
commit 66b55d9d0a
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -1888,9 +1888,11 @@ void Ekf::runYawEKFGSF()
_yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias); _yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
// basic sanity check on GPS velocity data // basic sanity check on GPS velocity data
if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON && if (_gps_data_ready
PX4_ISFINITE(_gps_sample_delayed.vel(0)) && PX4_ISFINITE(_gps_sample_delayed.vel(1))) { && (_gps_sample_delayed.sacc > FLT_EPSILON) && (_gps_sample_delayed.sacc < _params.req_sacc)
_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc); && PX4_ISFINITE(_gps_sample_delayed.vel(0)) && PX4_ISFINITE(_gps_sample_delayed.vel(1))
) {
_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.sacc);
} }
} }