From c0890793214a0d1cde52700a54e2386c5ceba338 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 12 Feb 2016 14:28:40 +1100 Subject: [PATCH] EKF: Split tilt and yaw align --- EKF/control.cpp | 4 ++-- EKF/ekf.cpp | 7 +++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 46cdb2a691..f071ab97e7 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -53,9 +53,9 @@ void Ekf::controlFusionModes() _control_status.flags.opt_flow = false; // GPS fusion mode selection logic - // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data + // To start using GPS we need tilt and yaw alignment completed, the local NED origin set and fresh GPS data if (!_control_status.flags.gps) { - if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised + if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) { _control_status.flags.gps = true; resetPosition(); diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 0e8a0267c1..a1d9fc4b44 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -143,15 +143,18 @@ bool Ekf::update() // Fuse magnetometer data using the selected fuson method and only if angular alignment is complete if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { - if (_control_status.flags.mag_3D && _control_status.flags.angle_align) { + if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) { fuseMag(); if (_control_status.flags.mag_dec) { fuseDeclination(); } - } else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) { + } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { fuseHeading(); + + } else { + // do no fusion at all } }