fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-14 14:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

terrain_estimator: fix sensor aid selection

Browse Source
This commit is contained in:
bresch
2020-06-03 11:22:47 +02:00
committed by Mathieu Bresciani
parent 821e1fa8fc
commit 8f533cb878
2 changed files with 24 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.h
+3
View File
@@ -625,6 +625,9 @@ private:
// return true if the initialisation is successful
bool initHagl();
bool shouldUseRangeFinderForHagl() const;
bool shouldUseOpticalFlowForHagl() const;
// run the terrain estimator
void runTerrainEstimator();
EKF/terrain_estimator.cpp
+21 -6
View File
@@ -55,7 +55,7 @@ bool Ekf::initHagl()
_time_last_fake_hagl_fuse = _time_last_imu;
initialized = true;
} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
} else if (shouldUseRangeFinderForHagl()
&& _range_sensor.isDataHealthy()) {
// if we have a fresh measurement, use it to initialise the terrain estimator
_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
@@ -64,7 +64,7 @@ bool Ekf::initHagl()
// success
initialized = true;
} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)
} else if (shouldUseOpticalFlowForHagl()
&& _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));
@@ -83,6 +83,16 @@ bool Ekf::initHagl()
return initialized;
}
bool Ekf::shouldUseRangeFinderForHagl() const
{
return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder);
}
bool Ekf::shouldUseOpticalFlowForHagl() const
{
return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow);
}
void Ekf::runTerrainEstimator()
{
// If we are on ground, store the local position and time to use as a reference
@@ -110,11 +120,13 @@ void Ekf::runTerrainEstimator()
_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
// Fuse range finder data if available
if (_range_sensor.isDataHealthy()) {
if (shouldUseRangeFinderForHagl()
&& _range_sensor.isDataHealthy()) {
fuseHagl();
}
if (_flow_for_terrain_data_ready) {
if (shouldUseOpticalFlowForHagl()
&& _flow_for_terrain_data_ready) {
fuseFlowForTerrain();
_flow_for_terrain_data_ready = false;
}
@@ -291,8 +303,11 @@ void Ekf::updateTerrainValidity()
_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;
_hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl()
&& recent_range_fusion
&& (_time_last_fake_hagl_fuse != _time_last_hagl_fuse);
_hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl()
&& recent_flow_for_terrain_fusion;
}
// get the estimated vertical position of the terrain relative to the NED origin