forked from Archive/PX4-Autopilot
Added height above ground source bitmask indicating which sensor is used
This commit is contained in:
parent
62e15cbacf
commit
b3dc06d0cb
|
@ -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;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue