[VTOL] No Global Position Failsafe not working properly in FW mode

[ThomasRigi]

Describe the bug
The following points don't work as they should upon triggering a GPS Failsafe on a VTOL drone in FW (flying in an AUTO mode):

1. The drone doesn't do the fixed bank loiter upon a GPS Failsafe. Instead, it pretty much continues to fly in whatever pose it happened to be upon triggering.
2. Also, the landing doesn't seem to work. In SITL it just keeps flying.
3. Recovering the GPS signal doesn't work properly.
4. It "randomly" falls back to the previous flight mode after some time (after it should have initiated landing)

To Reproduce
Steps to reproduce the behavior:

1. Start SITL with make px4_sitl gazebo_standard_vtol
2. Set the NAV_GPSF_* parameters to some non-zero value
3. Launch a mission or just loiter in FW
4. Trigger the failsafe by disabling the GPS signal: gpssim stop
5. The drone will not be doing the fixed bank loiter properly
6. Enable the GPS driver again: gpssim start
7. The drone will not (directly) recover its global position even though on QGC you can see its exact position
8. Wait
9. The drone will not initiate landing (even though navigator says Terminate)
10. At some point the drone will switch back to what it was doing before (Mission or Loiter)

Expected behavior
The drone should do the fixed bank loiter as a FW plane and upon recovery of its global position continue its mission or whatever it was doing before.

Log Files and Screenshots
SITL, v1.9 : https://logs.px4.io/plot_app?log=5befa577-0ade-4c90-b82c-045830915138
SITL, current master : https://logs.px4.io/plot_app?log=ac15492a-04b5-4e27-851e-cdb00cb27d62

Real flight, v1.9 : https://logs.px4.io/plot_app?log=7c1a4be9-8e6b-41e2-b7fa-dd29cd270795

• our pilot took over after 7s, but clearly the fixed bank angle didn't work.
  

Drone (please complete the following information):

• SITL : standard VTOL quadplane
• Real drone : standard VTOL, running v1.9 on a Cube

Additional context
I tested the Failsafe on the Gazebo plane and it works as it should. I also tested the Failsafe on SITL VTOL in MC and it's good too.

Insights on the root cause of the failsafe on the real drone would be very welcome too :) I can't quite pin down what caused the velocity and position innovations to diverge suddenly.
I know that the vibrations and the magnetic interference could have been part of the issue, but we have over 10h of flight time with the same setup (or even more vibrations) on v1.8.2 where to my knowledge it never occurred and another 5h on v1.9 (with two triggers so far)

[ThomasRigi]

Can we get GpsFailure::on_active() to publish the GPSF loiter setpoints in _fw_virtual_att_sp for VTOLs instead of publishing to att_sp?

Currently there is no new fw virtual attitude setpoint generated throughout the failsafe duration, so this line overwrites the attitude setpoint set by the failsafe by the last virtual setpoint before the failsafe (if my understanding is right).

This should hopefully fix the bank loiter part. About recovering the global position and continuing the mission I wouldn't know where to start.

If the loiter times out and the GPS still hasn't recovered I think it would be best to force a MC landing

[ThomasRigi]

Reply to myself: yes, we can.
I will create a PR shortly after some more SITL testing. Adding
if (_navigator->get_vstatus()->is_vtol) { _virt_att_sp_pub.publish(att_sp); } else { _att_sp_pub.publish(att_sp); }
in GpsFailure::on_active() works for the fixed bank loiter.
I have no idea how my changes influence the GPS recovery and continuing the mission, but in SITL it now works sometimes 🤷‍♂️
(edit) The recovery still doesn't work reliably, must have been a lucky coincidence when it worked

[ThomasRigi]

#12778

[sfuhrer]

@ThomasRigi thanks for you PR #12778 , this should fix at least the fixed-bank loiter.

The recovery, termination and the overall behavior for VTOLs is still open, maybe we find some time in the near future to push further all of that.

Have you by now figured out what the root of the No global position in your flight? It looks for me as if the problem is within the EKF, and not from the GPS, as the GPS speed seems good right before the fail is triggered:

Also: I noticed that you had QuadChute towards the end of the flight, was that related to the other stuff?

[julianoes]

@sfuhrer this came up in the dev call because it's a release blocker. Do you think we can have a look this together?

[julianoes]

Fixed in #12778.