From 20c3db69e49d0e9847a7ccd8e9a70a69ea619955 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 19 Mar 2020 11:50:52 +0100 Subject: [PATCH] Terrain Est: Add sensor aiding bitmask This adds the ability to select range finder and/or optical flow fusion for terrain estimate. --- EKF/common.h | 7 +++++++ EKF/terrain_estimator.cpp | 7 ++++--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index a95ae7ddaf..a83b0f33a0 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -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 diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index f1e89fe23b..976543f431 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -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) {