Use course over ground as the default for fixed wing navigation "heading"

[breadoven]

Changes fixed wing navigation "heading" to use course over ground rather than raw IMU heading. Ground course is considered a better direction parameter to use in the case of a fixed wing given the navigation "heading" parameter posControl.actualState.yaw represents the direction of travel over the ground rather than the actual aircraft heading.

Change won't in fact make much difference unless a compass is used in which case it should prevent navigation problems in windy conditions where the difference between GPS CoG and IMU heading becomes significant.

Related to #8519.

[shota3527]

Is the reflash rate and latency enough for navigation controllers?
Maybe a test is needed to find out whether it may cause oscillation on the course hold or not

[breadoven]

Updated to add a specific course over ground position estimator variable. This uses GPS CoG directly if the GPS update rate is at least 5Hz otherwise it calculates course over ground based on change in X and Y positions between updates. This tracks GPS CoG very consistently.

Odd thing noticed from testing is the GPS update rate is shown as 10Hz in the configurator GPS tab (U/blox7 used) but debugging shows message update times generally slightly longer than 200ms, i.e. < 5 Hz.

EDIT: Ah, 5Hz update rate is set by HITL ... forgot about that.

[breadoven]

Is the reflash rate and latency enough for navigation controllers? Maybe a test is needed to find out whether it may cause oscillation on the course hold or not

I tested using the HITL sim with wind and turbulence and it tracks accurately, course hold is very steady. Needs flight testing to be sure though.

Calculating course over ground just based on X, Y position change between updates is very accurate, better than I was expecting (this is using the GPS task update rate of 50 Hz). So this method can be used instead if the GPS update rate is too low.

[b14ckyy]

I also did HITL tests just now. Looks good to me so far. No flight tests currently as weather just does not allow test flying.
This is flown with 20kph winds from straight north. WP Tracking is disabled, Turn smoothing set to CUT (old method, not the updated one) Tested with 3d11e10

6.0 FP1, Compass set to NONE. Flies like compass enabled as expected.

This PR with Compass disabled:

This PR with Compass Enabled:

[wilco1967]

Not sure if this is related, but yesterday I made some flights using 6.0.0. FP1 release.

in WP mode, using nav_fw_wp_tracking_accuracy = 5, the plane flew the sections between WP's perfectly.

But when flying in 3d cruise mode, the plane often makes strange uncommanded sudden 'corrections' to it's heading.
Also, the telemetry heading often seems off from the actual course over ground. perhaps 10..20 deg.
It seems like, when the indicated heading deviates too much, it makes a correction.

When supposed to fly a straight line, the actual heading over ground slowly changes, making a very slight curve. then at some point, it makes a sudden correction to come back to the intended header. The general direction is maintained alright, just these occasional corrections (like 20..60 sec apart).

here's a view from google earth of actual track (EdgeTx telemetry). This was supposed to be a straight line.

I think I do have the BB log, if interested (need to pull the SD card).

It also seems to affect RTH mode. when RTH is activated, it goes into the general direction of 'home', but it's not 100% accurate (perhaps 10 deg off, but not consitent). After some time, it makes a sudden correction to where it needs to go. It comes back home fine, but just not in a perfectly straight line.

note the (right) yellow line (yellow = RTH). notice the sudden correction.

The white/grey points are WP mode. This is perfect....

There was hardly any wind during the flight (less then 10 km/h).

I'm guessing, this may be a side effect from the WP tracking option ?

in iNav 5.1, it flew perfectly straigt line in 3d cruise.

The plane has no compass. GPS heading only.

I'll try to get the BB log out, if someone is interested.

[wilco1967]

got the BB log.
But it's to big to upload here.... (35 MB)

let me know if interested, I'll upload it somewhere else.

[breadoven]

WP tracking accuracy doesn't have any affect on Cruise mode or normal RTH only WP mode and RTH Trackback.

I'm guessing the curved Cruise track you're seeing is caused by the new AHRS affecting the heading due to wind compensation. I've noticed the wind estimator struggles when flying straight with the predicted wind drifting consistently which would account for the consistent drift in the Cruise track. Not sure why it seems to reset at regular intervals though, looks like some sort of set point has been triggered. Turning off imu_gps_yaw_windcomp should confirm if it's the AHRS wind compensation that's affecting the heading and therefore ground course.

In fact this is the reason for this PR. Fixed wing navigation really needs to use course over ground to track consistently. Trying to use true heading with wind compensation that is sluggish to respond to actual wind changes or which drifts anyway is going to cause the ground course to drift also.

[b14ckyy]

@wilco1967 can you please open a new issue and yes please also upload the blackbox logs there as a zip.
This is good feedback to optimized stuff with the developer who made the AHRS update

[shota3527]

I have had a look at the position estimator, And I feel like the ground course from the position estimator might be better if the noise level in the accelerometer is low. And the actual position control in multi-rotor also uses the position estimator data.
maybe we can try different INAV_GPS_COG_MAX_UPDATE_TIME_MS

[breadoven]

I have had a look at the position estimator, And I feel like the ground course from the position estimator might be better if the noise level in the accelerometer is low.

Do you mean in the case the GPS update rate is < 5 Hz so ground course is calculated from position change increments ?

And the actual position control in multi-rotor also uses the position estimator data. maybe we can try different INAV_GPS_COG_MAX_UPDATE_TIME_MS

Right now multi-rotor wouldn't use ground course only fixed wing. Are you suggesting using it for MR also ?

[shota3527]

Do you mean in the case the GPS update rate is < 5 Hz so ground course is calculated from position change increments ?

I mean we can always use data from the position estimator regardless GPS update rate or to increase the threshold for like 20 Hz

Right now multi-rotor wouldn't use ground course only fixed wing. Are you suggesting using it for MR also ?

As other part of navigation controllers uses the data from the position estimator for good reason, I suggest the ground course estimation also use data from the position estimator. Data from the position estimator fuses accelerometer ,gyro ,gps velocity and gps position

And, instead of increments in posEstimator.est.pos.x why not using posEstimator.est.vel .

[breadoven]

My reasoning in regards to the update rate is simply that ground course is only being used for fixed wing navigation and therefore doesn't require a particularly high update rate due to the inherent relatively slow heading change response expected of a fixed wing. 10Hz should be fine with 5Hz being a guesstimated minimum limit. Given that any recent GPS will work at 10Hz it seems sensible to just use the output from the GPS directly rather than have INAV process at a higher rate when it doesn't seem to offer any obvious benefit. I'm guessing that > 90% of INAV users install a GPS that will run at 10Hz.

This might be different if you wanted to use ground course for Multi Rotor but that isn't the point of this PR. I think ground course perhaps would be useful for MR navigation also but that would have to be done separately because the affect isn't as obvious as it is for a fixed wing.

Why do you think a higher update rate is required ?

Agree about posEstimator.est.vel, I'll update to use it instead.

[MrD-RC]

Would it be worth setting the default protocol to UBLOX7? I would imagine that pretty much all modules used would support that.

[shota3527]

I think a higher update rate/sensor fusion is required in fixed-wing navigation.
other FW navigation controllers use data from the position estimator, not data directly from GPS module is the proof.

If the GPS data is enough for navigation controllers, why FW navigation controllers use the data from position estimator? perhaps position estimator can achieve better overall performance?
How will eph(Standard deviation of horizontal position error), epv in GPS data affect the ground course estimation/navigation controllers?
Why did old imu.c used the GPS gound-course only for yaw correction, not replacing yaw by GPS gound-course?
These questions suggest that raw GPS data is not enough in general.

Position estimator data for gound-course estimation might be beneficial in agile configurations, poor GPS signal, soft serial GPS. Also eliminating potential small oscillations in the internal state of navigation controllers.

I know the focus of this pr is FW, but I think MR and FW should use as same code as possible.

[breadoven]

I think the position estimator update rates are probably more related to multi rotor use rather than fixed wing. And even if you have a fast agile fixed wing it's unlikely to be using that agility and speed flying in Nav modes.

Also ground course is only used by the Nav functions which won't work if the positional accuracy is no longer trusted so problems with accuracy in regard to ground course don't seem relevant.

I don't have a problem using ground course calculated from positional change only, it tracks GPS CoG smoothly. Just thought INAV could save on a morsel of number crunching by letting the GPS do it instead. Having said that this could be useful for multi rotor so perhaps it's easiest to just opt for a higher update rate ... I'll change this accordingly. Suggest setting the update rate at 20 Hz for now since it'll still only be used for fixed wing as it stands.

[breadoven]

Would it be worth setting the default protocol to UBLOX7? I would imagine that pretty much all modules used would support that.

Do you get a warning if the GPS doesn't support UBLOX7 ?

[b14ckyy]

@breadoven I never saw a warning but I have at least one GPS that does not support UBLOX7 but UBLOX and it seems like INAV does automatically follow back to UBLOX. This GPS (an Old M6N with a fake M7N firmware on it) then runs just at 5Hz.

[shota3527]

@breadoven
Thanks for the effort.
But I have realized beside switching by the update rate, taking GPS signal quality into account is also needed. especially in a banked turn, due to GPS antenna orientation, it may lose some sats and result in a worse deviation in GPS measurements. in these cases, the accumulated acceleration by accelerometer/gyro will be more trustworthy. The number of sat should also be taken into account.

BTW the position estimator has weights between different sensor data, But the weight will not be dynamically adjusted by gps eph or epv. if we can implement a kind of linear Kalman filter approach in the position estimator, we can depend on the position estimator solely.

[breadoven]

@shota3527 I think this is fine to merge as is. I can't say I've ever really noticed any appreciable degradation in GPS positional accuracy caused by manoeuvring, certainly nothing that would have any noticeable affect on fixed wing navigation control. Looking at any recent log you typically have 12+ satellites fixed for the duration of a flight. Also using log positional data to calculate ground course and comparing it to GPS CoG shows the 2 values track each other precisely so I don't think there's any really issue here. Even if the positional accuracy did degrade the worst that might happen is a bit of heading drift but if it was ever that bad the navigation mode would shut off anyway.

However, feel free to improve this if you think it will provide a tangible benefit.

[b14ckyy]

Great. I will compile a master and as soon as I can fly again I will do someWP Mission tests with Compass enabled and the Path tracking updates in place as well.