diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index c81af9c581fa..9426fa266433 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/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(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(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(); } } diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index d8295fd60c25..cd815ed56f10 100644 --- a/src/modules/ekf2/module.yaml +++ b/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 diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index 595d518abb7c..3b9cd2363b7b 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/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