Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2023-08-12 12:49:55 +10:00 committed by Randy Mackay
parent f269f2277c
commit 9fc8078df4
1 changed files with 23 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -705,7 +705,8 @@ void NavEKF3_core::checkGyroCalStatus(void)
void NavEKF3_core::updateFilterStatus(void) void NavEKF3_core::updateFilterStatus(void)
{ {
// init return value // 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 doingBodyVelNav = (PV_AidingMode != AID_NONE) && (imuSampleTime_ms - prevBodyVelFuseTime_ms < 5000);
bool doingFlowNav = (PV_AidingMode != AID_NONE) && flowDataValid; bool doingFlowNav = (PV_AidingMode != AID_NONE) && flowDataValid;
bool doingWindRelNav = (!tasTimeout && assume_zero_sideslip()) || !dragTimeout; bool doingWindRelNav = (!tasTimeout && assume_zero_sideslip()) || !dragTimeout;
@ -718,28 +719,30 @@ void NavEKF3_core::updateFilterStatus(void)
bool hgtNotAccurate = (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && !validOrigin; bool hgtNotAccurate = (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && !validOrigin;
// set individual flags // set individual flags
filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) status.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid status.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid
filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid status.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 status.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 status.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid status.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode status.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 status.flags.pred_horiz_pos_rel = status.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 status.flags.pred_horiz_pos_abs = status.flags.horiz_pos_abs; // EKF3 enters the required mode before flight
filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected status.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 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
filterStatus.flags.touchdown = dal.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode status.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); status.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 status.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.gps_quality_good = gpsGoodToAlign;
// for reporting purposes we report rejecting airspeed after 3s of not fusing when we want to fuse the data // 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 - lastTasFailTime_ms) < 1000 &&
(imuSampleTime_ms - lastTasPassTime_ms) > 3000; (imuSampleTime_ms - lastTasPassTime_ms) > 3000;
filterStatus.flags.initalized = filterStatus.flags.initalized || healthy(); status.flags.initalized = status.flags.initalized || healthy();
filterStatus.flags.dead_reckoning = (PV_AidingMode != AID_NONE) && doingWindRelNav && !((doingFlowNav && gndOffsetValid) || doingNormalGpsNav || doingBodyVelNav); status.flags.dead_reckoning = (PV_AidingMode != AID_NONE) && doingWindRelNav && !((doingFlowNav && gndOffsetValid) || doingNormalGpsNav || doingBodyVelNav);
filterStatus.value = status.value;
} }
void NavEKF3_core::runYawEstimatorPrediction() void NavEKF3_core::runYawEstimatorPrediction()