fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 20:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: height aid source consistency (#20405)

Browse Source 
- working towards keeping all height source (baro/ev/gnss/rng) handling as consistent as possible, possibly refactoring these out into separate classes later
This commit is contained in:
Daniel Agar
2022-10-19 15:35:07 -04:00
committed by GitHub
parent 870229ef49
commit f9509b442c
21 changed files with 890 additions and 1083 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+16 -1
View File
@@ -129,8 +129,23 @@ void Ekf::controlFusionModes()
// Run the kinematic consistency check when not moving horizontally
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
&& (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float var = sq(_params.range_noise) + dist_dependant_var;
_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
_rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _imu_sample_delayed.time_us);
_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _imu_sample_delayed.time_us);
}
} else {
// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_control_status.flags.in_air
&& _range_sensor.isRegularlySendingData()
&& _range_sensor.isDataReady()) {
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setValidity(true); // bypass the checks
}
}