Height source vision: Fallback to rangefinder if available (#994)

EKF/control.cpp

@@ -787,7 +787,9 @@ void Ekf::controlHeightSensorTimeouts()
 	const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us);
 
 	// check if height has been inertial deadreckoning for too long
-	const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
+	// 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));
 
 	if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
 
@@ -876,6 +878,13 @@ void Ekf::controlHeightSensorTimeouts()
 				failing_height_source = "ev";
 				new_height_source = "ev";
 
+			// Fallback to rangefinder data if available
+			} else if (_range_sensor.isHealthy()) {
+				setControlRangeHeight();
+				request_height_reset = true;
+				failing_height_source = "ev";
+				new_height_source = "rng";
+
 			} else if (!_baro_hgt_faulty) {
 				startBaroHgtFusion();
 
@@ -1061,11 +1070,13 @@ void Ekf::controlHeightFusion()
 
 	case VDIST_SENSOR_EV:
 
-		// don't start using EV data unless data is arriving frequently
+		// don't start using EV data unless data is arriving frequently, do not reset if pref mode was height
 		if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
 			fuse_height = true;
 			setControlEVHeight();
-			resetHeight();
+			if (!_control_status_prev.flags.rng_hgt) {
+				 resetHeight();
+			}
 		}
 
 		if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {

EKF/ekf_helper.cpp

@@ -262,6 +262,20 @@ void Ekf::resetHeight()
 
 	// reset the vertical position
 	if (_control_status.flags.rng_hgt) {
+
+		// a fallback from any other height source to rangefinder happened
+		if(!_control_status_prev.flags.rng_hgt) {
+
+			if (_control_status.flags.in_air && isTerrainEstimateValid()) {
+			    _hgt_sensor_offset = _terrain_vpos;
+			} else if (_control_status.flags.in_air) {
+				 _hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
+			} else {
+				_hgt_sensor_offset = _params.rng_gnd_clearance;
+			}
+
+		}
+
 		// update the state and associated variance
 		resetVerticalPositionTo(_hgt_sensor_offset - _range_sensor.getDistBottom());
 

test/test_EKF_fusionLogic.cpp

@@ -379,6 +379,7 @@ TEST_F(EkfFusionLogicTest, doVisionHeightFusion)
 	_sensor_simulator.runSeconds(12);
 
 	// THEN: EKF should stop to intend to use vision height
-	// TODO: This is not happening
-	EXPECT_TRUE(_ekf_wrapper.isIntendingVisionHeightFusion()); // TODO: Needs to change
+
+	EXPECT_FALSE(_ekf_wrapper.isIntendingVisionHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
 }