EKF: Ensure use of EV aiding inhibits use of other height sources

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;

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;