Better Nav Turn handling with proper wind compensation

[b14ckyy]

Right now up to 5.1, the heading control seems to be very simple.
With compass disabled, the ground course gets aligned to the Heading Setpoint, with compass enabled, the nose of the craft will be aligned to the heading setpoint. But both methods have downsides as we know. With compass heading, any crosswind will throw the plane off course and this is also not how real planes navigate. But also with Ground course navigation we have issues that remain.

Assuming the plane is flying straight to the north like in this graphic. The wind is coming from north straight to south and wind speed is NOT known, no compass is enabled. The nose (heading) and course (over ground) are lined up
Now the plane wants to make a navigation turn to fly straight east, yellow line to head for a new waypoint or just because the pilot commands the course change in cruise mode.
From my understanding without knowing that there is wind, the plane will use the AHRS data that are realtime calculated based on the gyro, and make a 90° turn to the right and point the nose straight to east along the yellow line. But then the ground course will be on the blue line due to the wind and the plane has to correct for that by turning back to north direction a bit until the ground course is aligned with straight east. The Nose heading will then be on the green line.

Now with the new AHRS updates (thanks a lot @shota3527) we have a lot more data available that does not necessarily need new sensors, but would of course benefit from real sensor data. Now we have reliable wind speed and direction estimation. By that we also have the ability of a virtual airspeed sensor.

With Windspeed, direction and airspeed and groudspeed known, the wind could be fully compensated in realtime. Any target ground course would have a corresponding plane heading that can be calculated with a sine or cosine function.

By this any course change could directly be corrected for the estimated wind-course-error to make much smoother turns with no under- or over-shooting the turn. In my Example above this would mean that a heading setpoint of 90° would result in a corrected heading setpoint of 70°. So INAV would make a 70° turn to the right to achieve a ground course of 90°.

Like in this graphic, any target ground course (green) would have a corresponding heading direction (blue). This error can be calculated. So the course setpoint can be recalculated into the corresponding heading setpoint.

This would have many positive side effects:

• We can have true heading and course heading even with compass disabled (Already part of the new AHRS)
• Compass can be enabled on fixed wing to allow dead reconing navigation without messing with general GPS navigation (see GPS Fix estimation (dead reconing, RTH without GPS fix) for fixed wing #8347) (Solved with Use course over ground as the default for fixed wing navigation "heading" #8529)
• turns in waypoint modes would be much more precise and go straight for the next WP instead of hunting for the course and without getting off-course by wind
• LOITER would be much cleaner and more symmetric (nice oval in strong winds and no kidney-shaped erratic turns when going into the wind
• HUD elements would be correctly aligned as they are based on heading and not ground course

[b14ckyy]

For now, #8529 might be an intermediate solution to keep the current behavior without breaking #8347, but on a long term a navigation upgrade might be sensible though.