From 52f6eea52b3caddac29991139d1ccb3af9173d3b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 14:20:46 +1000 Subject: [PATCH] EKF: capture position and velocity innovation test failures --- EKF/vel_pos_fusion.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 8608e29afe..5cb9187259 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -201,19 +201,28 @@ void Ekf::fuseVelPosHeight() innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass; innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align; - // record the successful velocity fusion time + // record the successful velocity fusion event if (vel_check_pass && _fuse_hor_vel) { _time_last_vel_fuse = _time_last_imu; + _sensor_fault_status.flags.reject_vel_NED = false; + } else if (!vel_check_pass) { + _sensor_fault_status.flags.reject_vel_NED = true; } - // record the successful position fusion time + // record the successful position fusion event if (pos_check_pass && _fuse_pos) { _time_last_pos_fuse = _time_last_imu; + _sensor_fault_status.flags.reject_pos_NE = false; + } else if (!pos_check_pass) { + _sensor_fault_status.flags.reject_pos_NE = true; } - // record the successful height fusion time + // record the successful height fusion event if (innov_check_pass_map[5] && _fuse_height) { _time_last_hgt_fuse = _time_last_imu; + _sensor_fault_status.flags.reject_pos_D = false; + } else if (!innov_check_pass_map[5]) { + _sensor_fault_status.flags.reject_pos_D = true; } for (unsigned obs_index = 0; obs_index < 6; obs_index++) {