mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-17 08:27:34 +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
@@ -179,6 +179,11 @@ struct auxVelSample {
|
||||
#define MASK_USE_GPSYAW (1<<7) ///< set to true to use GPS yaw data if available
|
||||
#define MASK_USE_EVVEL (1<<8) ///< sset to true to use external vision velocity data
|
||||
|
||||
enum TerrainFusionMask : int32_t {
|
||||
TerrainFuseRangeFinder = (1 << 0),
|
||||
TerrainFuseOpticalFlow = (1 << 1)
|
||||
};
|
||||
|
||||
// Integer definitions for mag_fusion_type
|
||||
#define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic
|
||||
#define MAG_FUSE_TYPE_HEADING 1 ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
|
||||
@@ -204,6 +209,8 @@ struct parameters {
|
||||
// measurement source control
|
||||
int32_t fusion_mode{MASK_USE_GPS}; ///< bitmasked integer that selects which aiding sources will be used
|
||||
int32_t vdist_sensor_type{VDIST_SENSOR_BARO}; ///< selects the primary source for height data
|
||||
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
|
||||
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
|
||||
int32_t sensor_interval_min_ms{20}; ///< minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
|
||||
|
||||
// measurement time delays
|
||||
|
||||
@@ -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