ekf2: move aiding start details in dedicated function

When an aiding source needs to be started, simply call the corresponding
starting function and any required reset is handled in there.

src/modules/ekf2/EKF/control.cpp

@@ -813,23 +813,14 @@ void Ekf::controlHeightFusion()
 
 	// FALLTHROUGH
 	case VDIST_SENSOR_BARO:
-		if (do_range_aid && _range_sensor.isDataHealthy()) {
-			setControlRangeHeight();
-
-			// we have just switched to using range finder, calculate height sensor offset such that current
-			// measurement matches our current height estimate
-			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
-				_hgt_sensor_offset = _terrain_vpos;
+		if (do_range_aid) {
+			if (!_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
+				startRngAidHgtFusion();
 			}
 
-		} else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) {
-			startBaroHgtFusion();
-
-		} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
-			// we have just switched to using gps height, calculate height sensor offset such that current
-			// measurement matches our current height estimate
-			if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
-				_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
+		} else {
+			if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
+				startBaroHgtFusion();
 			}
 		}
 
@@ -858,49 +849,34 @@ void Ekf::controlHeightFusion()
 		break;
 
 	case VDIST_SENSOR_GPS:
-
 		// NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
 		// to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
 		// Do switching between GPS and rangefinder if using range finder as a height source when close
 		// to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers.
 		if (do_range_aid) {
 			if (!_control_status_prev.flags.rng_hgt && _range_sensor.isDataHealthy()) {
-				setControlRangeHeight();
-
-				// we have just switched to using range finder, calculate height sensor offset such that current
-				// measurement matches our current height estimate
-				_hgt_sensor_offset = _terrain_vpos;
+				startRngAidHgtFusion();
 			}
 
 		} else {
-			if (_control_status_prev.flags.rng_hgt) {
-				// must stop using range finder so find another sensor now
+			if (!_control_status.flags.gps_hgt) {
 				if (!_gps_hgt_intermittent && _gps_checks_passed) {
-					// GPS quality OK
+					// In fallback mode and GPS has recovered so start using it
 					startGpsHgtFusion();
 
-				} else if (!_baro_hgt_faulty) {
+				} else if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
 					// Use baro as a fallback
 					startBaroHgtFusion();
 				}
-
-			} else if (_control_status.flags.baro_hgt && !_gps_hgt_intermittent && _gps_checks_passed) {
-				// In baro fallback mode and GPS has recovered so start using it
-				startGpsHgtFusion();
 			}
 		}
 
 		break;
 
 	case VDIST_SENSOR_EV:
-
-		// don't start using EV data unless data is arriving frequently, do not reset if pref mode was height
+		// don't start using EV data unless data is arriving frequently
 		if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
-			setControlEVHeight();
-
-			if (!_control_status_prev.flags.rng_hgt) {
-				resetHeight();
-			}
+			startEvHgtFusion();
 		}
 
 		break;
@@ -908,6 +884,7 @@ void Ekf::controlHeightFusion()
 
 	updateBaroHgtBias();
 	updateBaroHgtOffset();
+	checkGroundEffectTimeout();
 
 	if (_control_status.flags.baro_hgt) {
 

src/modules/ekf2/EKF/ekf.h

@@ -915,10 +915,15 @@ private:
 
 	void startBaroHgtFusion();
 	void startGpsHgtFusion();
+	void startRngHgtFusion();
+	void startRngAidHgtFusion();
+	void startEvHgtFusion();
 
 	void updateBaroHgtOffset();
 	void updateBaroHgtBias();
 
+	void checkGroundEffectTimeout();
+
 	// return an estimation of the GPS altitude variance
 	float getGpsHeightVariance();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1284,27 +1284,58 @@ void Ekf::startBaroHgtFusion()
 	// We don't need to set a height sensor offset
 	// since we track a separate _baro_hgt_offset
 	_hgt_sensor_offset = 0.0f;
-
-	// Turn off ground effect compensation if it times out
-	if (_control_status.flags.gnd_effect) {
-		if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
-
-			_control_status.flags.gnd_effect = false;
-		}
-	}
 }
 
 void Ekf::startGpsHgtFusion()
 {
-	setControlGPSHeight();
+	if (!_control_status.flags.gps_hgt) {
+		setControlGPSHeight();
 
-	// we have just switched to using gps height, calculate height sensor offset such that current
-	// measurement matches our current height estimate
-	if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
+		// calculate height sensor offset such that current
+		// measurement matches our current height estimate
 		_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
 	}
 }
 
+void Ekf::startRngHgtFusion()
+{
+	if (!_control_status.flags.rng_hgt) {
+		setControlRangeHeight();
+
+		// Range finder is the primary height source, the ground is now the datum used
+		// to compute the local vertical position
+		_hgt_sensor_offset = 0.f;
+
+		if (!_control_status_prev.flags.ev_hgt) {
+			// EV and range finders are using the same height datum
+			resetHeight();
+		}
+	}
+}
+
+void Ekf::startRngAidHgtFusion()
+{
+	if (!_control_status.flags.rng_hgt) {
+		setControlRangeHeight();
+
+		// calculate height sensor offset such that current
+		// measurement matches our current height estimate
+		_hgt_sensor_offset = _terrain_vpos;
+	}
+}
+
+void Ekf::startEvHgtFusion()
+{
+	if (!_control_status.flags.ev_hgt) {
+		setControlEVHeight();
+
+		if (!_control_status_prev.flags.rng_hgt) {
+			// EV and range finders are using the same height datum
+			resetHeight();
+		}
+	}
+}
+
 void Ekf::updateBaroHgtOffset()
 {
 	// calculate a filtered offset between the baro origin and local NED origin if we are not
@@ -1351,6 +1382,17 @@ void Ekf::updateBaroHgtBias()
 	}
 }
 
+void Ekf::checkGroundEffectTimeout()
+{
+	// Turn off ground effect compensation if it times out
+	if (_control_status.flags.gnd_effect) {
+		if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
+
+			_control_status.flags.gnd_effect = false;
+		}
+	}
+}
+
 Vector3f Ekf::getVisionVelocityInEkfFrame() const
 {
 	Vector3f vel;

src/modules/ekf2/test/test_EKF_gps.cpp

@@ -152,6 +152,7 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback)
 	_sensor_simulator.runSeconds(1);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
 
 	// WHEN: stopping GPS fusion
 	_sensor_simulator.stopGps();