diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 582506d876e6428e0849a258d811b2bfca83c86a..241335965b39aa7581435a73cbe389d5ef64cb95 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -743,11 +743,18 @@ void NavEKF::SelectVelPosFusion() if (imuSampleTime_ms - lastFixTime_ms > gpsRetryTimeout) { posTimeout = true; velTimeout = true; - //If this happens in flight and we don't have airspeed or sideslip assumption to constrain drift, then go into constant velocity mode and stay there until disarmed + //If this happens in flight and we don't have airspeed or sideslip assumption to constrain drift, then go into constant position mode and stay there until disarmed if (vehicleArmed && !useAirspeed() && !assume_zero_sideslip()) { - constVelMode = true; - constPosMode = false; // always clear constant position mode if constant velocity mode is active + constVelMode = false; // always clear constant velocity mode if constant velocity mode is active + constPosMode = true; PV_AidingMode = AID_NONE; + // reset the velocity + ResetVelocity(); + // store the current position to be used to keep reporting the last known position + lastKnownPositionNE.x = state.position.x; + lastKnownPositionNE.y = state.position.y; + // reset the position + ResetPosition(); } } @@ -3509,9 +3516,14 @@ void NavEKF::getAccelNED(Vector3f &accelNED) const { } // return NED velocity in m/s +// void NavEKF::getVelNED(Vector3f &vel) const { vel = state.velocity; + if (constPosMode) { + vel.x = lastKnownPositionNE.x; + vel.y = lastKnownPositionNE.y; + } } // return the last calculated NED position relative to the reference point (m). @@ -4310,6 +4322,7 @@ void NavEKF::InitialiseVariables() summedDelVel.zero(); velNED.zero(); gpsPosGlitchOffsetNE.zero(); + lastKnownPositionNE.zero(); gpsPosNE.zero(); prevTnb.zero(); memset(&P[0][0], 0, sizeof(P)); @@ -4530,6 +4543,8 @@ void NavEKF::performArmingChecks() } // zero stored velocities used to do dead-reckoning heldVelNE.zero(); + // zero last known position used to deadreckon if GPS is lost + lastKnownPositionNE.zero(); // set various useage modes based on the condition at arming. These are then held until the vehicle is disarmed. if (!vehicleArmed) { PV_AidingMode = AID_NONE; // When dis-armed, we only estimate orientation & height using the constant position mode diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 269e7d7dbcdcfac0862a2b6489d7743bc6db5a27..3d9be0122128af9ff82e197b48b76dc1c5ea1ed8 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -555,6 +555,7 @@ private: float IMU1_weighting; // Weighting applied to use of IMU1. Varies between 0 and 1. bool yawAligned; // true when the yaw angle has been aligned Vector2f gpsPosGlitchOffsetNE; // offset applied to GPS data in the NE direction to compensate for rapid changes in GPS solution + Vector2f lastKnownPositionNE; // last known position uint32_t lastDecayTime_ms; // time of last decay of GPS position offset float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold float posTestRatio; // sum of squares of GPS position innovation divided by fail threshold