Plane: postition control QPOS1 and QPOS2

[IamPete1]

This adds position control to the QPOS1 and QPOS2 VTOL approach states. It attempts to fly a straight line from the entry of QPOS1 to the destination. In QPOS1 the target position is simply the closest point on that line. This allows to to "free wheel" along the axis of the line while trying to minimize cross track. If the vehicle ever goes beyond the destination the target point becomes "locked" to the destination, this means that the postilion controller helps slow down in the case of overshoot.

Once in QPOS2 that target point is advances along the line at WP speed until it reaches the destination, where it is again locked. The threshold for the switch to QPOS2 is now purely based on ground speed.

In both QPOS1 and QPOS2 the nose of the vehicle is pointed into the apparent wind.

Some testing with 10m/s cross wind:
PR Normal approach:

Master Normal approach:

So for normal approach we see a very little difference, maybe slightly less overshoot.

PR "too close" approach:

Master "too close":

A significant improvement, PR stops much sooner thanks to the additional "pull" of the target potion.

We still suffer somewhat when the vehicle is not traveling towards the destination when entering QPOS1. This PR apply the deceleration against the direction between the start of QPOS1 and the destination. Master always applies it away from the destination.

I think this is a improvement, and it simplifys the code a little. I was tempted to combine QPOS1 and QPOS2, there is now very little difference. There is still more improvements possible especially for "too close" approaches.

Needs lots of edge case testing and testing with ship landing.

[IamPete1]

Rebased, I'm fairly happy that this is at least as good as the current method and in much better in cases with some misalignment where the vehicle is not traveling towards the destination when it switches in to QPSO1.

This is very similar to how the old leash based position controller worked.

It is more vulnerable to bad wind estimate as it is now always used to calculate heading rather than just in the overshoot case.

[tridge]

@IamPete1 where the wind could be very bad is if we haven't flown significantly in fwd flight, for example a QLOITER flight followed by QRTL, this might end up yawing around a lot?

[IamPete1]

@tridge We currently don't update the wind estimate once the VTOL motors are active, so it should just end up yawing to a random heading in that case, it will not keep yawing to different headings. But of course that random heading could be completely wrong resulting in trying to fly backwards.

[tridge]

we need some sort of threshold on this, based on either wind estimate or wind variance. We also need to allow for pilot yaw override if stick mixing is enabled

[timtuxworth]

This might help with the problem I saw here https://www.dropbox.com/scl/fi/nl3yrk05md41ndbuzez2s/log_52_2025-10-20-14-24-50.bin?rlkey=kb8nppl9kn2rvus6ih2et0htl&st=qz7rq1ad&dl=0 where a QRTL while flying away from home resulted in a very exciting time. This flight replicates a similar flight where I lost the vehicle, so it wasn't fun.

Note it's the 2nd transition in this log that's the most interesting. Here's a video from the onboard camera that shows how this looks. I'm surprised the vehicle survived. https://youtu.be/E8bJBzEQbvM

[timtuxworth]

Maybe I'm not understanding this @IamPete1 , but as far as I can see the instability I saw was because the plane was trying to yaw to the target (i.e. next waypoint) at POS1 while at the same time it was also braking and switching to VTOL. I'm thinking that might be the cause of the instability and a better answer would be to not do that - it should use the current direction between POS1 and POS2 and not change the target direction until it has completed the switch to VTOL.

Of course I could be missing the point.