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 } }