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

ekf2: include 0 timestamp checks in helpers (isTimedOut(), isRecent(), etc)

Browse Source 
- EKF isTimedOut(), isRecent(), and isNewestSampleRecent() need to handle the case where the timestamp has never been set
 - reset() more thoroughly reset fields (mainly impacts unit tests)
This commit is contained in:
Daniel Agar
2023-02-03 10:00:51 -05:00
committed by GitHub
parent 264a99fb77
commit 66ad7fd06c
11 changed files with 834 additions and 767 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+56 -5
View File
@@ -81,6 +81,62 @@ void Ekf::reset()
resetGpsDriftCheckFilters();
_output_predictor.reset();
// Ekf private fields
_time_last_horizontal_aiding = 0;
_time_last_v_pos_aiding = 0;
_time_last_v_vel_aiding = 0;
_time_last_hor_pos_fuse = 0;
_time_last_hgt_fuse = 0;
_time_last_hor_vel_fuse = 0;
_time_last_ver_vel_fuse = 0;
_time_last_heading_fuse = 0;
_time_last_flow_terrain_fuse = 0;
_time_last_zero_velocity_fuse = 0;
_time_last_healthy_rng_data = 0;
_last_known_pos.setZero();
_time_acc_bias_check = 0;
_gps_checks_passed = false;
_gps_alt_ref = NAN;
_baro_counter = 0;
_mag_counter = 0;
_time_bad_vert_accel = 0;
_time_good_vert_accel = 0;
_clip_counter = 0;
resetEstimatorAidStatus(_aid_src_baro_hgt);
resetEstimatorAidStatus(_aid_src_rng_hgt);
resetEstimatorAidStatus(_aid_src_airspeed);
resetEstimatorAidStatus(_aid_src_sideslip);
resetEstimatorAidStatus(_aid_src_fake_pos);
resetEstimatorAidStatus(_aid_src_fake_hgt);
resetEstimatorAidStatus(_aid_src_ev_hgt);
resetEstimatorAidStatus(_aid_src_ev_pos);
resetEstimatorAidStatus(_aid_src_ev_vel);
resetEstimatorAidStatus(_aid_src_ev_yaw);
resetEstimatorAidStatus(_aid_src_gnss_hgt);
resetEstimatorAidStatus(_aid_src_gnss_pos);
resetEstimatorAidStatus(_aid_src_gnss_vel);
resetEstimatorAidStatus(_aid_src_gnss_yaw);
resetEstimatorAidStatus(_aid_src_mag_heading);
resetEstimatorAidStatus(_aid_src_mag);
resetEstimatorAidStatus(_aid_src_aux_vel);
resetEstimatorAidStatus(_aid_src_optical_flow);
resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
}
bool Ekf::update()
@@ -198,11 +254,6 @@ bool Ekf::initialiseFilter()
// Initialise the terrain estimator
initHagl();
// reset the essential fusion timeout counters
_time_last_hgt_fuse = _time_delayed_us;
_time_last_hor_pos_fuse = _time_delayed_us;
_time_last_hor_vel_fuse = _time_delayed_us;
// reset the output predictor state history to match the EKF initial values
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);