QuadPlane: fix QLoiter backtransitions

[robertlong13]

Summary

Fix a bug that can cause excessive position hunting when switching to QLoiter/QLand from fixed-wing.

Classification & Testing (check all that apply and add your own)

• Checked by a human programmer
• Non-functional change
• No-binary change
• Infrastructure change (e.g. unit tests, helper scripts)
• Automated test(s) verify changes (e.g. unit test, autotest)
• Tested manually, description below (e.g. SITL)
• Tested on hardware
• Logs attached
• Logs available on request

Description

Currently, the stopping behavior when switching to QLoiter depends on state that accumulated during your transition to fixed-wing flight. This bug is long-standing (tested back to 4.5). Below are two screenshots from a test on master.

If you don't do your outbound transition perfectly straight with wings level, you get this weird hard bank when you switch into QLoiter/QLand.

The fix I'm proposing in this PR is to initialize the loiter at the current position, re-seeding the velocity PID integrator for a pure brake (only when initializing from a period of position controller inactivity). The vehicle now rolls wings level and pitches back to a stop from any entry attitude, regardless of the state of the forwar-transition, stopping in ~6s from 25m/s and remaining within 2m of the final landing point, where it previously wallowed for 13s and wandered around more than 10m.

Screenshot of ground tracks before/after my fix:

My only issue is that the code for my fix looks a tad hacky with the vel_pid stuff I'm doing, but I think the idea is sound, and I believe this is the cleanest way to achieve it. @lthall (or anyone else) if you have any opinions on a better shape of this, I'd be happy to hear them. Or if you just wanna swipe my autotest and fix this a different way in a superseding PR, I'm totally fine with that.

The new autotest reliably fails, on all 4 criteria, before the fix, and reliably passes after. Plenty of headroom.

Turning transition (transition tail freezes a banked attitude target)

criterion	bound	master (3 runs)	with fix (3 runs)
retreat from closest approach to stopping point	< 5 m	10.03 / 9.37 / 11.98	1.89 / 1.90 / 1.88
settle from 20 m out to within 2 m	< 8 s	11.60 / 11.30 / 12.60	2.10 / 2.10 / 2.10
ground-course winding above 3 m/s	< 60°	318 / 314 / 338	18.0 / 18.0 / 17.9
N/S displacement of stopping point from entry	< 25 m	43.3 / 44.8 / 42.5	16.9 / 17.0 / 17.0

Every master run fails all four criteria; every fixed run passes all four.

Straight transition (control: transition tail freezes a level attitude target)

criterion	bound	master (3 runs)	with fix (3 runs)
retreat from closest approach to stopping point	< 5 m	1.86 / 1.84 / 1.84	1.89 / 1.89 / 1.88
settle from 20 m out to within 2 m	< 8 s	2.10 / 2.10 / 2.10	2.10 / 2.10 / 2.10
ground-course winding above 3 m/s	< 60°	14.4 / 14.4 / 14.4	9.4 / 17.5 / 17.7
N/S displacement of stopping point from entry	< 25 m	13.2 / 13.2 / 13.2	17.5 / 17.4 / 14.9

Master already passes when its stale attitude target happens to be level, the same code and thresholds, demonstrating the expectation is realistic and the fix simply makes the happy case the only case.

[IamPete1]

I'm a bit surprised about the leftover state. We do relax the attitude controllers just before we run them if they have not been run recently. I guess the problem is that the loiter/pos-control init is called before that relax is done? So they see the stale values even though the attitude controller do not use them.

[robertlong13]

I'm a bit surprised about the leftover state. We do relax the attitude controllers just before we run them if they have not been run recently. I guess the problem is that the loiter/pos-control init is called before that relax is done? So they see the stale values even though the attitude controller do not use them.

It's the input shaping in the pos-control init that does it. If it initializes from a banked attitude, it will calculate the initial acceleration from that (which is huge), then jerk-limit smooth that out (slowly). This means it will try to follow the banked turn you're doing if you initialize off that (or, because of the leftovers, follow the curve of the banked turn you did an hour ago)

You may notice I've completely reworked how I fix this now, I have a whole write-up with more details, but I wanted to wait until the morning to post it so I'd be fresh for any followup.

Long story short

• master's happy path essentially initializes from level attitude, just not on purpose (and only for a clean takeoff). My old fix was to just make it do that on purpose instead and add a method in AC to clear the target angles to level and zero the rates, and then init as normal, but I had a feeling there would be some resistance to that.
• Instead, my fix is to add a brief "level out" phase upon entry before initializing the loiter controller. That phase takes about 1s on the stock quad plane but surprisingly doesn't hurt the stopping distance (increases distance by 2 meters, and decreases the landing time by 1s)

[robertlong13]

I've reworked the mechanism of my fix in this PR, after some conversation with Leonard, a lot of thought, and a little sleep, and I think this is actually a good shape, with solid rationale. We know that the existing behavior is trying to initialize from attitude targets that may be hours old. The question is which approach to take to fix it.

Option 1: Current Attitude, Immediate Handoff to AC_Loiter

Leonard was experimenting with initializing the loiter controller with the current attitude among other things, which causes it initially to carry on with any banked fixed-wing turn you were doing at the time, braking while slowly leveling back out.

This definitely could be desirable behavior, but the main issue is that the heading stays locked, uncoordinating the turn and fighting physics on the airframe. If we can solve that problem, I'm onboard with it.

Leonard was attempting to solve it with quadplane.desired_auto_yaw_rate_cds(), but that helper is disabled during pure VTOL modes, and even if you change that, you don't want to leave turn coordination feeding into QLoiter at all times. It makes sense to do it at fixed-wing speeds, but once we settle closer to hover speeds, the yaw rates blow up (that's why desired_auto_yaw_rate_cds() clamps the velocity to AIRSPEED_MIN, which is another reason that particular function won't work for coordinating here). Once established in the hover, we want the normal weathervaning controller. The trick is figuring out where to do that handoff, which kinda leads to "we need a two-stage process" (which I do in option 3). I couldn't think of a way to achieve the coordination cleanly myself. I've adjusted my autotest criteria to pass if someone can make that happen though (tested against some unspeakably dirty hacks to coordinate the turns), as long as it also passes the other criteria on settling time and overshoot.

One of the other really good things worth noting on that branch is the switch to using input_thrust_vector_rate_heading_rads instead of input_euler_angle_roll_pitch_euler_rate_yaw_cd, which fixes the issue where large accumulated yaw error ends up messing up the desired roll and pitch. That's the main reason why the current behavior on master hunts around so much worse when you hit QLand during a bank after similarly banking at the end of your outbound transition; you're essentially initializing to your current bank angle (by coincidence), but the subsequent accumulation of yaw error plus the euler angle target issue creates problems. So that change alone is a pretty huge improvement. Here's a picture of what that looks like if you don't do that (taken from the new autotest run against master):

(that change does have a couple unintended consequences to chase down, most notably making the pitch up way more aggressive, because it bypasses SLT_Transition::set_VTOL_roll_pitch_limit(), which is our main "rip the wings off" protection)

Option 2: Level Attitude, Immediate Handoff to AC_Loiter

One option I was playing with was zeroing out the target rates and roll/pitch angles, then doing the loiter init. This has the benefit of being identical to what we've been doing for years, only we've previously been doing it on accident, since that's the state we tend to be in at the end of transition. Doing it explicitly is just a strictly better version of what we're doing now, and I do think that patch would be safe.

Leonard's reaction to this idea is "Loiter needs to be initialised as loiter is initialised. If you need it to behave differently you need a different flight mode.", and he's not wrong, so that leads me to option 3.

Option 3: Level Out First, Later Handoff to AC_Loiter

Initializing the loiter controller in the middle of a fixed-wing maneuver with accelerations far outside the limits it was designed for isn't great. We probably ultimately want to split this up into stages, like we do with auto landings, and hand off to AC_Loiter only when it's sane.

If I wanted to start with the dead-simplest two-stage switch to QLoiter, it would be something like "level out briefly, then init QLoiter", so that's essentially all I'm doing in this PR. I was originally writing this whole thing up to justify option 2, but I tried this one real quick to see how well it would work. I was very surprised to see that it works almost identically to option 2. When hitting QLoiter/QLand from banked flight, this strategy only increases the stopping distance by about 2m and actually decreases the landing time by 1s (marginally less ballooning). If you enter during level flight, the leveling stage immediately ends, so it has zero impact on those cases.

The benefit of this, over option 2, is that this requires no mucking around in the common libraries and lays the foundation for more sophisticated stage-1 behaviors later. In the future, things like the pitch-up rate limit with Q_BACKTRANS_MS should get moved to this stage-1 logic instead of being imposed on the AC position controller as an external limit.

[timtuxworth]

Sounds really interesting @robertlong13 have you had a chance to test it on a real plane?