diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index b2c7ced31c..6ca20cb017 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -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 diff --git a/EKF/ekf.h b/EKF/ekf.h index 35136f5433..d5cbab5007 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -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) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 77cc681810..380fe1ff6f 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -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));