EKF2: set baro bias when GNSS is alt ref but fusion is disabled

src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp

@@ -85,14 +85,21 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 		}
 
 		// determine if we should use height aiding
+		const bool common_conditions_passing = measurement_valid
+						       && _local_origin_lat_lon.isInitialized()
+						       && _gps_checks_passed;
+
 		const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
-				&& measurement_valid
-				&& _local_origin_lat_lon.isInitialized()
-				&& _gps_checks_passed;
+				&& common_conditions_passing;
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
 
+		const bool altitude_initialisation_conditions_passing = common_conditions_passing
+				&& !PX4_ISFINITE(_local_origin_alt)
+				&& _params.height_sensor_ref == static_cast<int32_t>(HeightSensor::GNSS)
+				&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
+
 		if (_control_status.flags.gps_hgt) {
 			if (continuing_conditions_passing) {
 
@@ -142,6 +149,15 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 				aid_src.time_last_fuse = _time_delayed_us;
 				bias_est.setFusionActive();
 				_control_status.flags.gps_hgt = true;
+
+			} if (altitude_initialisation_conditions_passing) {
+
+				// Do not start GNSS altitude aiding, but use measurement
+				// to initialize altitude and bias of other height sensors
+				_information_events.flags.reset_hgt_to_gps = true;
+
+				initialiseAltitudeTo(measurement, measurement_var);
+				bias_est.reset();
 			}
 		}
 

src/modules/ekf2/module.yaml

@@ -92,6 +92,8 @@ parameters:
           by this parameter. The range sensor and vision options should only be used
           when for operation over a flat surface as the local NED origin will move
           up and down with ground level.
+          If GPS is set as reference but altitude fusion is disabled in EKF2_GPS_CTRL,
+          the GPS altitude is still used to initiaize the bias of the other height sensors.
       type: enum
       values:
         0: Barometric pressure

src/modules/ekf2/test/test_EKF_height_fusion.cpp

@@ -208,6 +208,32 @@ TEST_F(EkfHeightFusionTest, gpsRef)
 	EXPECT_NEAR(_ekf->aid_src_rng_hgt().innovation, 0.f, 0.2f);
 }
 
+TEST_F(EkfHeightFusionTest, gpsRefNoAltFusion)
+{
+	// GIVEN: GNSS alt reference but not selected as an aiding source
+	_ekf_wrapper.setGpsHeightRef();
+	_ekf_wrapper.enableBaroHeightFusion();
+	_ekf_wrapper.disableGpsHeightFusion();
+	_ekf_wrapper.enableRangeHeightFusion();
+	_sensor_simulator.runSeconds(1);
+
+	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); // Fallback to baro as GNSS alt is disabled
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeightFusion());
+
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
+
+	// THEN: the altitude estimate is initialised using GNSS altitude
+	EXPECT_NEAR(_ekf->getLatLonAlt().altitude(), _sensor_simulator._gps.getData().alt, 1.f);
+	// We cannot check the value of the bias estimate as the status is only updatad when the bias estimator is
+	// active. Since the estimator had a baro fallback, the baro bias estimate is not actively updated.
+	// EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, _sensor_simulator._baro.getData() - _sensor_simulator._gps.getData().alt, 0.2f);
+}
+
 TEST_F(EkfHeightFusionTest, baroRefFailOver)
 {
 	// GIVEN: baro reference with GPS and range height fusion