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

AP_NavEKF3: continuously update gpsGoodToAlign 

use it to determine how to handle a height datum reset
This commit is contained in:
Andrew Tridgell 2019-07-02 13:42:52 +10:00
parent 9372073f63
commit f3fc910abe
5 changed files with 37 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -576,13 +576,8 @@ void NavEKF3_core::readGpsData()
useGpsVertVel = false;
}
// Monitor quality of the GPS velocity data before and after alignment using separate checks
if (PV_AidingMode != AID_ABSOLUTE) {
// Pre-alignment checks
gpsGoodToAlign = calcGpsGoodToAlign();
} else {
gpsGoodToAlign = false;
}
// Monitor quality of the GPS velocity data before and after alignment
calcGpsGoodToAlign();
// Post-alignment checks
calcGpsGoodForFlight();

2
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -607,7 +607,7 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan)
// report the reason for why the backend is refusing to initialise
const char *NavEKF3_core::prearm_failure_reason(void) const
{
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
if (gpsGoodToAlign) {
// we are not failing
return nullptr;
}

18
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -212,21 +212,21 @@ bool NavEKF3_core::resetHeightDatum(void)
// reset the height state
stateStruct.position.z = 0.0f;
// adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
if (validOrigin) {
const AP_GPS &gps = AP::gps();
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
if (validOrigin && !inFlight) {
if (!gpsGoodToAlign) {
// if we don't have GPS lock then we shouldn't be doing a
// resetHeightDatum, but if we do then the best option is
// to maintain the old error
ekfGpsRefHgt += (int32_t)(100.0f * oldHgt);
} else {
// if we have a good GPS lock then reset to the GPS
// altitude. This ensures the reported AMSL alt from
// getLLH() is equal to GPS altitude, while also ensuring
// that the relative alt is zero
ekfGpsRefHgt = gps.location().alt*0.01;
} else {
// if we don't have GPS lock then we shouldn't be doing a
// resetHeightDatum, but if we do then the best option is
// to maintain the old error
ekfGpsRefHgt += oldHgt;
ekfGpsRefHgt = AP::gps().location().alt*0.01;
}
}
// adjust the terrain state
terrainState += oldHgt;
return true;

29
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -17,17 +17,28 @@ extern const AP_HAL::HAL& hal;
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 NavEKF3_core::calcGpsGoodToAlign(void)
void NavEKF3_core::calcGpsGoodToAlign(void)
{
if (inFlight && assume_zero_sideslip() && !use_compass()) {
// this is a special case where a plane has launched without magnetometer
// is now in the air and needs to align yaw to the GPS and start navigating as soon as possible
return true;
gpsGoodToAlign = true;
return;
}
// User defined multiplier to be applied to check thresholds
float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler;
if (gpsGoodToAlign) {
/*
if we have already passed GPS alignment checks then raise
the check threshold so that we have some hysterisis and
don't continuously change from able to arm to not able to
arm
*/
checkScaler *= 1.3f;
}
// 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 && yawTestRatio <= 1.0f) || !consistentMagData) {
@ -222,16 +233,20 @@ bool NavEKF3_core::calcGpsGoodToAlign(void)
lastGpsVelFail_ms = imuSampleTime_ms;
}
// record time of fail
// record time of pass or fail
if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || vAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) {
lastGpsVelFail_ms = imuSampleTime_ms;
} else {
lastGpsVelPass_ms = imuSampleTime_ms;
}
// continuous period without fail required to return a healthy status
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
return true;
// continuous period of 10s without fail required to set healthy
// continuous period of 5s without pass required to set unhealthy
if (!gpsGoodToAlign && imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
gpsGoodToAlign = true;
} else if (gpsGoodToAlign && imuSampleTime_ms - lastGpsVelPass_ms > 5000) {
gpsGoodToAlign = false;
}
return false;
}
// update inflight calculaton that determines if GPS data is good enough for reliable navigation

5
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -751,8 +751,8 @@ private:
// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
bool getTouchdownExpected();
// Assess GPS data quality and return true if good enough to align the EKF
bool calcGpsGoodToAlign(void);
// Assess GPS data quality and set gpsGoodToAlign
void calcGpsGoodToAlign(void);
// set the class variable true if the delta angle bias variances are sufficiently small
void checkGyroCalStatus(void);
@ -905,6 +905,7 @@ private:
float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
float gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver
uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
uint32_t lastGpsVelPass_ms; // time of last GPS vertical velocity consistency check pass
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
float posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
bool useGpsVertVel; // true if GPS vertical velocity should be used