TURN ASSIST improvements for Fixed Wing

[avsaase]

Closes #6339.

One think to note is that the rate of turn calculations now depend on the targeted bank angle, but the transformation of the turn rate into the targetRates in the body reference frame still relies on the actual attitude. Moving a model on the bench this seems to give sensible results, but it does sounds a bit contradictory so I'll have to see in flight how it works out.

[avsaase]

Btw, the reason I keep going on and on about TURN ASSIST is that I see video's like this and this of airplanes tipping over when making a turn in a nav mode popping up on the INAV fixed wing Facebook group multiple times per week. You can't rule out user error with these video's but it would be nice if the nav modes were a little more robust.

[avsaase]

Here is the log file for the second video: LOG00013.zip. RTH is activated at 1:58. The airplane banks to the left but as the airplane pitches down a little it starts to give more roll input (PID_sum[roll] increases) after which the airplane tips over. There are also some downward jumps in PID_sum[roll] as it crosses 90 degree bank angle. Unfortunately the pitch and roll angles are not properly logged, but is seem that when the plane is banked more than 90 degrees it tries to level out. But as soon as the angle becomes less than 90 degrees it again gives a lot of roll input tipping it back over.

Testing this on the bench (with unchanged pidTurnAssistant) shows that the rudder and elevator deflection increases with the bank angle until it crosses 90 degrees when they jump back to center. To me it makes no sense that the rudder deflection keeps increasing with bank angle, because a decreasing fraction of the yaw rotation actually contributes to the turn, with the rest only pointing the nose down. I think this is because the larger bank angle also increases coordinatedTurnRateEarthFrame. With this PR for a given bankAngleTarget, larger bank angles decrease the rudder deflection, but still increase the elevator deflection, so perhaps this will help.

Another thing is the check if (calculateCosTiltAngle() >= 0.173648f), which should disable the turn assist calculation when the bank and/or pitch angle is too large. If I understand correctly this evaluates to a roll limit of around 35 degrees if pitch is zero, but I'm not able to see any change in the outputs around this threshold. Is this working as it should?

A final point is the updating of the roll target rates in pidTurnAssistant. In see how this helps to keep a consistent turn when climbing or diving, but I suspect this contributes to the instability in the video. I imagine it working like a positive feedback loop: the airplane banks, it banks to much, the nose drops, now the airplane is pitching down so it needs more roll for the coordinated turn, more roll means even larger bank angle, nose drops more, etc. I will test with a fixedWingCoordinatedRollGain and see if it makes a difference. In term of a mathematical/physical reason for making a change instead of a practical one, all I can think of is that the FAA handbook only talks about constant altitude coordinated turns. If anything, a coordinated turn while climbing or diving should yield a slower turn rate because more distance needs to be traveled.

WDYT? I'll stop rambling now ;-)

[digitalentity]

The qustion is why the airplane reaches 90 deg? IMO this is misconfiguration. The airplane shouldn't be able to cross 90 deg boundary as this leads to gimbal lock of the Euler angle triad.

Having rudder working always makes some sense actually. The more you bank, the less authority your rudder will have on your heading, however the more you bank, the sharper turn you expect to do (larger heading change rate).

On the other hand, our self-leveling controller will also get confused as we reach 90deg bank, as PID expects pitch to be controlled by elevator. Generally, I think it's not safe to expect INAV to do hard turns at 80deg bank, so when we go back to the issue you desribe this is still unrealistic expectations/configuration.

[avsaase]

I completely agree. I'd say 60 degrees is a reasonable limit for coordinated turns. Would it makes sense to either freeze the targetRates when the bank angle becomes too large, or limit coordinatedTurnRateEarthFrame to, say, 90 degrees per second? [EDIT: on furhter thought, this PR already limits coordinatedTurnRateEarthFrame because bankAngleTarget is already constrained by fw.max_bank_angle or max_angle_inclination[FD_ROLL] depending on the mode. Adding another constraint to limit this to +- 60 degrees makes sense I think.]

Regarding the rudder deflection with increasing bank angle, this makes sense if you treat the axis as independent. But in reality rudder also induces roll and INAV doesn't know this before it happens. I wonder if implementing some sort of yaw2roll_factor could solve this.

I don't have a diff from the second video but here is a screenshot that was posted along with the video that shows the max bank angle in the nav modes is 40 degrees:

So clearly, the auto pilot is not adhering to the constraints correctly.

I have now also received a dump and log for the first video. The airplane is not perfectly trimmed, but I think there is still something to learn in this log. At 5:49.951 RTH is engaged (1) and the plane banks to the right to make the turn (2). At 5:51.018 the roll target rate (axisRate[0]) goes back to zero (3), presumably because the desired bank angle is achieved. Around the same time the target yaw rate goes below -100 degrees per second to make the coordinated turn (4). As a result, the plane keep rolling instead of maintaining a constant bank angle (5). The target roll rate now goes into the other direction (6) to get bank to the maximum bank angle but it fails to do this. Eventually it tips over (7) and RTH is disabled.

IMO this sequence of events is perfectly explained by a positive feedback loop of rudder inducing roll, means tighter turn, means more rudder, until it tips over (which this PR will hopefully fix), and the underlying issue that INAV doesn't understand how rudder affects roll.

[avsaase]

I have now tested this in several flights both on a rudder plane (Ranger G2) and a rudderless plane (Dart 250) and I am very happy with how this flies. It is hard to quantify but I feel the overall stability when making automated turns is much better on both airplanes. While this is only a minor change to the logic, it will affect all fixed wing pilots using any of the navigation modes. So I think it would be good if this is tested by others as well.