fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 02:47:35 +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 // return true if the initialisation is successful
bool initHagl(); bool initHagl();
bool shouldUseRangeFinderForHagl() const;
bool shouldUseOpticalFlowForHagl() const;
// run the terrain estimator // run the terrain estimator
void runTerrainEstimator(); 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; _time_last_fake_hagl_fuse = _time_last_imu;
initialized = true; initialized = true;
} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder) } else if (shouldUseRangeFinderForHagl()
&& _range_sensor.isDataHealthy()) { && _range_sensor.isDataHealthy()) {
// if we have a fresh measurement, use it to initialise the terrain estimator // if we have a fresh measurement, use it to initialise the terrain estimator
_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom(); _terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
@@ -64,7 +64,7 @@ bool Ekf::initHagl()
// success // success
initialized = true; initialized = true;
} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow) } else if (shouldUseOpticalFlowForHagl()
&& _flow_for_terrain_data_ready) { && _flow_for_terrain_data_ready) {
// initialise terrain vertical position to origin as this is the best guess we have // initialise terrain vertical position to origin as this is the best guess we have
_terrain_vpos = fmaxf(0.0f, _state.pos(2)); _terrain_vpos = fmaxf(0.0f, _state.pos(2));
@@ -83,6 +83,16 @@ bool Ekf::initHagl()
return initialized; 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() void Ekf::runTerrainEstimator()
{ {
// If we are on ground, store the local position and time to use as a reference // 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); _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
// Fuse range finder data if available // Fuse range finder data if available
if (_range_sensor.isDataHealthy()) { if (shouldUseRangeFinderForHagl()
&& _range_sensor.isDataHealthy()) {
fuseHagl(); fuseHagl();
} }
if (_flow_for_terrain_data_ready) { if (shouldUseOpticalFlowForHagl()
&& _flow_for_terrain_data_ready) {
fuseFlowForTerrain(); fuseFlowForTerrain();
_flow_for_terrain_data_ready = false; _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_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.range_finder = shouldUseRangeFinderForHagl()
_hagl_sensor_status.flags.flow = recent_flow_for_terrain_fusion; && 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 // get the estimated vertical position of the terrain relative to the NED origin