From 8bcedb228bd322d7f69dee547a38c38edb3660ed Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 24 Sep 2015 16:12:45 +1000 Subject: [PATCH] AP_NavEKF2: Bring pre-flight GPS checks up to date with EKF1 --- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 84 +++++++++++++++++++++--- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 5 ++ 2 files changed, 79 insertions(+), 10 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 2b71deb1a8..ea55ad0e91 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -3880,6 +3880,9 @@ void NavEKF2_core::readMagData() // read compass data and scale to improve numerical conditioning magDataNew.mag = _ahrs->get_compass()->get_field() * 0.001f; + // check for consistent data between magnetometers + consistentMagData = _ahrs->get_compass()->consistent(); + // check if compass offsets have been changed and adjust EKF bias states to maintain consistent innovations if (_ahrs->get_compass()->healthy(0)) { Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0); @@ -4168,6 +4171,7 @@ void NavEKF2_core::InitialiseVariables() hgtMeasTime_ms = imuSampleTime_ms; magMeasTime_ms = imuSampleTime_ms; timeTasReceived_ms = 0; + magYawResetTimer_ms = imuSampleTime_ms; // initialise other variables gpsNoiseScaler = 1.0f; @@ -4661,9 +4665,13 @@ void NavEKF2_core::setTouchdownExpected(bool val) expectGndEffectTouchdown = val; } -// Monitor GPS data to see if quality is good enough to initialise the EKF -// Monitor magnetometer innovations to to see if the heading is good enough to use GPS -// Return true if all criteria pass for 10 seconds +/* Monitor GPS data to see if quality is good enough to initialise the EKF + Monitor magnetometer innovations to to see if the heading is good enough to use GPS + Return true if all criteria pass for 10 seconds + + We also record the failure reason so that prearm_failure_reason() + can give a good report to the user on why arming is failing +*/ bool NavEKF2_core::calcGpsGoodToAlign(void) { // calculate absolute difference between GPS vert vel and inertial vert vel @@ -4673,10 +4681,21 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) } else { velDiffAbs = 0.0f; } + // fail if velocity difference or reported speed accuracy greater than threshold bool gpsVelFail = (velDiffAbs > 1.0f) || (gpsSpdAccuracy > 1.0f); - // fail if not enough sats - bool numSatsFail = _ahrs->get_gps().num_sats() < 6; + + if (velDiffAbs > 1.0f) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "GPS vert vel error %.1f", (double)velDiffAbs); + } + if (gpsSpdAccuracy > 1.0f) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "GPS speed error %.1f", (double)gpsSpdAccuracy); + } + // fail if horiziontal position accuracy not sufficient float hAcc = 0.0f; bool hAccFail; @@ -4685,6 +4704,26 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) } else { hAccFail = false; } + if (hAccFail) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "GPS horiz error %.1f", (double)hAcc); + } + + // If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states + // This enables us to handle large changes to the external magnetic field environment that occur before arming + if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f) || !consistentMagData) { + magYawResetTimer_ms = imuSampleTime_ms; + } + if (imuSampleTime_ms - magYawResetTimer_ms > 5000) { + // reset heading and field states + Vector3f eulerAngles; + getEulerAngles(eulerAngles); + stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); + // reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds + magYawResetTimer_ms = imuSampleTime_ms; + } + // fail if magnetometer innovations are outside limits indicating bad yaw // with bad yaw we are unable to use GPS bool yawFail; @@ -4693,17 +4732,42 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) } else { yawFail = false; } - // record time of fail - // assume fail first time called - if (gpsVelFail || numSatsFail || hAccFail || yawFail || lastGpsVelFail_ms == 0) { + if (yawFail) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "Mag yaw error x=%.1f y=%.1f", + (double)magTestRatio.x, + (double)magTestRatio.y); + } + + // fail if not enough sats + bool numSatsFail = _ahrs->get_gps().num_sats() < 6; + if (numSatsFail) { + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), + "GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats()); + } + + // record time of fail if failing + if (gpsVelFail || numSatsFail || hAccFail || yawFail) { + lastGpsVelFail_ms = imuSampleTime_ms; + } + + if (lastGpsVelFail_ms == 0) { + // first time through, start with a failure lastGpsVelFail_ms = imuSampleTime_ms; + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF warmup"); } + + // DEBUG PRINT + //hal.console->printf("velDiff = %5.2f, nSats = %i, hAcc = %5.2f, sAcc = %5.2f, delTime = %i\n", velDiffAbs, _ahrs->get_gps().num_sats(), hAcc, gpsSpdAccuracy, imuSampleTime_ms - lastGpsVelFail_ms); // continuous period without fail required to return healthy + if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) { + // we have not failed in the last 10 seconds return true; - } else { - return false; } + + return false; } // Read the range finder and take new measurements if available diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index e4a8ed985a..e3656f0ee1 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -699,6 +699,8 @@ private: float innovYaw; // compass yaw angle innovation (rad) uint32_t timeTasReceived_ms; // tie last TAS data was received (msec) bool gpsQualGood; // true when the GPS quality can be used to initialise the navigation system + uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks + bool consistentMagData; // true when the magnetometers are passing consistency checks // variables added for optical flow fusion of_elements storedOF[OBS_BUFFER_LENGTH]; // OF data buffer @@ -799,6 +801,9 @@ private: } mag_state; + // string representing last reason for prearm failure + char prearm_fail_string[40]; + #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN // performance counters perf_counter_t _perf_UpdateFilter;