diff --git a/EKF/common.h b/EKF/common.h index d632981faf..6c0bf4a02e 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -497,4 +497,12 @@ union ekf_solution_status { uint16_t value; }; +union terrain_fusion_status_u { + struct { + bool range_finder: 1; ///< 0 - true if we are fusing range finder data + bool flow: 1; ///< 1 - true if we are fusing flow data + } flags; + uint8_t value; +}; + } diff --git a/EKF/ekf.h b/EKF/ekf.h index ddfc7fd929..ed866ce071 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -221,6 +221,8 @@ public: bool isTerrainEstimateValid() const override; + uint8_t getTerrainEstimateSensorBitfield() const override {return _hagl_sensor_status.value;} + void updateTerrainValidity(); // get the estimated terrain vertical position relative to the NED origin @@ -486,9 +488,11 @@ private: // Terrain height state estimation float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m) float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) - uint64_t _time_last_hagl_fuse{0}; ///< last system time that the hagl measurement failed it's checks (uSec) + uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator + uint64_t _time_last_fake_hagl_fuse{0}; ///< last system time that a fake range sample was fused by the terrain estimator bool _terrain_initialised{false}; ///< true when the terrain estimator has been initialized bool _hagl_valid{false}; ///< true when the height above ground estimate is valid + terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground // height sensor status bool _baro_hgt_faulty{false}; ///< true if valid baro data is unavailable for use diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 5ec9656bca..3ca6fb4efe 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -271,6 +271,8 @@ public: //[[deprecated("Replaced by isTerrainEstimateValid")]] bool get_terrain_valid() { return isTerrainEstimateValid(); } + virtual uint8_t getTerrainEstimateSensorBitfield() const = 0; + // get the estimated terrain vertical position relative to the NED origin virtual float getTerrainVertPos() const = 0; diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 3e69a28b55..e635a21271 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -52,6 +52,7 @@ bool Ekf::initHagl() _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; // use the ground clearance value as our uncertainty _terrain_var = sq(_params.rng_gnd_clearance); + _time_last_fake_hagl_fuse = _time_last_imu; initialized = true; } else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder) @@ -289,6 +290,9 @@ void Ekf::updateTerrainValidity() && !_control_status.flags.opt_flow; _hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion)); + + _hagl_sensor_status.flags.range_finder = recent_range_fusion && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse); + _hagl_sensor_status.flags.flow = recent_flow_for_terrain_fusion; } // get the estimated vertical position of the terrain relative to the NED origin