Improved wind estimator validity check

[breadoven]

Should provide a more realistic method for determining wind estimator validity. The current method flags the wind estimate as valid with only 1 estimate calculation occurring which is nowhere near enough for an accurate wind estimation. Also the existing method used to flag an invalid estimate can take up to 15 minutes to trigger which seems excessive and not very practical. It should be noted that the existing method hasn't been removed by this PR, the new method simply complements it.

The method used by this PR is based on a scoring system. Each new estimate calculation adds to the score and each 10s timeout without a new estimate calculation decrements from the score.

In order to avoid an excessive time delay achieving a valid estimate the filtering has been relaxed to improve response. This has necessitated additional wind estimate "spike" elimination to ensure the filtering isn't excessively affected by large spikes that can occur in the wind calculations (can be multiples of the current estimate wind speed). Spike elimination just ignores wind estimate calculations that are more than 3m/s from the current estimate. A forced update is also used to ensure the estimate doesn't get stuck if a large mismatch between the wind calculations and the current estimate should occur. A forced update occurs the next time an estimate is calculated after the estimate update has timed out.

The estimate is considered valid with a score > WINDESTIMATOR_VALIDITY_THRESHOLD with max score limited to WINDESTIMATOR_VALIDITY_THRESHOLD + 15. WINDESTIMATOR_VALIDITY_THRESHOLD has been set to 15 which appears to be the number of estimate calculations required to get a reasonable estimate based on current filtering. The estimate is considered invalid with a score of 0. This means a valid estimate will take around 2.5 to 5 minutes to become invalid if no new estimate calculations occur.

One issue with this is that it takes a little longer to get a valid wind estimate but that seems better than having an estimate that is totally inaccurate flagged as valid.

Works as expected using HITL..

[github-actions]

Test firmware build ready — commit 24fdafa

Download firmware for PR #11614

238 targets built. Find your board's .hex file by name on that page (e.g. MATEKF405SE.hex). Files are individually downloadable — no GitHub login required.

Development build for testing only. Use Full Chip Erase when flashing.

[Jetrell]

One issue with this is that it takes longer to get a valid wind estimate but that seems better than having an estimate that is totally inaccurate flagged as valid.

From your HITL test. How much longer does it take approximately ?
I ask due to the way APA PID attenuation works with virtual airspeed. If the user has their gains tuned tighter and they launch into a strong headwind. This can lead to oscillations until the estimate is correct.
I have experienced this in one case. The oscillations only lasted for a couple of seconds after launch until the reading was valid.
But if it takes too much longer to get a valid estimation. The airspeed will have increase significantly, making it even worse.

[breadoven]

It seems to take > 30s to achieve a valid estimation flying in a loiter hold. Flying with less positional change would take longer. In Cruise the estimation doesn't update at all, it just degrades. Currently it's flagged as valid after a single estimate update even though the estimate is nonsense ... which isn't helpful.

[Jetrell]

I was going to test this today. But I wanted a bit more insight.

It should be noted that the existing method hasn't been removed by this PR, the new method simply complements it.

Being that the current wind estimator implementation appears to provide some form of reading in a sort period of time after the first turn, as inaccurate as it maybe. Yet never the less, a 50% less accurate read is better than no read at all.
Does the scoring method still allow this inaccurate data from the original method to act as a first-read ? So the estimator output still has something to feed to features that are using this data.. Or does it just provide nothing for that first 30sec. Or the following 30sec after the data may become untrusted inflight ?

[breadoven]

I was going to test this today. But I wanted a bit more insight.

It should be noted that the existing method hasn't been removed by this PR, the new method simply complements it.

Being that the current wind estimator implementation appears to provide some form of reading in a sort period of time after the first turn, as inaccurate as it maybe. Yet never the less, a 50% less accurate read is better than no read at all. Does the scoring method still allow this inaccurate data from the original method to act as a first-read ? So the estimator output still has something to feed to features that are using this data.. Or does it just provide nothing for that first 30sec. Or the following 30sec after the data may become untrusted inflight ?

The wind estimate will still display the current estimate in the OSD but it'll be flagged with an * to show it's not valid. As for the things that depend on the wind estimate it depends on whether you're also using the Auto speed PR. If you are then this PR will prevent the virtual pitot from working for the things using pitotValidForAirspeed until a valid wind estimate is available. So this affects Auto speed mode and fixed wing TPA factor although it just means Auto speed will fall back to using ground speed and TPA will use throttle based TPA rather than airspeed based. Other parts of the code check pitot validity differently (just sensor present and updating) so won't be affected. This inconsistency probably needs fixing, I guess pitotValidForAirspeed was added later but not applied to older code.

I should probably move the wind estimate validity check I added to the Auto speed PR to this PR actually to avoid confusing things. But this PR followed on from the Auto speed one when I realised that the virtual pitot validity test didn't seem very robust because it ignored wind estimate which is fundamental to the virtual pitot working correctly.

[breadoven]

I updated this with some changes to deal with an issue related to large spikes that can occur with the wind calculations. The spikes can be multiples of the actual windspeed so need to be ignored/filtered out. The current method just used a fairly severe level of filtering to deal with the spikes but that makes the estimate slow to update especially during the initial estimate after launch on a windy day.

It seems better to just ignore the spikes and wait for cleaner input to add to the existing estimate. The latest change filters out the spikes by only allowing new wind updates to be added to the estimate if the wind is within 3 m/s of the estimate. The existing filter used for the estimate itself has also been relaxed by using an Alpha of 0.1 rather than the 0.02 used before. This reduces the update lag in the estimate but doesn't seem to cause instability/inaccuracy because there are no large spikes to deal with any more.

Just in case the estimate gets stuck updating because of an excessive mismatch between the wind calculation and estimate (> 3 m/s) a forced update is used to realign the estimate. The forced update is triggered by a timeout in the estimate update.

The other change made forces the wind estimate to update for the first 15 wind calculations after initial launch without any spike filtering. This is intended to get a rough estimate quickly. The number of updates required for the estimate to be considered valid has been reduced to 15 in line with this so the estimate should get close to a useful valid value very quickly after launch. After the first 15 calculations the spike filtering is used to better refine the estimate.

This all seems to work OK in HITL but needs double checking from real flight to ensure that firstly it updates regularly enough and also that the output is still relatively stable.

[Jetrell]

I ran a test with this too today by itself, without any other new builds as part of it.

It was hard to say if it was any better or not. The wind direction and speed seemed typical from the ground based measurements at the flight location. i.e. wind was between 7 - 15km/h from the East to NorthEast.

However during one of the loops I performed. The wind direction indicator flipped 180degs. And the virtual airspeed value appeared to overflow, when it jumped to 1037km/h. Then return to normal. Something I can't say I have seen happen before... It also messed-up the OSD airspeed element.
You can see this incident at log time index 05:21.458.
It took another full 180deg turn to get the wind direction indicator pointing correctly. i.e, pointing to the west, showing the wind was blowing from the east.

I'll DM some logs to you. Both for this PR and #11646... And also the Auto speed mode.

[breadoven]

Not sure what was causing the problem if the test only used this PR. However, one noticeable thing is the log Wind(0) went to 0 shortly before the airspeed spike which you wouldn't expect given it typically floats around 0 + and -. I guess it could have just happened to be exactly 0 but it's more likely it's a logging limitation and in fact it overflowed in the log so was probably in fact a large number. The log is limited to unsigned 16 bit.

Anyway I've made some changes to this (commit pending) because I noticed a flaw in the logic of using windLength to filter out spikes. wingLength can appear to be within the change threshold to the last estimate when in fact 1 of the 3 wind components it's calculated from isn't, e.g. wind(x) can flip from -15 to +15 (a large change of 30) which windLength misses because it's calculated from the squares of the wind components. I've also changed the logic so all large spikes are filtered, something the last commit didn't do in some instances for a few updates only because I thought the other basic filter would limit their affect. Seems that's not the case, you can get very large spikes which will completely mess up the estimate from a single update.