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

AP_NavEKF3: Make bad IMU status more persistent

This commit is contained in:
Paul Riseborough 2021-07-16 16:26:03 +10:00 committed by Andrew Tridgell
parent 8f1b98a0cb
commit e3bdbcd8ea
3 changed files with 17 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -689,9 +689,15 @@ void NavEKF3_core::FuseVelPosNED()
const ftype velDErr = stateStruct.velocity.z - velPosObs[2];
// check if they are the same sign and both more than 3-sigma out of bounds
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[9][9] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS_DATA_CHECKS[2]))) {
badIMUdata_ms = imuSampleTime_ms;
} else {
goodIMUdata_ms = imuSampleTime_ms;
}
if (imuSampleTime_ms - badIMUdata_ms < 10000) {
badIMUdata = true;
} else {
badIMUdata = false;
}
}
@ -1217,7 +1223,8 @@ void NavEKF3_core::selectHeightForFusion()
// If we haven't fused height data for a while, then declare the height data as being timed out
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
hgtRetryTime_ms = ((useGpsVertVel || useExtNavVel) && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) {
if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms ||
(badIMUdata && (imuSampleTime_ms - goodIMUdata_ms < BAD_IMU_DATA_TIMEOUT_MS))) {
hgtTimeout = true;
} else {
hgtTimeout = false;

4
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -233,6 +233,8 @@ void NavEKF3_core::InitialiseVariables()
hgtTimeout = true;
tasTimeout = true;
badIMUdata = false;
badIMUdata_ms = 0;
goodIMUdata_ms = 0;
vertVelVarClipCounter = 0;
finalInflightYawInit = false;
dtIMUavg = ins.get_loop_delta_t();
@ -1135,7 +1137,7 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
ftype _gyrNoise = constrain_ftype(frontend->_gyrNoise, 0.0f, 1.0f);
daxVar = dayVar = dazVar = sq(dt*_gyrNoise);
}
ftype _accNoise = badIMUdata ? BAD_IMU_DATA_ACC_P_NSE : constrain_ftype(frontend->_accNoise, 0.0f, 10.0f);
ftype _accNoise = badIMUdata ? BAD_IMU_DATA_ACC_P_NSE : constrain_ftype(frontend->_accNoise, 0.0f, BAD_IMU_DATA_ACC_P_NSE);
dvxVar = dvyVar = dvzVar = sq(dt*_accNoise);
if (!inhibitDelVelBiasStates) {

6
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -96,8 +96,12 @@
#define EK3_POSXY_STATE_LIMIT 1.0e6
#endif
// IMU acceleration process noise in m/s/s used when bad vibration affected IMU accel is detected
#define BAD_IMU_DATA_ACC_P_NSE 5.0f
// Number of milliseconds of bad IMU data before a reset to vertical position and velocity height sources is performed
#define BAD_IMU_DATA_TIMEOUT_MS 1000
class NavEKF3_core : public NavEKF_core_common
{
public:
@ -960,6 +964,8 @@ private:
bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out
bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out
bool badIMUdata; // boolean true if the bad IMU data is detected
uint32_t badIMUdata_ms; // time stamp bad IMU data was last detected
uint32_t goodIMUdata_ms; // time stamp good IMU data was last detected
uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit
ftype gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts