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

ekf2: add kconfig option to enable/disable range finder fusion (and terrain estimator)

Browse Source
This commit is contained in:
Daniel Agar
2023-03-20 12:41:42 -04:00
parent 0784901a66
commit 32a5bd32ad
17 changed files with 343 additions and 173 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+10 -5
View File
@@ -63,9 +63,11 @@ void Ekf::reset()
_state.wind_vel.setZero();
_state.quat_nominal.setIdentity();
#if defined(CONFIG_EKF2_RANGE_FINDER)
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
#endif // CONFIG_EKF2_RANGE_FINDER
_control_status.value = 0;
_control_status_prev.value = 0;
@@ -94,17 +96,13 @@ void Ekf::reset()
_time_last_hor_vel_fuse = 0;
_time_last_ver_vel_fuse = 0;
_time_last_heading_fuse = 0;
_time_last_flow_terrain_fuse = 0;
_time_last_zero_velocity_fuse = 0;
_time_last_healthy_rng_data = 0;
_last_known_pos.setZero();
_time_acc_bias_check = 0;
_gps_checks_passed = false;
_gps_alt_ref = NAN;
_baro_counter = 0;
@@ -115,7 +113,6 @@ void Ekf::reset()
_clip_counter = 0;
resetEstimatorAidStatus(_aid_src_baro_hgt);
resetEstimatorAidStatus(_aid_src_rng_hgt);
#if defined(CONFIG_EKF2_AIRSPEED)
resetEstimatorAidStatus(_aid_src_airspeed);
#endif // CONFIG_EKF2_AIRSPEED
@@ -152,6 +149,10 @@ void Ekf::reset()
resetEstimatorAidStatus(_aid_src_optical_flow);
resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_RANGE_FINDER)
resetEstimatorAidStatus(_aid_src_rng_hgt);
#endif // CONFIG_EKF2_RANGE_FINDER
}
bool Ekf::update()
@@ -179,8 +180,10 @@ bool Ekf::update()
// control fusion of observation data
controlFusionModes(imu_sample_delayed);
#if defined(CONFIG_EKF2_RANGE_FINDER)
// run a separate filter for terrain estimation
runTerrainEstimator(imu_sample_delayed);
#endif // CONFIG_EKF2_RANGE_FINDER
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(),
_state.quat_nominal, _state.vel, _state.pos);
@@ -218,8 +221,10 @@ bool Ekf::initialiseFilter()
// initialise the state covariance matrix now we have starting values for all the states
initialiseCovariance();
#if defined(CONFIG_EKF2_RANGE_FINDER)
// Initialise the terrain estimator
initHagl();
#endif // CONFIG_EKF2_RANGE_FINDER
// reset the output predictor state history to match the EKF initial values
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);