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

AP_NavEKF: Add public function to declare GPS glitch

Browse Source
This commit is contained in:
Paul Riseborough 2015-09-22 23:31:32 +10:00 committed by Randy Mackay
parent 21e4910149
commit 8a9d9c04c5
2 changed files with 27 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4281,12 +4281,15 @@ void NavEKF::readGpsData()
useGpsVertVel = false;
}
// Monitor quality of the GPS velocity data for alignment
gpsGoodToAlign = calcGpsGoodToAlign();
// read latitutde and longitude from GPS and convert to local NE position relative to the stored origin
// If we don't have an origin, then set it to the current GPS coordinates
const struct Location &gpsloc = _ahrs->get_gps().location();
if (validOrigin) {
gpsPosNE = location_diff(EKF_origin, gpsloc);
} else if (calcGpsGoodToAlign()){
} else if (gpsGoodToAlign){
// Set the NE origin to the current GPS position
setOrigin();
// Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
@ -4950,7 +4953,7 @@ void NavEKF::getFilterStatus(nav_filter_status &status) const
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
bool optFlowNavPossible = flowDataValid && (_fusionModeGPS == 3);
bool gpsNavPossible = !gpsNotAvailable && (_fusionModeGPS <= 2);
bool gpsNavPossible = !gpsNotAvailable && (_fusionModeGPS <= 2) && gpsGoodToAlign;
bool filterHealthy = healthy();
bool gyroHealthy = checkGyroHealthPreFlight();
@ -5042,6 +5045,18 @@ void NavEKF::performArmingChecks()
meaHgtAtTakeOff = hgtMea;
// reset the vertical position state to faster recover from baro errors experienced during touchdown
state.position.z = -hgtMea;
// record the time we disarmed
timeAtDisarm_ms = imuSampleTime_ms;
// if the GPS is not glitching when we land, we reset the timer used to check GPS quality
// timer is not set to zero to avoid triggering an automatic fail
if (gpsAccuracyGood) {
lastGpsVelFail_ms = 1;
gpsGoodToAlign = true;
}
// we reset the GPS drift checks when disarming as the vehicle has been moving during flight
gpsDriftNE = 0.0f;
gpsVertVelFilt = 0.0f;
gpsHorizVelFilt = 0.0f;
} else if (_fusionModeGPS == 3) { // arming when GPS useage has been prohibited
if (optFlowDataPresent()) {
PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
@ -5509,4 +5524,10 @@ bool NavEKF::checkGyroHealthPreFlight(void) const
return retVar;
}
// returns true of the EKF thinks the GPS is glitching or unavailable
bool NavEKF::getGpsGlitchStatus(void) const
{
return !gpsAccuracyGood;
}
#endif // HAL_CPU_CLASS

4
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -267,6 +267,9 @@ public:
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation
bool getHeightControlLimit(float &height) const;
// returns true of the EKF thinks the GPS is glitching
bool getGpsGlitchStatus(void) const;
// return the amount of yaw angle change due to the last yaw angle reset in radians
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t getLastYawResetAngle(float &yawAng);
@ -700,6 +703,7 @@ private:
float gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks
float gpsVertVelFilt; // amount of filterred vertical GPS velocity detected durng pre-flight GPS checks
float gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
bool gpsGoodToAlign; // true when GPS quality is good enough to set an EKF origin and commence GPS navigation
// Used by smoothing of state corrections
Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement