Browse Source

AP_NavEKF: Fix bug affecting in-flight GPS acquisition

This bug caused velocities to be reset to zero
mission-4.1.18
Paul Riseborough 10 years ago committed by Andrew Tridgell
parent
commit
1c244af3d8
  1. 1
      libraries/AP_NavEKF/AP_NavEKF.cpp

1
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -4014,6 +4014,7 @@ void NavEKF::readGpsData()
if (vehicleArmed && _fusionModeGPS != 3) { if (vehicleArmed && _fusionModeGPS != 3) {
constPosMode = false; constPosMode = false;
PV_AidingMode = AID_ABSOLUTE; PV_AidingMode = AID_ABSOLUTE;
gpsNotAvailable = false;
// Initialise EKF position and velocity states // Initialise EKF position and velocity states
ResetPosition(); ResetPosition();
ResetVelocity(); ResetVelocity();

Loading…
Cancel
Save