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

AP_NavEKF2: Improve GPS status reporting

This commit is contained in:
Paul Riseborough 2015-10-07 17:38:26 -07:00 committed by Randy Mackay
parent d0080b66cd
commit 22920aafad
7 changed files with 137 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -753,6 +753,16 @@ void NavEKF2::getFilterStatus(nav_filter_status &status) const
}
}
/*
return filter gps quality check status
*/
void NavEKF2::getFilterGpsStatus(nav_gps_status &status) const
{
if (core) {
core->getFilterGpsStatus(status);
}
}
// send an EKF_STATUS_REPORT message to GCS
void NavEKF2::send_status_report(mavlink_channel_t chan)
{

5
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -200,6 +200,11 @@ public:
*/
void getFilterTimeouts(uint8_t &timeouts) const;
/*
return filter gps quality check status
*/
void getFilterGpsStatus(nav_gps_status &faults) const;
/*
return filter status flags
*/

6
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -386,9 +386,13 @@ void NavEKF2_core::readGpsData()
useGpsVertVel = false;
}
// Monitor quality of the GPS velocity data for alignment
// Monitor quality of the GPS velocity data before and after alignment using separate checks
if (PV_AidingMode != AID_ABSOLUTE) {
// Pre-alignment checks
gpsQualGood = calcGpsGoodToAlign();
} else {
// Post-alignment checks
calcGpsGoodForFlight();
}
// read latitutde and longitude from GPS and convert to local NE position relative to the stored origin

16
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -413,6 +413,22 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const
status.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
status.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
status.flags.using_gps = (imuSampleTime_ms - lastPosPassTime_ms) < 4000;
status.flags.gps_glitching = !gpsAccuracyGood; // The GPS is glitching
}
/*
return filter gps quality check status
*/
void NavEKF2_core::getFilterGpsStatus(nav_gps_status &faults) const
{
// init return value
faults.value = 0;
// set individual flags
faults.flags.bad_sAcc = gpsCheckStatus.bad_sAcc; // reported speed accuracy is insufficient
faults.flags.bad_hAcc = gpsCheckStatus.bad_hAcc; // reported horizontal position accuracy is insufficient
faults.flags.bad_sats = gpsCheckStatus.bad_sats; // reported number of satellites is insufficient
faults.flags.bad_yaw = gpsCheckStatus.bad_yaw; // EKF heading accuracy is too large for GPS use
}
// send an EKF_STATUS message to GCS

124
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -28,40 +28,16 @@ extern const AP_HAL::HAL& hal;
*/
bool NavEKF2_core::calcGpsGoodToAlign(void)
{
// calculate absolute difference between GPS vert vel and inertial vert vel
float velDiffAbs;
if (_ahrs->get_gps().have_vertical_velocity()) {
velDiffAbs = fabsf(gpsDataDelayed.vel.z - stateStruct.velocity.z);
} else {
velDiffAbs = 0.0f;
}
// fail if velocity difference or reported speed accuracy greater than threshold
bool gpsVelFail = (velDiffAbs > 1.0f) || (gpsSpdAccuracy > 1.0f);
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 reported speed accuracy greater than threshold
gpsCheckStatus.bad_sAcc = (gpsSpdAccuracy > 1.0f);
// fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f;
bool hAccFail;
float hAcc;
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
hAccFail = hAcc > 5.0f;
gpsCheckStatus.bad_hAcc = (hAcc > 5.0f);
} else {
hAccFail = false;
}
if (hAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS horiz error %.1f", (double)hAcc);
gpsCheckStatus.bad_hAcc = false;
}
// If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states
@ -80,42 +56,17 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
// fail if magnetometer innovations are outside limits indicating bad yaw
// with bad yaw we are unable to use GPS
bool yawFail;
if (magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) {
yawFail = true;
} else {
yawFail = false;
}
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);
}
gpsCheckStatus.bad_yaw = (magTestRatio.x > 1.0f || magTestRatio.y > 1.0f);
// 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());
}
gpsCheckStatus.bad_sats = (_ahrs->get_gps().num_sats() < 6);
// record time of fail if failing
if (gpsVelFail || numSatsFail || hAccFail || yawFail) {
if (gpsCheckStatus.bad_sAcc || gpsCheckStatus.bad_sats || gpsCheckStatus.bad_hAcc || gpsCheckStatus.bad_yaw) {
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
// We need 10 seconds of continual good data to start using GPS to reduce the chance of encountering bad data during takeoff
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
// we have not failed in the last 10 seconds
return true;
@ -124,6 +75,61 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
return false;
}
// update inflight calculaton that determines if GPS data is good enough for reliable navigation
void NavEKF2_core::calcGpsGoodForFlight(void)
{
// use a simple criteria based on the GPS receivers claimed speed accuracy and the EKF innovation consistency checks
// set up varaibles and constants used by filter that is applied to GPS speed accuracy
const float alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data
const float tau = 10.0f; // time constant (sec) of peak hold decay
if (lastGpsCheckTime_ms == 0) {
lastGpsCheckTime_ms = imuSampleTime_ms;
}
float dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
lastGpsCheckTime_ms = imuSampleTime_ms;
float alpha2 = constrain_float(dtLPF/tau,0.0f,1.0f);
// get the receivers reported speed accuracy
float gpsSpdAccRaw;
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccRaw = 0.0f;
}
// filter the raw speed accuracy using a LPF
sAccFilterState1 = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
// apply a peak hold filter to the LPF output
sAccFilterState2 = max(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2));
// Apply a threshold test with hysteresis to the filtered GPS speed accuracy data
if (sAccFilterState2 > 1.5f ) {
gpsSpdAccPass = false;
} else if(sAccFilterState2 < 1.0f) {
gpsSpdAccPass = true;
}
// Apply a threshold test with hysteresis to the normalised position and velocity innovations
// Require a fail for one second and a pass for 10 seconds to transition
if (lastInnovFailTime_ms == 0) {
lastInnovFailTime_ms = imuSampleTime_ms;
lastInnovPassTime_ms = imuSampleTime_ms;
}
if (velTestRatio < 1.0f && posTestRatio < 1.0f) {
lastInnovPassTime_ms = imuSampleTime_ms;
} else if (velTestRatio > 0.7f || posTestRatio > 0.7f) {
lastInnovFailTime_ms = imuSampleTime_ms;
}
if ((imuSampleTime_ms - lastInnovPassTime_ms) > 1000) {
ekfInnovationsPass = false;
} else if ((imuSampleTime_ms - lastInnovFailTime_ms) > 10000) {
ekfInnovationsPass = true;
}
// both GPS speed accuracy and EKF innovations must pass
gpsAccuracyGood = gpsSpdAccPass && ekfInnovationsPass;
}
// Detect if we are in flight or on ground
void NavEKF2_core::detectFlight()
{
@ -281,4 +287,4 @@ void NavEKF2_core::detectOptFlowTakeoff(void)
}
#endif // HAL_CPU_CLASS
#endif // HAL_CPU_CLASS

9
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -170,6 +170,15 @@ void NavEKF2_core::InitialiseVariables()
prevMotorsArmed = false;
innovationIncrement = 0;
lastInnovation = 0;
memset(&gpsCheckStatus, 0, sizeof(gpsCheckStatus));
gpsSpdAccPass = false;
ekfInnovationsPass = false;
sAccFilterState1 = 0.0f;
sAccFilterState2 = 0.0f;
lastGpsCheckTime_ms = 0;
lastInnovPassTime_ms = 0;
lastInnovFailTime_ms = 0;
gpsAccuracyGood = false;
}
// Initialise the states from accelerometer and magnetometer data (if present)

30
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -193,6 +193,11 @@ public:
*/
void getFilterTimeouts(uint8_t &timeouts) const;
/*
return filter gps quality check status
*/
void getFilterGpsStatus(nav_gps_status &status) const;
/*
Return a filter function status that indicates:
Which outputs are valid
@ -546,6 +551,9 @@ private:
// Assess GPS data quality and return true if good enough to align the EKF
bool calcGpsGoodToAlign(void);
// update inflight calculaton that determines if GPS data is good enough for reliable navigation
void calcGpsGoodForFlight(void);
// Read the range finder and take new measurements if available
// Apply a median filter to range finder data
void readRangeFinder();
@ -720,6 +728,16 @@ private:
bool motorsArmed; // true when the motors have been armed
bool prevMotorsArmed; // value of motorsArmed from previous frame
// variable used by the in-flight GPS quality check
bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks
bool ekfInnovationsPass; // true when GPS innovations pass in-flight checks
float sAccFilterState1; // state variable for LPF applid to reported GPS speed accuracy
float sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed
uint32_t lastGpsCheckTime_ms; // last time in msec the GPS quality was checked
uint32_t lastInnovPassTime_ms; // last time in msec the GPS innovations passed
uint32_t lastInnovFailTime_ms; // last time in msec the GPS innovations failed
bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight.
// States used for unwrapping of compass yaw error
float innovationIncrement;
float lastInnovation;
@ -791,6 +809,7 @@ private:
uint32_t touchdownExpectedSet_ms; // system time at which expectGndEffectTouchdown was set
float meaHgtAtTakeOff; // height measured at commencement of takeoff
// flags indicating severw numerical errors in innovation variance calculation for different fusion operations
struct {
bool bad_xmag:1;
bool bad_ymag:1;
@ -799,6 +818,14 @@ private:
bool bad_sideslip:1;
} faultStatus;
// flags indicating which GPS quality checks are failing
struct {
bool bad_sAcc:1;
bool bad_hAcc:1;
bool bad_sats:1;
bool bad_yaw:1;
} gpsCheckStatus;
// states held by magnetomter fusion across time steps
// magnetometer X,Y,Z measurements are fused across three time steps
// to level computational load as this is an expensive operation
@ -821,9 +848,6 @@ 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;