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

fixed method which calculates validity of terrain estimate

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2019-05-24 12:17:21 -04:00
committed by Paul Riseborough
parent cd34ab802d
commit 0f49eb34a0
1 changed files with 11 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+11 -1
View File
@@ -242,6 +242,7 @@ void Ekf::fuseFlowForTerrain()
_terrain_vpos += Kx * _flow_innov[0]; _terrain_vpos += Kx * _flow_innov[0];
// guard against negative variance // guard against negative variance
_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f); _terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
_time_last_of_fuse = _time_last_imu;
} }
// Calculate observation matrix for flow around the vehicle y axis // Calculate observation matrix for flow around the vehicle y axis
@@ -269,6 +270,7 @@ void Ekf::fuseFlowForTerrain()
_terrain_vpos += Ky * _flow_innov[1]; _terrain_vpos += Ky * _flow_innov[1];
// guard against negative variance // guard against negative variance
_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f); _terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
_time_last_of_fuse = _time_last_imu;
} }
} }
@@ -281,7 +283,15 @@ bool Ekf::get_terrain_valid()
// determine terrain validity // determine terrain validity
void Ekf::update_terrain_valid() void Ekf::update_terrain_valid()
{ {
if (_terrain_initialised && ((_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e6)) { // we have been fusing range finder measurements in the last 5 seconds
bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < 5*1000*1000;
// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
// this can only be the case if the main filter does not fuse optical flow
bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < 5*1000*1000) && !_control_status.flags.opt_flow;
if (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion)) {
_hagl_valid = true; _hagl_valid = true;