Reproducible flyaway in loiter, looks like EKF ignoring gps

I sometimes operate in a gps-denied environment, and will fly in alt_hold mode to good enough vantage point to get a stable hdop, then proceed with a mission- usually just staying parked in loiter mode and filming until the battery is low.

Several times, I’ve found the drone in a state where it will hover perfectly in alt_hold, nicely trimmed and with no wind, but will immediately accelerate and fly straight off in loiter mode. This state persists no matter how many times I regain control in alt_hold mode and re-select loiter, and even if I land and disarm/re-arm, and how solid the GPS signal becomes. The only thing that fixes it is a soft reboot or hard power cycle.

Looking at the EKF4.FS,TS,SS flags in one of these bad flights, the AP_NavEKF code is stuck in constPosMode, even after the gps signal has become rock-solid. Looking at the code, I see a couple issues:

If AP_NavEKF::gpsNotAvailable is ever set to “true” because of spotty gps, the code skips calling AP_NavEKF::readGpsData() ever again, and this flag can never get cleared, even during rearming. The NavEKF code will silently operate in constPosMode and not use gps inputs, which gives very erratic and crash-certain behavior in any gps-required flight modes. Since GPS reception is subsequently fine, no alarms or warnings will sound. It will just consistently fly away any time a gps-required mode is entered.

This can be reproduced in sitl by disabling gps for the first 20-30 seconds, then re-enabling it. It can be reproduced in hardware by powering on the drone in a gps-derived environment, then taking it outside after 20 seconds or so. EKF4.SS will show constPosMode for the entire flight, no matter how good the GPS reception becomes, or how many times it’s disarmed/rearmed. A breakpoint on readGpsData() confirms what a quick look at the code suggests- readGpsData() will never again be called- the gpsNotAvailable flag will never have a chance to get cleared.

Two issues: 1) AP_NavEKF code should re-assess and recover from a gpsNotAvailable=true situation, ideally in-flight, and at least during re-arming. 2) If AP_NavEKF is operating in diminished mode such as constPosMode, it should trigger a GPS failsafe if a gps-required flight mode is active, and prevent a gps-required flight mode from being entered.

Disregard. Looks like these bugs were all fixed in 3.3-dev on 1/28. Things move fast around here!

commit 2c012c2763bb26c30e988509020cca33e810d57c
Author: priseborough
Date: Wed Jan 28 15:24:10 2015 +1100

AP_NavEKF: Always check for new GPS data
This fixes a bug that meant that once the EKF had started up in a non-GPS mode, it would no longer read the GPS and therefore would never be able to use GPS again until reset.