fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 20:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: EKFGSF_yaw minor cleanup (#20510)

Browse Source 
- don't store unnecessary IMU copy in class, pass it through where required
 - remove custom pi constants
 - remove unused fields from ahrs_ekf_gsf (vel_NE, fuse_gps, accel_dt)
 - explicitly initialize everything in header
 - apply PX4 code style
 - set GPS velocity before yaw estimator update, not after
This commit is contained in:
Daniel Agar
2022-12-09 13:24:00 -05:00
committed by GitHub
parent 643eed51cb
commit 8cf13d50a8
5 changed files with 150 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+6 -1
View File
@@ -121,7 +121,12 @@ void Ekf::controlFusionModes()
_gps_sample_delayed.pos -= pos_offset_earth.xy();
_gps_sample_delayed.hgt += pos_offset_earth(2);
_gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise);
// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
if ((_gps_sample_delayed.sacc > 0.f) && (_gps_sample_delayed.sacc < _params.req_sacc)
&& _gps_sample_delayed.vel.isAllFinite()
) {
_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), math::max(_gps_sample_delayed.sacc, _params.gps_vel_noise));
}
}
}