Waypoint turn smoothing and tracking accuracy improvements

[breadoven]

Hopefully closes #8513 by improving WP turn smoothing behaviour. Main change is to increase the distance from the waypoint that the turn starts by adding an additional speed related distance based on a 1 second delayed control response time. Although this is just a guestimate of the time required before the plane starts to react to the turn command it seems to result in turns that are closer to what was originally expected. The turn smoothing CUT option has also been changed so it no longer uses a loiter turn. Instead it initiates a normal turn and simply relies on the start point of the turn to control the turn behaviour relative to the waypoint. HITL sim tested but needs further testing to see how useful this is. Max turn angle also increased from 120 to 160 degrees.

PR also improves WP Tracking Accuracy behaviour, reducing overshoot and making the nav_fw_wp_tracking_accuracy setting more effective. Seems to track much better when tested on the HITL sim but needs flight testing to be sure.

[b14ckyy]

Fantastic! Will test after work in the sim and see if I am able to fight test it on the weekend. Thanks a lot fo looking into it again!

[b14ckyy]

I did some HITL tests today and I have to say, this is a MASSIVE improvement. I accidently erased my config on the HITL FC and my backup was somewhat old and not well tuned unfortunately. Just did an autotune again.
In calm conditions it was really really good.

Smothing on with 6/60 tracking

Smoothing CUT with 6/45 tracking

The only problem I had was, when I enabled wind. This flight was on CUT with 6/60 tracking settings and 20kph wind from the north east 320°. The plane had lots of trouble staying on the path and in crosswind it was constantly waving forth and back.
I am not sure if that has something to do with the simulated heading data (compass was disabled and pitot tube enabled) so this needs field testing but maybe you have seen that as well?

Overall a big improvement and I hope the Wind problems are caused by the tune and not by the path tracking having issues. Wind behavior was better on the previous method.

EDIT: Maybe I should try turning off the angle mode bank smoothing to see ifn that makes a difference.

[b14ckyy]

I turned off control smoothness and it shows more what happens. During Path tracking there seems to be a hard transition between a steeper and shallower approach angle and this conflicts with the wind as the plane is pushed out of the threshold distance and then turns in again in a repeated pattern?

https://www.youtube.com/watch?v=5HT0ylNRM1s

[breadoven]

Not sure why it's doing that with wind on. Seems to be turning away from the course line when it hasn't actually reached it yet. It should continue to head toward it before turning to align with it. And it also never actually reaches the course line which I don't remember seeing. Wind certainly made it less stable when I tested with 30+ km/h wind. There was some weaving but it always tracked the course line even with X Plane's weird turbulence turned on. Have to admit I have heading set to use GPS course over ground rather than the IMU heading (assuming HITL simulates this ?). Should reset this back to IMU heading and check again and see if it makes a difference. I was also using Tracking Accuracy settings of 5 and 60 degs, or even 3 worked without too much overshoot and oscillation.

One change I made in this PR was to cut out the tracking accuracy when the plane is within 2m of the course line, it just uses the original method instead, i.e. head directly to the WP, which actually works well once you're on course. So the tracking accuracy method only kicks in to push it back on course when needed. Perhaps this causes the wandering pattern as it switches in and out.

It really needs flight testing though because although the HITL sim is very useful it doesn't entirely replicate reality.

[b14ckyy]

Yeah I agree. When X-Plane Attitude is enabled, the heading is used as if a compass would be active afaik but not sure. With IMU estimation active it SHOULD act like no compass on a real plane with GPS heading. Maybe @RomanLut can confirm.

anyway with the current code I will test in flight as soon as possible and see how it behaves with hopefully some wind.

Turn smoothing is perfect now. even with wind. with some fine tune in the navigation bank angle and control smoothness this gives awesome results.

[breadoven]

I tested again with IMU heading rather than GPS course over ground used as the aircraft heading (actually the value used for posControl.actualState.yaw) and with wind it behaves the same way as your tests @b14ckyy. So it seems the difference between your wind results and my previous wind test results, which accurately tracked along the course line, is due to the heading source used.

Aircraft heading isn't the same as course over ground when wind is present. You can see in HITL that the aircraft is obviously crabbing to counter wind drift so the nose is pointing in a different direction to the course over ground. INAV navigation should use course over ground to work as intended rather than aircraft heading, certainly as far as posControl.actualState.yaw is concerned at least. Having said that I thought GPS CoG is the main component used for the IMU heading so don't know if this is a HITL affect or maybe to do with the new AHRS.

Not sure what the solution is, either hard code the use of GPS CoG for the posControl.actualState.yaw value used for fixed wing or make it optional (just in case you want inaccurate flight paths flying in windy conditions).

[b14ckyy]

Yeah that's what I meant that I was not sure how the HITL sim interpolates the heading in sensor estimation mode so I just did a quick test.
Compass is disabled in INAV, Sensor estimation active. INAV Heading says 180° but the actual course is more like 165-170°. So in the HITL inav behaves the same as if it had a compass enabled and flies by heading and not GPS bearing. So this behavior should not happen when flying in reality with no compass.

Unfortunately this will be an issue in the future if someone wants to use Dead Reckoning navigation, because then Compass is needed. But this will mess up the navigation.

Let me test with compass enabled in the HITL. I assume the updated AHRS should be aware if there is a heading/bearing discrepancy as this is also needed for dead reckoning navigation. Maybe having compass disabled but still getting a compass-like heading from the HITL really confuses the navigation system.

EDIT: nope it is the same erratic behavior.

[b14ckyy]

I think I see 2 (actually 3) options that we have.

Option 1: Hardcode to only use GPS bearing for navigation and ignore compass when turned on (or use compass only for OSD heading indicator and things like radar) but then you have to make sure we do not break #8347 if it gets implemented as well. But this would be just a band aid

Option 2: You set the Path tracking back to the previous method and we live with the S-turn alignment for now. This method just had a offset from the path but consistent flight

Or the 3rd option: a proper redesign of the navigation controller in general to also take wind speed and direction into account for direction changes and compensate for bearing/heading offset in any direction. I think this was also part of #8407 as during these tests INAV did always perfect turns even with crosswinds. THis AHRS was able to pre-estimate the ground course after a turn of x degrees based on wind speed and direction. This also created nearly perfect circles in loiter and very nice turns in crosswinds.

[breadoven]

I think as things stand now it makes sense to just use GPS course over ground for fixed wing navigation "heading" given this is really the direction expected for posControl.actualState.yaw rather than the direction the nose is pointing. Also navigation during WP mode is obviously dependent on GPS position data to work anyway so using the GPS derived heading makes the navigation position data consistent.

With this change GPS CoG is only used when reliable otherwise it defaults back to the current IMU heading. So the change shouldn't affect things if a compass is available and the GPS has failed or airspeed is low, it'll just revert to using the IMU compass derived heading. It also prevents a compass from affecting navigation if the GPS is working. The OSD heading uses the IMU derived heading which is fine, shows the direction the nose is pointing as it should. I'll do it as a separate PR since it also affects the other Nav functions as well. Test it and see.

[breadoven]

I tested again with a change to the HITL MSP which set the IMU yaw (heading) to GPS course over ground rather than the yaw value output by the simulator. I also set posControl.actualState.yaw to use IMU heading rather than GPS CoG and the erratic HITL behaviour with wind present is gone. So the HITL behaviour is just down to the fact INAV is receiving X-Plane's true aircraft heading value which is significantly different to the GPS CoG when wind drift is present.

I also checked some old flight test OSD DVR recordings that show GPS CoG debug info and OSD heading and it's obvious there is little difference between the 2 values, they only really diverge briefly during manoeuvres (no compass used). Not surprising given the main input to the IMU heading is GPS CoG in the absence of a compass. In fact having a compass present only seems to cause problems according to issues such as #8503. This in itself is probably a good reason to hard code use of GPS CoG for fixed wing to avoid these problems.

[b14ckyy]

that's great news. I guess it's worth a PR in the HITL github as well? Or was the change on INAV side only?
We should also check to make sure we don't break the G-Force compensation and #8347

I'll run a HITL test as well and send the build to one of my testers in Hawaii who has always summer :D

[RomanLut]

As far as I know, attitude.values.yaw is meant to be true heading.
The fact that INAV can not estimate true heading without compass, does not change this definition. It only tells that estimated heading may be wrong.

In fact, with current attitude estimation code,
no compass => attitude.values.yaw = COG (wrong estimation, but the best what we can do)
with compas => attitude.values.yaw = compass heading (true heading)

So, navigation code should use proper source.
If COG is required by navigation code, than it has to use COG from gps data, not the heading from attitude.
Naviagation code should not assume that attutude heading is COG.

Setting attitude to "wrong" heading (COG) is not a solution to a problem. Because problem will reappear if MAG is enabled.

[RomanLut]

p.s." Attitude -> Copy from X-Plane" mode in simulator does exactly what name tells: it copies true heading to attitude.values regardles enabled/present sensors. In this mode, attitude estimation code should be disabled in INAV ( in fact, final code which writes to attitude.values is disabled in current implementation).

[breadoven]

As far as I know, attitude.values.yaw is meant to be true heading. The fact that INAV can not estimate true heading without compass, does not change this definition. It only tells that estimated heading may be wrong.

In fact, with current attitude estimation code, no compass => attitude.values.yaw = COG (wrong estimation, but the best what we can do) with compas => attitude.values.yaw = compass heading (true heading)

So, navigation code should use proper source. If COG is required by navigation code, than it has to use COG from gps data, not the heading from attitude. Naviagation code should not assume that attutude heading is COG.

Setting attitude to "wrong" heading (COG) is not a solution to a problem. Because problem will reappear if MAG is enabled.

This is how I see it also.

attitude.values.yaw should always be the IMU heading as determined by the IMU based on available sensors. It will only provide an accurate "true" heading if a compass is available otherwise it just provides essentially flight course based on GPS CoG. This is what is shown in the OSD heading field.

posControl.actualState.yaw used by the navigation code as a heading input only appears to work as intended if it is the flight course rather than aircraft true heading so should use GPS CoG even if a compass is available. This is best handled in the INAV navigation position estimate code, see #8529.

So there is no need to change the INAV HITL MSP code, it's correct as it is. It's just the INAV navigation position estimation that needs changing. This will work with the HITL sim using the simulated GPS CoG.

[shota3527]

As far as I know, attitude.values.yaw is meant to be true heading. The fact that INAV can not estimate true heading without compass, does not change this definition. It only tells that estimated heading may be wrong.

In fact, with current attitude estimation code, no compass => attitude.values.yaw = COG (wrong estimation, but the best what we can do) with compas => attitude.values.yaw = compass heading (true heading)

So, navigation code should use proper source. If COG is required by navigation code, than it has to use COG from gps data, not the heading from attitude. Naviagation code should not assume that attutude heading is COG.

Setting attitude to "wrong" heading (COG) is not a solution to a problem. Because problem will reappear if MAG is enabled.

This has been changed in the new AHRS.
no compass => attitude.values.yaw = COG with wind compensation(true heading/the direction of the nose)
with compas => attitude.values.yaw = compass heading (true heading)
True heading is needed for ahrs g force compensation by gps speed vector(VELNED). So attitude.values.yaw`s coordinates is hopefully alined with GPS coordinates even with wind exists

I think the reflash rate/latency of gpsSol.groundCourse might be insufficient for navigation, so may be we should calculate groundCourse with gyro data somewhere, maybe in imu.c?

[b14ckyy]

instead of further taking over this PR further, I opened a discussion thread based on what I just discussed with @shota3527
#8533

[breadoven]

I think the reflash rate/latency of gpsSol.groundCourse might be insufficient for navigation, so may be we should calculate groundCourse with gyro data somewhere, maybe in imu.c?

Seems the GPS update rate is typically 10 Hz which should be OK for fixed wing navigation although it might be as low as 1 Hz which would be too slow. So it would make sense to output ground course from the IMU based I guess mainly on GPS CoG together with gyro input as required to provide a useable update rate in all cases. Better to have this from the IMU rather than using it directly from the GPS output.

[RomanLut]

FYI GPS update rate in HITL is set to 5Hz.

[b14ckyy]

Guys I assume we can merge this change now? The GPS Heading topic is separate anyway and at the end it should work no matter what.

[breadoven]

Guys I assume we can merge this change now? The GPS Heading topic is separate anyway and at the end it should work no matter what.

I think it's much better behaved than the initial version so I don't see any problem merging. I did test it the other day in a brief window of low wind/no rain but it was getting dark so I couldn't do much. It behaved as expected although I think the tracking accuracy setting could have been set lower than 5, possibly as low as 3 on a T tail fixed wing. On 5 it was slow to align after the turns which is good because it means the overshoot issue is now better controlled. Setting it lower should speed up the alignment. The turn smoothing also seemed to work better now it starts earlier.

Switching off imu_gps_yaw_windcomp should avoid wind related issues at least until ground course is sorted out.

[b14ckyy]

I agree. I also did one quick test in real flight but with defaults 6 and 45°. I think we should keep that as default. In the sim everything below 5 caused issues with tailless wings. Also we should not only consider perfectly tuned planes. Better have a safe value than a sharp setting that can cause trouble.