diff --git a/EKF/control.cpp b/EKF/control.cpp index 8a18c65655..32bb61e0a2 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -151,9 +151,7 @@ void Ekf::controlExternalVisionFusion() // if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames // needs to be calculated and the observations rotated into the EKF frame of reference - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) - && !(_params.fusion_mode & MASK_USE_EVYAW)) { - + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) { // rotate EV measurements into the EKF Navigation frame calcExtVisRotMat(); } @@ -182,7 +180,7 @@ void Ekf::controlExternalVisionFusion() } // external vision yaw aiding selection logic - if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { + if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { // don't start using EV data unless daa is arriving frequently if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // reset the yaw angle to the value from the observaton quaternion @@ -465,9 +463,12 @@ void Ekf::controlGpsFusion() if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) { // If the heading is not aligned, reset the yaw and magnetic field states - if (!_control_status.flags.yaw_align) { + // Do not use external vision for yaw if using GPS because yaw needs to be + // defined relative to an NED reference frame + if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw) { + _control_status.flags.yaw_align = false; + _control_status.flags.ev_yaw = false; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); - } // If the heading is valid start using gps aiding @@ -490,7 +491,6 @@ void Ekf::controlGpsFusion() if (_control_status.flags.gps) { ECL_INFO("EKF commencing GPS fusion"); _time_last_gps = _time_last_imu; - } } } diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 70cb873582..5ecc258e82 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -567,7 +567,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) Dcmf R_to_earth(euler321); // calculate the observed yaw angle - if (_params.fusion_mode & MASK_USE_EVYAW) { + if (_control_status.flags.ev_yaw) { // convert the observed quaternion to a rotation matrix Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence @@ -621,7 +621,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) R_to_earth(2, 1) = s1; // calculate the observed yaw angle - if (_params.fusion_mode & MASK_USE_EVYAW) { + if (_control_status.flags.ev_yaw) { // convert the observed quaternion to a rotation matrix Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence @@ -703,14 +703,12 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) _R_to_earth = quat_to_invrotmat(_state.quat_nominal); // reset the rotation from the EV to EKF frame of reference if it is being used - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) - && !(_params.fusion_mode & MASK_USE_EVYAW)) { - + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) { resetExtVisRotMat(); } // update the yaw angle variance using the variance of the measurement - if (_params.fusion_mode & MASK_USE_EVYAW) { + if (!_control_status.flags.ev_yaw) { // using error estimate from external vision data angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));