From 5b72cb761003a0292295983d5dac99450a7756db Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 6 Sep 2014 06:33:47 +1000 Subject: [PATCH] AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 101 +++++++++++++++--------------- libraries/AP_NavEKF/AP_NavEKF.h | 2 +- 2 files changed, 52 insertions(+), 51 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 88c56e3645..e89c0dd054 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -369,7 +369,7 @@ bool NavEKF::healthy(void) const if (state.velocity.is_nan()) { return false; } - if (filterDiverged || (hal.scheduler->millis() - lastDivergeTime_ms < 10000)) { + if (filterDiverged || (imuSampleTime_ms - lastDivergeTime_ms < 10000)) { return false; } // If measurements have failed innovation consistency checks for long enough to time-out @@ -680,12 +680,12 @@ void NavEKF::SelectVelPosFusion() // If a long time since last GPS update, then reset position and velocity and reset stored state history uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS; - if (hal.scheduler->millis() - secondLastFixTime_ms > gpsRetryTimeout) { + if (imuSampleTime_ms - secondLastFixTime_ms > gpsRetryTimeout) { ResetPosition(); ResetVelocity(); StoreStatesReset(); } - } else if (hal.scheduler->millis() - lastFixTime_ms > (uint32_t)(_msecGpsAvg + 40)) { + } else if (imuSampleTime_ms - lastFixTime_ms > (uint32_t)(_msecGpsAvg + 40)) { // Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same // measurement until the next one arrives to provide a smoother output fuseVelData = false; @@ -699,7 +699,7 @@ void NavEKF::SelectVelPosFusion() newDataHgt = false; // enable fusion fuseHgtData = true; - } else if (hal.scheduler->millis() - lastHgtTime_ms > (uint32_t)(_msecHgtAvg + 40)) { + } else if (imuSampleTime_ms - lastHgtTime_ms > (uint32_t)(_msecHgtAvg + 40)) { // timeout fusion of height data if stale. Needed because we repeatedly fuse the same // measurement until the next one arrives to provide a smoother output fuseHgtData = false; @@ -746,9 +746,9 @@ void NavEKF::SelectMagFusion() // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout // If we have a vehicle that can fly without a compass (a vehicle that doesn't have significant sideslip) then the compass is permanently failed and will not be used until the filter is reset if (magHealth) { - lastHealthyMagTime_ms = hal.scheduler->millis(); + lastHealthyMagTime_ms = imuSampleTime_ms; } else { - if ((hal.scheduler->millis() - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) { + if ((imuSampleTime_ms - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) { magTimeout = true; if (assume_zero_sideslip()) { magFailed = true; @@ -762,7 +762,7 @@ void NavEKF::SelectMagFusion() bool dataReady = statesInitialised && use_compass() && newDataMag; if (dataReady) { - MAGmsecPrev = IMUmsec; + MAGmsecPrev = imuSampleTime_ms; fuseMagData = true; } else @@ -785,7 +785,7 @@ void NavEKF::SelectTasFusion() tasDataWaiting = (statesInitialised && !inhibitWindStates && (tasDataWaiting || newDataTas)); // if we have waited too long, set a timeout flag which will force fusion to occur - bool timeout = ((IMUmsec - TASmsecPrev) >= TASmsecMax); + bool timeout = ((imuSampleTime_ms - TASmsecPrev) >= TASmsecMax); // we don't fuse airspeed measurements if magnetometer fusion has been performed in the same frame, unless timed out or the fuseMeNow option is selected // this helps to spreasthe load associated with fusion of different measurements across multiple frames @@ -793,7 +793,7 @@ void NavEKF::SelectTasFusion() if (tasDataWaiting && (!magFusePerformed || timeout || fuseMeNow)) { FuseAirspeed(); - TASmsecPrev = IMUmsec; + TASmsecPrev = imuSampleTime_ms; tasDataWaiting = false; } } @@ -808,9 +808,9 @@ void NavEKF::SelectBetaFusion() // we are a fly forward vehicle type AND NOT using a full range of sensors with healthy position // AND NOT on the ground AND enough time has lapsed since our last fusion // AND (we have not fused magnetometer data on this time step OR the immediate fusion flag is set) - if (assume_zero_sideslip() && !(use_compass() && useAirspeed() && posHealth) && !inhibitWindStates && ((IMUmsec - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) { + if (assume_zero_sideslip() && !(use_compass() && useAirspeed() && posHealth) && !inhibitWindStates && ((imuSampleTime_ms - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) { FuseSideslip(); - BETAmsecPrev = IMUmsec; + BETAmsecPrev = imuSampleTime_ms; } } @@ -1679,15 +1679,15 @@ void NavEKF::FuseVelPosNED() // calculate max valid position innovation squared based on a maximum horizontal inertial nav accel error and GPS noise parameter // max inertial nav error is scaled with horizontal g to allow for increased errors when manoeuvring float accelScale = (1.0f + 0.1f * accNavMagHoriz); - float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(hal.scheduler->millis() - posFailTime))); + float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - posFailTime))); posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2; posHealth = ((posTestRatio < 1.0f) || badIMUdata); // declare a timeout condition if we have been too long without data - posTimeout = ((hal.scheduler->millis() - posFailTime) > gpsRetryTime); + posTimeout = ((imuSampleTime_ms - posFailTime) > gpsRetryTime); // use position data if healthy, timed out, or in static mode if (posHealth || posTimeout || staticMode) { posHealth = true; - posFailTime = hal.scheduler->millis(); + posFailTime = imuSampleTime_ms; // if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps if (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) { gpsPosGlitchOffsetNE.x += posInnov[0]; @@ -1748,11 +1748,11 @@ void NavEKF::FuseVelPosNED() // fail if the ratio is greater than 1 velHealth = ((velTestRatio < 1.0f) || badIMUdata); // declare a timeout if we have not fused velocity data for too long - velTimeout = (hal.scheduler->millis() - velFailTime) > gpsRetryTime; + velTimeout = (imuSampleTime_ms - velFailTime) > gpsRetryTime; // if data is healthy or in static mode we fuse it if (velHealth || staticMode) { velHealth = true; - velFailTime = hal.scheduler->millis(); + velFailTime = imuSampleTime_ms; } else if (velTimeout && !posHealth) { // if data is not healthy and timed out and position is unhealthy we reset the velocity, but do not fuse data on this time step ResetVelocity(); @@ -1777,11 +1777,11 @@ void NavEKF::FuseVelPosNED() hgtTestRatio = sq(hgtInnov) / (sq(_hgtInnovGate) * varInnovVelPos[5]); // fail if the ratio is > 1, but don't fail if bad IMU data hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); - hgtTimeout = (hal.scheduler->millis() - hgtFailTime) > hgtRetryTime; + hgtTimeout = (imuSampleTime_ms - hgtFailTime) > hgtRetryTime; // Fuse height data if healthy or timed out or in static mode if (hgtHealth || hgtTimeout || staticMode) { hgtHealth = true; - hgtFailTime = hal.scheduler->millis(); + hgtFailTime = imuSampleTime_ms; // if timed out, reset the height, but do not fuse data on this time step if (hgtTimeout) { ResetHeight(); @@ -2681,8 +2681,8 @@ void NavEKF::zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last) void NavEKF::StoreStates() { // Don't need to store states more often than every 10 msec - if (hal.scheduler->millis() - lastStateStoreTime_ms >= 10) { - lastStateStoreTime_ms = hal.scheduler->millis(); + if (imuSampleTime_ms - lastStateStoreTime_ms >= 10) { + lastStateStoreTime_ms = imuSampleTime_ms; if (storeIndex > 49) { storeIndex = 0; } @@ -2701,7 +2701,7 @@ void NavEKF::StoreStatesReset() // store current state vector in first column storeIndex = 0; storedStates[storeIndex] = state; - statetimeStamp[storeIndex] = hal.scheduler->millis(); + statetimeStamp[storeIndex] = imuSampleTime_ms; storeIndex = storeIndex + 1; } @@ -2925,7 +2925,7 @@ void NavEKF::ForceSymmetry() // set the filter status as diverged and re-initialise the filter filterDiverged = true; faultStatus.diverged = true; - lastDivergeTime_ms = hal.scheduler->millis(); + lastDivergeTime_ms = imuSampleTime_ms; InitialiseFilterDynamic(); return; } @@ -2996,8 +2996,8 @@ void NavEKF::readIMUData() Vector3f accel1; // acceleration vector in XYZ body axes measured by IMU1 (m/s^2) Vector3f accel2; // acceleration vector in XYZ body axes measured by IMU2 (m/s^2) - // get the time the IMU data was read - IMUmsec = hal.scheduler->millis(); + // the imu sample time is sued as a common time reference throughout the filter + imuSampleTime_ms = hal.scheduler->millis(); // limit IMU delta time to prevent numerical problems elsewhere dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(), 0.001f, 1.0f); @@ -3051,8 +3051,8 @@ void NavEKF::readGpsData() // get state vectors that were stored at the time that is closest to when the the GPS measurement // time after accounting for measurement delays - RecallStates(statesAtVelTime, (IMUmsec - constrain_int16(_msecVelDelay, 0, 500))); - RecallStates(statesAtPosTime, (IMUmsec - constrain_int16(_msecPosDelay, 0, 500))); + RecallStates(statesAtVelTime, (imuSampleTime_ms - constrain_int16(_msecVelDelay, 0, 500))); + RecallStates(statesAtPosTime, (imuSampleTime_ms - constrain_int16(_msecPosDelay, 0, 500))); // read the NED velocity from the GPS velNED = _ahrs->get_gps().velocity(); @@ -3082,14 +3082,14 @@ void NavEKF::readHgtData() lastHgtMeasTime = _baro.get_last_update(); // time stamp used to check for timeout - lastHgtTime_ms = hal.scheduler->millis(); + lastHgtTime_ms = imuSampleTime_ms; // get measurement and set flag to let other functions know new data has arrived hgtMea = _baro.get_altitude(); newDataHgt = true; // get states that wer stored at the time closest to the measurement time, taking measurement delay into account - RecallStates(statesAtHgtTime, (IMUmsec - _msecHgtDelay)); + RecallStates(statesAtHgtTime, (imuSampleTime_ms - _msecHgtDelay)); } else { newDataHgt = false; } @@ -3109,7 +3109,7 @@ void NavEKF::readMagData() magData = _ahrs->get_compass()->get_field() * 0.001f + magBias; // get states stored at time closest to measurement time after allowance for measurement delay - RecallStates(statesAtMagMeasTime, (IMUmsec - _msecMagDelay)); + RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - _msecMagDelay)); // let other processes know that new compass data has arrived newDataMag = true; @@ -3131,7 +3131,7 @@ void NavEKF::readAirSpdData() VtasMeas = aspeed->get_airspeed() * aspeed->get_EAS2TAS(); lastAirspeedUpdate = aspeed->last_update_ms(); newDataTas = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _msecTasDelay)); + RecallStates(statesAtVtasMeasTime, (imuSampleTime_ms - _msecTasDelay)); } else { newDataTas = false; } @@ -3311,29 +3311,30 @@ void NavEKF::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f // zero stored variables - this needs to be called before a full filter initialisation void NavEKF::ZeroVariables() { + // initialise time stamps + imuSampleTime_ms = hal.scheduler->millis(); + lastHealthyMagTime_ms = imuSampleTime_ms; + lastDivergeTime_ms = imuSampleTime_ms; + TASmsecPrev = imuSampleTime_ms; + BETAmsecPrev = imuSampleTime_ms; + lastMagUpdate = imuSampleTime_ms; + lastHgtMeasTime = imuSampleTime_ms; + lastHgtTime_ms = imuSampleTime_ms; + velFailTime = imuSampleTime_ms; + posFailTime = imuSampleTime_ms; + hgtFailTime = imuSampleTime_ms; + lastStateStoreTime_ms = imuSampleTime_ms; + lastFixTime_ms = imuSampleTime_ms; + secondLastFixTime_ms = imuSampleTime_ms; + lastDecayTime_ms = imuSampleTime_ms; + velTimeout = false; posTimeout = false; hgtTimeout = false; filterDiverged = false; magTimeout = false; magFailed = false; - lastHealthyMagTime_ms = hal.scheduler->millis(); - lastStateStoreTime_ms = 0; - lastFixTime_ms = 0; - secondLastFixTime_ms = 0; - lastMagUpdate = 0; - lastAirspeedUpdate = 0; - velFailTime = 0; - posFailTime = 0; - hgtFailTime = 0; storeIndex = 0; - TASmsecPrev = 0; - BETAmsecPrev = 0; - MAGmsecPrev = 0; - HGTmsecPrev = 0; - lastMagUpdate = 0; - lastAirspeedUpdate = 0; - lastHgtMeasTime = 0; dtIMU = 0; dt = 0; hgtMea = 0; @@ -3385,8 +3386,8 @@ bool NavEKF::use_compass(void) const // apply glitch offset to GPS measurements void NavEKF::decayGpsOffset() { - float lapsedTime = 0.001f*float(hal.scheduler->millis() - lastDecayTime_ms); - lastDecayTime_ms = hal.scheduler->millis(); + float lapsedTime = 0.001f*float(imuSampleTime_ms - lastDecayTime_ms); + lastDecayTime_ms = imuSampleTime_ms; float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y); // decay radius if larger than velocity of 1.0 multiplied by lapsed time (plus a margin to prevent divide by zero) if (offsetRadius > (lapsedTime + 0.1f)) { @@ -3422,11 +3423,11 @@ void NavEKF::checkDivergence() } bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f); lastGyroBias = state.gyro_bias; - if (hal.scheduler->millis() - lastDivergeTime_ms > 10000) { + if (imuSampleTime_ms - lastDivergeTime_ms > 10000) { if (divergenceDetected) { filterDiverged = true; faultStatus.diverged = true; - lastDivergeTime_ms = hal.scheduler->millis(); + lastDivergeTime_ms = imuSampleTime_ms; } else { filterDiverged = false; } diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 7f3252da31..e628f490ec 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -431,7 +431,7 @@ private: Vector3f velDotNED; // rate of change of velocity in NED frame Vector3f velDotNEDfilt; // low pass filtered velDotNED uint32_t lastAirspeedUpdate; // last time airspeed was updated - uint32_t IMUmsec; // time that the last IMU value was taken + uint32_t imuSampleTime_ms; // time that the last IMU value was taken ftype gpsCourse; // GPS ground course angle(rad) ftype gpsGndSpd; // GPS ground speed (m/s) bool newDataGps; // true when new GPS data has arrived