fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 17:29:07 +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 16:32:24 +01:00 committed by Daniel Agar
parent 9521e81b09
commit cd38621dd6
3 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/ekf.cpp
View File

@ -223,6 +223,7 @@ bool Ekf::initialiseFilter()
_time_last_delpos_fuse = _time_last_imu;
_time_last_hor_vel_fuse = _time_last_imu;
_time_last_hagl_fuse = _time_last_imu;
_time_last_flow_terrain_fuse = _time_last_imu;
_time_last_of_fuse = _time_last_imu;
// reset the output predictor state history to match the EKF initial values

1
EKF/ekf.h
View File

@ -351,6 +351,7 @@ private:
uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec)
uint64_t _time_last_delpos_fuse{0}; ///< time the last fusion of incremental horizontal position measurements was performed (uSec)
uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec)
uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec)
uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec)
uint64_t _time_last_fake_pos{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)

7
EKF/terrain_estimator.cpp
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));