Allow rangefinder fusion in vision height mode (Fix for #994) (#999)

Co-authored-by: Daniel Agar <daniel@agar.ca>

EKF/control.cpp

@@ -788,8 +788,7 @@ void Ekf::controlHeightSensorTimeouts()
 
 	// check if height has been inertial deadreckoning for too long
 	// in vision hgt mode check for vision data
-	const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6)  ||
-			 (_control_status.flags.ev_hgt && !isRecent(_time_last_ext_vision, 5 * EV_MAX_INTERVAL));
+	const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
 
 	if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
 
@@ -1079,15 +1078,13 @@ void Ekf::controlHeightFusion()
 			}
 		}
 
-		if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
-			// switch to baro if there was a reset to baro
-			fuse_height = true;
-		}
-
-		// determine if we should use the vertical position observation
-		if (_control_status.flags.ev_hgt) {
-			fuse_height = true;
-		}
+		if (_control_status.flags.ev_hgt && _ev_data_ready) {
+		     fuse_height = true;
+		} else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
+		      fuse_height = true;
+		} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
+		      fuse_height = true;
+	    }
 
 		break;
 	}