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

AP_NavEKF: Allow 10 seconds for the filter to settle after initialisation 

Filter is declared unhealthy and will not arm for first 10 seconds after initialisation
This commit is contained in:
priseborough 2014-12-22 08:10:30 +11:00 committed by Randy Mackay
parent 0a5de21dc7
commit a63af34d8f
2 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -441,6 +441,10 @@ bool NavEKF::healthy(void) const
// extremely unhealthy. // extremely unhealthy.
return false; return false;
} }
// Give the filter 10 seconds to settle before use
if ((imuSampleTime_ms - ekfStartTime_ms) < 10000) {
return false;
}
// all OK // all OK
return true; return true;
@ -684,9 +688,9 @@ void NavEKF::UpdateFilter()
// check if on ground // check if on ground
SetFlightAndFusionModes(); SetFlightAndFusionModes();
// determine vehicle arm status // determine vehicle arm status and don't allow filter to arm until it has been running for long enough to stabilise
prevVehicleArmed = vehicleArmed; prevVehicleArmed = vehicleArmed;
vehicleArmed = getVehicleArmStatus(); vehicleArmed = (getVehicleArmStatus() && (imuSampleTime_ms - ekfStartTime_ms) > 10000);
// check to see if arm status has changed and reset states if it has // check to see if arm status has changed and reset states if it has
if (vehicleArmed != prevVehicleArmed) { if (vehicleArmed != prevVehicleArmed) {
@ -4353,6 +4357,7 @@ void NavEKF::ZeroVariables()
flowMeaTime_ms = imuSampleTime_ms; flowMeaTime_ms = imuSampleTime_ms;
prevFlowFusionTime_ms = imuSampleTime_ms; prevFlowFusionTime_ms = imuSampleTime_ms;
rngMeaTime_ms = imuSampleTime_ms; rngMeaTime_ms = imuSampleTime_ms;
ekfStartTime_ms = imuSampleTime_ms;
gpsNoiseScaler = 1.0f; gpsNoiseScaler = 1.0f;
velTimeout = true; velTimeout = true;

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -541,6 +541,7 @@ private:
uint32_t timeAtLastAuxEKF_ms; // last time the auxilliary filter was run to fuse range or optical flow measurements uint32_t timeAtLastAuxEKF_ms; // last time the auxilliary filter was run to fuse range or optical flow measurements
uint32_t secondLastFixTime_ms; // time of second last GPS fix used to determine how long since last update uint32_t secondLastFixTime_ms; // time of second last GPS fix used to determine how long since last update
uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator
Vector3f lastAccel1; // acceleration from previous IMU1 sample used for trapezoidal integrator Vector3f lastAccel1; // acceleration from previous IMU1 sample used for trapezoidal integrator
Vector3f lastAccel2; // acceleration from previous IMU2 sample used for trapezoidal integrator Vector3f lastAccel2; // acceleration from previous IMU2 sample used for trapezoidal integrator