From fb1acd20652629399847ec1a678cc3e9bc39deb2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 12 Aug 2023 12:49:55 +1000 Subject: [PATCH] AP_NavEKF3: fixed race condition in updateFilterStatus() filter status was initially set to zero then updated. This interacts with the IMU filtering code which checks filter status from a different thread to determine active_EKF_type(). When the race condition is hit then the IMU we are running notch filters on changes for a single sample, causing a notch filter glitch --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 43 +++++++++++---------- 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 07bd834c02..beca7ae17b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -687,7 +687,8 @@ void NavEKF3_core::checkGyroCalStatus(void) void NavEKF3_core::updateFilterStatus(void) { // init return value - filterStatus.value = 0; + nav_filter_status status; + status.value = 0; bool doingBodyVelNav = (PV_AidingMode != AID_NONE) && (imuSampleTime_ms - prevBodyVelFuseTime_ms < 5000); bool doingFlowNav = (PV_AidingMode != AID_NONE) && flowDataValid; bool doingWindRelNav = (!tasTimeout && assume_zero_sideslip()) || !dragTimeout; @@ -700,28 +701,30 @@ void NavEKF3_core::updateFilterStatus(void) bool hgtNotAccurate = (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && !validOrigin; // set individual flags - filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) - filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid - filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid - filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav || doingBodyVelNav) && filterHealthy; // relative horizontal position estimate valid - filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid - filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid - filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid - filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode - filterStatus.flags.pred_horiz_pos_rel = filterStatus.flags.horiz_pos_rel; // EKF3 enters the required mode before flight - filterStatus.flags.pred_horiz_pos_abs = filterStatus.flags.horiz_pos_abs; // EKF3 enters the required mode before flight - filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected - filterStatus.flags.takeoff = dal.get_takeoff_expected(); // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator - filterStatus.flags.touchdown = dal.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode - filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); - filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy - filterStatus.flags.gps_quality_good = gpsGoodToAlign; + status.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) + status.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid + status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid + status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav || doingBodyVelNav) && filterHealthy; // relative horizontal position estimate valid + status.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid + status.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid + status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid + status.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode + status.flags.pred_horiz_pos_rel = status.flags.horiz_pos_rel; // EKF3 enters the required mode before flight + status.flags.pred_horiz_pos_abs = status.flags.horiz_pos_abs; // EKF3 enters the required mode before flight + status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected + status.flags.takeoff = dal.get_takeoff_expected(); // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator + status.flags.touchdown = dal.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode + status.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); + status.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy + status.flags.gps_quality_good = gpsGoodToAlign; // for reporting purposes we report rejecting airspeed after 3s of not fusing when we want to fuse the data - filterStatus.flags.rejecting_airspeed = lastTasFailTime_ms != 0 && + status.flags.rejecting_airspeed = lastTasFailTime_ms != 0 && (imuSampleTime_ms - lastTasFailTime_ms) < 1000 && (imuSampleTime_ms - lastTasPassTime_ms) > 3000; - filterStatus.flags.initalized = filterStatus.flags.initalized || healthy(); - filterStatus.flags.dead_reckoning = (PV_AidingMode != AID_NONE) && doingWindRelNav && !((doingFlowNav && gndOffsetValid) || doingNormalGpsNav || doingBodyVelNav); + status.flags.initalized = status.flags.initalized || healthy(); + status.flags.dead_reckoning = (PV_AidingMode != AID_NONE) && doingWindRelNav && !((doingFlowNav && gndOffsetValid) || doingNormalGpsNav || doingBodyVelNav); + + filterStatus.value = status.value; } void NavEKF3_core::runYawEstimatorPrediction()