Added height above ground source bitmask indicating which sensor is used

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;
+};
+
 }

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

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;
 

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