aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Bring pre-flight GPS checks up to date with EKF1

Browse Source
This commit is contained in:
Paul Riseborough 2015-09-24 16:12:45 +10:00
parent 5cb088fe14
commit 8bcedb228b
2 changed files with 82 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -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;
}
// continuous period without fail required to return healthy
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
return true;
} else {
return false;
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;
}
return false;
}
// Read the range finder and take new measurements if available

5
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -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;