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:
bresch
2020-03-19 11:50:52 +01:00
committed by Mathieu Bresciani
parent f38388cdab
commit 20c3db69e4
2 changed files with 11 additions and 3 deletions
+7
View File
@@ -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
+4 -3
View File
@@ -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) {