fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: set baro bias when GNSS is alt ref

Browse Source 
Do this even when GNSS altitude fusion is disabled.
This commit is contained in:
bresch 2024-12-11 14:51:54 +01:00 committed by Mathieu Bresciani
parent fad9ae855d
commit f9140fcd50
3 changed files with 47 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
View File

@ -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();
}
}

2
src/modules/ekf2/module.yaml
View File

@ -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

26
src/modules/ekf2/test/test_EKF_height_fusion.cpp
View File

@ -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