diff --git a/EKF/control.cpp b/EKF/control.cpp index d205dbfafc..c90ce59713 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -175,6 +175,11 @@ void Ekf::controlExternalVisionFusion() _control_status.flags.ev_pos = true; ECL_INFO("EKF commencing external vision position fusion"); + if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)) { + // Reset transformation between EV and EKF navigation frames when starting fusion + resetExtVisRotMat(); + } + // reset the position if we are not already aiding using GPS, else use a relative position // method for fusing the position data if (_control_status.flags.gps) { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 06fb8113a1..1d49580f36 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -214,9 +214,8 @@ bool Ekf::initialiseFilter() // check to see if we have enough measurements and return false if not bool hgt_count_fail = _hgt_counter <= 2u * _obs_buffer_length; bool mag_count_fail = _mag_counter <= 2u * _obs_buffer_length; - bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2u * _obs_buffer_length); - if (hgt_count_fail || mag_count_fail || ev_count_fail) { + if (hgt_count_fail || mag_count_fail) { return false; } else { @@ -260,13 +259,6 @@ 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