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

AP_NavEKF: clean up determination of GPS availability

This commit is contained in:
priseborough 2014-12-22 08:26:55 +11:00 committed by Randy Mackay
parent 40b02fc19a
commit b6d300db19
1 changed files with 3 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -456,7 +456,7 @@ void NavEKF::ResetPosition(void)
if (posHoldMode || gpsInhibitMode != 0) { if (posHoldMode || gpsInhibitMode != 0) {
state.position.x = 0; state.position.x = 0;
state.position.y = 0; state.position.y = 0;
} else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { } else if (!gpsNotAvailable) {
// write to state vector and compensate for GPS latency // write to state vector and compensate for GPS latency
state.position.x = gpsPosNE.x + gpsPosGlitchOffsetNE.x + 0.001f*velNED.x*float(_msecPosDelay); state.position.x = gpsPosNE.x + gpsPosGlitchOffsetNE.x + 0.001f*velNED.x*float(_msecPosDelay);
state.position.y = gpsPosNE.y + gpsPosGlitchOffsetNE.y + 0.001f*velNED.y*float(_msecPosDelay); state.position.y = gpsPosNE.y + gpsPosGlitchOffsetNE.y + 0.001f*velNED.y*float(_msecPosDelay);
@ -476,9 +476,7 @@ void NavEKF::ResetVelocity(void)
state.velocity.zero(); state.velocity.zero();
state.vel1.zero(); state.vel1.zero();
state.vel2.zero(); state.vel2.zero();
} else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D && _fusionModeGPS <= 1) { } else if (!gpsNotAvailable) {
// read the GPS
readGpsData();
// reset horizontal velocity states, applying an offset to the GPS velocity to prevent the GPS position being rejected when the GPS position offset is being decayed to zero. // reset horizontal velocity states, applying an offset to the GPS velocity to prevent the GPS position being rejected when the GPS position offset is being decayed to zero.
state.velocity.x = velNED.x + gpsVelGlitchOffset.x; // north velocity from blended accel data state.velocity.x = velNED.x + gpsVelGlitchOffset.x; // north velocity from blended accel data
state.velocity.y = velNED.y + gpsVelGlitchOffset.y; // east velocity from blended accel data state.velocity.y = velNED.y + gpsVelGlitchOffset.y; // east velocity from blended accel data
@ -744,7 +742,7 @@ void NavEKF::UpdateFilter()
// define rules used to set position hold mode // define rules used to set position hold mode
// position hold enables attitude only estimates without GPS by fusing zeros for position // position hold enables attitude only estimates without GPS by fusing zeros for position
if ((vehicleArmed && gpsInhibitMode == 1) || !vehicleArmed) { if (gpsInhibitMode == 1) {
posHoldMode = true; posHoldMode = true;
} else { } else {
posHoldMode = false; posHoldMode = false;