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

flow terrain: use dedicated variable for last fusion time

Browse Source 
This avoids confusion with the flow "for velocity" fusion in the main
EKF
This commit is contained in:
bresch
2021-02-08 23:26:33 -05:00
committed by Daniel Agar
parent 9521e81b09
commit cd38621dd6
3 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+3 -4
View File
@@ -244,7 +244,7 @@ void Ekf::fuseFlowForTerrain()
_terrain_vpos += Kx * _flow_innov(0);
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
_time_last_of_fuse = _time_last_imu;
_time_last_flow_terrain_fuse = _time_last_imu;
}
// Calculate observation matrix for flow around the vehicle y axis
@@ -272,7 +272,7 @@ void Ekf::fuseFlowForTerrain()
_terrain_vpos += Ky * _flow_innov(1);
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
_time_last_of_fuse = _time_last_imu;
_time_last_flow_terrain_fuse = _time_last_imu;
}
}
@@ -283,8 +283,7 @@ void Ekf::updateTerrainValidity()
// 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
const bool recent_flow_for_terrain_fusion = isRecent(_time_last_of_fuse, (uint64_t)5e6)
&& !_control_status.flags.opt_flow;
const bool recent_flow_for_terrain_fusion = isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6);
_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));