fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-12 21:50:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Enable use of rotated external nav estimates

Browse Source
This commit is contained in:
Paul Riseborough
2017-10-23 08:28:22 +11:00
parent f921b2de5c
commit 01d68ef67c
7 changed files with 249 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+6
View File
@@ -273,6 +273,11 @@ bool Ekf::initialiseFilter()
// calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(mag_init);
// initialise the rotation from EV to EKF navigation frame if required
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
resetExtVisRotMat();
}
if (_control_status.flags.rng_hgt) {
// if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup
// so it can be used as a backup ad set the initial height using the range finder
@@ -297,6 +302,7 @@ bool Ekf::initialiseFilter()
// reset the essential fusion timeout counters
_time_last_hgt_fuse = _time_last_imu;
_time_last_pos_fuse = _time_last_imu;
_time_last_delpos_fuse = _time_last_imu;
_time_last_vel_fuse = _time_last_imu;
_time_last_hagl_fuse = _time_last_imu;
_time_last_of_fuse = _time_last_imu;