mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 22:23:33 +08:00
Terrain Est: Add sensor aiding bitmask
This adds the ability to select range finder and/or optical flow fusion for terrain estimate.
This commit is contained in:
committed by
Mathieu Bresciani
parent
f38388cdab
commit
20c3db69e4
@@ -56,7 +56,8 @@ bool Ekf::initHagl()
|
||||
_terrain_var = sq(_params.rng_gnd_clearance);
|
||||
initialized = true;
|
||||
|
||||
} else if (_rng_hgt_valid
|
||||
} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
|
||||
&& _rng_hgt_valid
|
||||
&& isRecent(latest_measurement.time_us, (uint64_t)2e5)
|
||||
&& _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
|
||||
// if we have a fresh measurement, use it to initialise the terrain estimator
|
||||
@@ -66,7 +67,8 @@ bool Ekf::initHagl()
|
||||
// success
|
||||
initialized = true;
|
||||
|
||||
} else if (_flow_for_terrain_data_ready) {
|
||||
} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)
|
||||
&& _flow_for_terrain_data_ready) {
|
||||
// initialise terrain vertical position to origin as this is the best guess we have
|
||||
_terrain_vpos = fmaxf(0.0f, _state.pos(2));
|
||||
_terrain_var = 100.0f;
|
||||
@@ -74,7 +76,6 @@ bool Ekf::initHagl()
|
||||
|
||||
} else {
|
||||
// no information - cannot initialise
|
||||
initialized = false;
|
||||
}
|
||||
|
||||
if (initialized) {
|
||||
|
||||
Reference in New Issue
Block a user