autotest: add test for EKF losing then refinding a GPS

[peterbarker]

This test cruelly rips away the GPS from the EKF, waits for it to enter land mode (and until 30 seconds have passed). It then gives the EKF the GPS back and calls RTL instantly.

The EKF seems to start to give position back to the vehicle (or else we wouldn't be able to enter RTL), but a few seconds later the EKF decides it is unhappy with the world again (variances) and forces and EKF failsafe again.

There should probably be some sort of position-reset happening within the EKF here.

2021-04-05 17:11:32.007 SRC=250/250:AT-0017.5: set_parameters: ({'GPS_TYPE': 1})
2021-04-05 17:11:32.010 SRC=250/250:AT-0017.5: GPS_TYPE want=1.000000 autopilot=None
2021-04-05 17:11:32.012 SRC=250/250:AT-0017.5: Requesting (GPS_TYPE) (retry=0)
2021-04-05 17:11:32.105 SRC=250/250:AT-0017.5: GPS_TYPE want=1.000000 autopilot=None
2021-04-05 17:11:32.110 SRC=250/250:AT-0017.5: Requesting (GPS_TYPE) (retry=1)
2021-04-05 17:11:32.305 SRC=250/250:AT-0017.5: Received wanted PARAM_VALUE GPS_TYPE=0.
2021-04-05 17:11:32.307 SRC=250/250:AT-0017.5: GPS_TYPE want=1.000000 autopilot=0.0
2021-04-05 17:11:32.310 SRC=250/250:AT-0017.5: Sending set (GPS_TYPE) to (1.000000) (o
2021-04-05 17:11:32.325 EKF3 IMU1 is using GPS
2021-04-05 17:11:32.332 EKF3 IMU0 is using GPS
2021-04-05 17:11:32.332 SRC=250/250:AT-0017.5: AP: EKF3 IMU1 is using GPS
2021-04-05 17:11:32.340 SRC=250/250:AT-0017.5: AP: EKF3 IMU0 is using GPS
2021-04-05 17:11:32.342 EKF3 lane switch 1
2021-04-05 17:11:32.342 Event: DATA_EKF_YAW_RESET
2021-04-05 17:11:32.342 Error: Subsys EKF_PRIMARY ECode 1 
2021-04-05 17:11:32.342 EKF primary changed:1
2021-04-05 17:11:32.350 SRC=250/250:AT-0017.5: AP: EKF3 lane switch 1
2021-04-05 17:11:32.355 SRC=250/250:AT-0017.5: AP: EKF primary changed:1
2021-04-05 17:11:32.485 SRC=250/250:AT-0017.5: Received wanted PARAM_VALUE GPS_TYPE=1.
2021-04-05 17:11:32.485 SRC=250/250:AT-0017.5: GPS_TYPE want=1.000000 autopilot=1.0
2021-04-05 17:11:32.485 SRC=250/250:AT-0017.5: GPS_TYPE is now 1.000000
2021-04-05 17:11:33.015 SRC=250/250:AT-0017.6: Changing mode to RTL
2021-04-05 17:11:33.017 SRC=250/250:AT-0017.6: Sending COMMAND_LONG to (1,1) (MAV_CMD_
2021-04-05 17:11:33.020 Mode change to RTL failed: requires position
2021-04-05 17:11:33.020 Error: Subsys FLIGHT_MODE ECode 6 
2021-04-05 17:11:33.027 SRC=250/250:AT-0017.6: AP: Mode change to RTL failed: requires
2021-04-05 17:11:33.340 Error: Subsys EKFCHECK ECode 0 
2021-04-05 17:11:33.340 EKF Failsafe Cleared
2021-04-05 17:11:33.340 Error: Subsys FAILSAFE_EKFINAV ECode 0 
2021-04-05 17:11:33.353 SRC=250/250:AT-0017.6: AP: EKF Failsafe Cleared
2021-04-05 17:11:33.440 Error: Subsys GPS ECode 2 
2021-04-05 17:11:33.440 GPS Glitch
2021-04-05 17:11:33.505 SRC=250/250:AT-0017.6: AP: GPS Glitch
2021-04-05 17:11:34.007 SRC=250/250:AT-0017.7: mav.flightmode=LAND Want=RTL custom=9
2021-04-05 17:11:34.010 SRC=250/250:AT-0017.7: Sending COMMAND_LONG to (1,1) (MAV_CMD_
2021-04-05 17:11:34.022 SRC=250/250:AT-0017.7: Got mode RTL
2021-04-05 17:11:34.252 SRC=250/250:AT-0017.7: RC values good
2021-04-05 17:11:34.255 SRC=250/250:AT-0017.7: Waiting RTL to reach Home and disarm
2021-04-05 17:11:34.840 SRC=250/250:AT-0017.8: Alt: 4.28  HomeDist: 110.45 
2021-04-05 17:11:35.238 SRC=250/250:AT-0017.9: Alt: 4.40  HomeDist: 110.56 

[peterbarker]

Running this test is simply:

./Tools/autotest/autotest.py --gdb --debug build.Copter test.Copter.RefindGPS

(subsequent runs can omit the build step if the code hasn't changed)

[rmackay9]

I think we should close this PR because I can't reproduce the issue. I think this PR is trying to switch to the semi-autonomous mode too quickly. It needs to give the EKF more time.

[peterbarker]

I think we should close this PR because I can't reproduce the issue. I think this PR is trying to switch to the semi-autonomous mode too quickly. It needs to give the EKF more time.

I disagree. The EKF is misbehaving and (a) this could happen on a real vehicle and (b) this might be a symptom of a deeper problem with EKF state.

.... additionally, if the EKF needs more time then it shouldn't be returning "I have a position" when it does not.

[peterbarker]

The associated issue has been marked as "fixed", and this test appear to be passing, so I think we're OK to merge this one.

[peterbarker]

Current output of this test shows the problem is no longer present:

[rmackay9]

I couldn't merge this cleanly into 4.1