diff --git a/EKF/control.cpp b/EKF/control.cpp index f16ab8da63..d49b2d3bdc 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -60,8 +60,12 @@ void Ekf::controlFusionModes() if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { // check for a exernal vision measurement that has fallen behind the fusion time horizon if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { - // turn on use of external vision measurements for position + // turn on use of external vision measurements for position and height _control_status.flags.ev_pos = true; + // turn off other forms of height aiding + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; // reset the position, height and velocity resetPosition(); resetVelocity(); @@ -456,7 +460,11 @@ void Ekf::controlFusionModes() } // Control the soure of height measurements for the main filter - if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) { + if (_control_status.flags.ev_pos) { + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) { _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 2b8d87d71a..607a9c901e 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -120,7 +120,7 @@ void Ekf::fuseVelPosHeight() } if (_fuse_height) { - if (_control_status.flags.baro_hgt && !_control_status.flags.gps && !_control_status.flags.ev_pos && !(_control_status.flags.rng_hgt && (_R_to_earth(2, 2) > 0.7071f))) { + if (_control_status.flags.baro_hgt) { fuse_map[5] = true; // vertical position innovation - baro measurement has opposite sign to earth z axis _vel_pos_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;