VTOL GPSF: fix fixed bank loiter

[ThomasRigi]

Pull request fixing part of #12758

Describe problem solved by the proposed pull request
Fixed bank loiter after a No Global Position Failsafe wasn't working for VTOLs in FW mode.

Test data / coverage
Tested in SITL make px4_sitl gazebo_standard_vtol by stopping gpssim driver. I also reactivated the gpssim driver to see if the vehicle recovers its position and continues its mission afterwards.

Logs:
Working fixed bank loiter, late recovery:
https://logs.px4.io/plot_app?log=217b432a-9452-4f67-862d-32322aaefe96

Working recovery:
https://logs.px4.io/plot_app?log=b165d3ea-54cc-4145-aeb7-acf75892611e
(running on an older commit head, but meanwhile it doesn't work anymore on this one, see this log: https://logs.px4.io/plot_app?log=2aad8195-476f-4af6-b289-c214e61fe853)

GPSF is still working on normal planes :
https://logs.px4.io/plot_app?log=79b5e483-f698-4aa0-8b30-265d25a3cd14

Describe your preferred solution
Publishing the failsafe values to fw_virtual_attitude_setpoint topic if the vehicle is a VTOL so that VtolType::update_fw_state() doesn't overwrite them with old values anymore.

Describe possible alternatives

Additional context
This PR does not fix the GPS recovery & continuing the automatic flight problems.
Also, the landing after "Terminate" is not working, the drone keeps loitering in fixed bank angle mode

[dagar]

Could you do this the way the FW controller does with only a single orb_advert_t? https://github.com/PX4/Firmware/blob/dd2d5c029f1a4260f9c8c8ad44fa787c7ce0b310/src/modules/fw_att_control/FixedwingAttitudeControl.cpp#L383-L398

[ThomasRigi]

@dagar Done. Thanks for pointing it out, I'm not yet familiar with the uORB implementation details.

Here's a SITL log after the change: https://logs.px4.io/plot_app?log=6ba1b5d6-b708-428b-bc8b-71fd156be88b

[julianoes]

@ThomasRigi thanks for the PR! We will try to review it.

[julianoes]

Could we do the same change without going back to the "old" uorb API? So could we use _att_sp_pub.publish(att_sp); in both places here?

[julianoes]

This should also go into the release/1.10 branch. @dagar

[ThomasRigi]

Is there someone who knows well about the COM_POS_FS* parameters? I've read on the parameter reference that the default values are optimised for MC and how to change them for FW. What do we do with VTOLs? Ideally they should work for both flight styles. Or do we need to create a set of parameters for each MC and FW and the flight stack will choose the right one depending on the flight mode?

Setting COM_POS_FS_GAIN = 0 and COM_POS_FS_PROB = 1 (as proposed for FW) makes the GPS recovery immediate, and the vehicle then continues the mission straightaway. But how will this affect a global position failure in MC?

[sfuhrer]

@ThomasRigi
I've just tested what with the same procedure as you specify in #12758. Your PR seems to fix the loiter after the GPS failure, thanks for finding this bug!
Please address the comment of Julian, then I'd say we can merge this (maybe also squash your commits into one single one).

[ThomasRigi]

Sorry for the late answer, I was away for three weeks.

I haven't managed to get it working with the new uORB API. I tried to initialise as uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{nullptr}; in gpsfailure.h and set the ORB_ID in gpsfailure.cpp, but as the _handle is protected I can't access it from within gpsfailure.cpp

How can I set the ORB_ID at a later point than when declaring _att_sp_pub with the new API?

[ThomasRigi]

@julianoes forgot to tag you in my last response.

If you have an idea of how to set the ORB ID with the new API, I can finalise and clean up the PR.

[julianoes]

@ThomasRigi aha, now I understand. I think you need to create both publications and then publish to one of them.

@dagar is that correct?

[julianoes]

@ThomasRigi I'm trying this pull request with standard_vtol and it correctly goes into fixed bank loiter, however, it seems to be pitching down about 5-10 degrees and loose altitude quickly until doing a hard landing. Is that expected?

[julianoes]

In another try it did a back transition and went into blind hover, is that also expected? I'm confused now.

@sfuhrer do you understand this?

[ThomasRigi]

@julianoes The falling out of sky is probably because of NAV_GPSF_* parameters. Try setting NAV_GPSF_P to 2 and NAV_GPSF_TR to 28 and it should do a nice loiter. Default is 0 thrust, so it's no wonder that the landing is hard

[ThomasRigi]

Blind hover? Maybe that was supposed to be a forced MC landing? I didn't delve into the failsafe logic, so I'm not sure. This PR only addresses the fixed bank loiter part.

[julianoes]

@ThomasRigi I've force pushed to this branch, and in my testing this did the same as your proposal (except that one time where it transitioned back).

[julianoes]

@sfuhrer can you review this please? I think it makes sense to get it into the release.

[sfuhrer]

@julianoes looks good to me, let's get this in.

[julianoes]

Thanks @sfuhrer!