Improvements to BARO_MSP

[ricardodeazambuja]

BARO_MSP would keep reporting good health even without ever receiving a MSP2_SENSOR_BAROMETER message because the way barometer.c was designed it didn't take into account a sensor, like the one in barometer_msp.c, that was not always connected to the FC. Now it will behave much more like the other sensors using MSP2. It should solve the problems people are having like in #7576, #8224.

[JulioCesarMatias]

An effort was also made here >> #8233

[ricardodeazambuja]

@JulioCesarMatias I will have a look at your code asap!

[ricardodeazambuja]

Shall we merge this PR and leave the more complex improvements for future PRs?

[DzikuVx]

@ricardodeazambuja I'd love to test it first but have very limited time at the moment. Let's try with 6.1

[MrD-RC]

Something in the changes to navigation_pos_estimator.c has caused issues for a lot of airplanes. Lots of people are reporting dolphining issues in 6.1 in modes with althold (cruise and rth for example). Reverting the changes this PR made to navigation_pos_estimator.c fixed the issue.

This PR was supposed to be for MSP Baro. However it has had a negative effect on regular baros in INAV. I think fixing this issue, and a 6.1.1 bug fix would be advantageous. If we go for a 6.1.1, this PR could be included too #9076

[MrD-RC]

For more info. I’ve also had a report that quads also have the issue, where althold is not working as it should. It seems like the new filter may be causing the issue, as that part is platform independent.

[M0j0Risin]

Just to add to this, it's my belief that the crux of the issue is that it's now entering that IF block on line 610 whereas before it was an ELSE IF. There was some logic added to create a boolean "correctionCalculated" which is being used as a return for the function. But it seems logical the intention may have been to also use it to bypass the block starting on line 610 if it were already completed by the previous logic.

I'm not sure I fully understand what's happening with that call to pt1FilterInit() on line 607, but I assume it must be important to these MSP baro improvements.

[ricardodeazambuja]

@MrD-RC, I am trying to understand where the source of the problem is. Is it the call to reset the filter pt1FilterInit or by not having the old else if... or both?

[M0j0Risin]

@ricardodeazambuja I can't speak for Darren and I can't speak to what's happening on multirotors. I can't even be certain it's the same issue with multirotors. I can only speak to what I've seen on fixed wing models.

I'm definitely not a seasoned developer on this project, so some things don't make complete sense to me. But bear with me while I try to explain what I'm seeing.

Most fixed wing flight controllers do have a barometer. So this EST_BARO_VALID flag is likely always set and they fall into the block on line 561 which does the business of setting the estimation based on barometer and some GPS data if available. In 6.0 that would have been it. But now fixed wing models will also fall into the block on 610 in each iteration, simply by virtue of STATE(FIXED_WING_LEGACY) which then starts doing the estimation again based on GPS info alone. I believe it's this doubling up of operations against the estimationContext that is causing the problem for fixed wing models. Some users have reported that disabling the barometer "fixed" this issue for them, which lends credence to this collision between these two sections of code being the problem.

So, it seems to me that one solution would be to wrap the whole IF on 610 in something like "if (!correctionCalculated) { }" to stop that block from being entered if the correction was already calculated in the previous block.

Then the question would really become, in which case does it ever fall into the block calling pt1FilterInit()? Or is this call really necessary? I assume it would only be for craft without a barometer or with a malfunctioning barometer where EST_BARO_VALID is never set. In which case that doesn't seem to be effecting fixed wing models with disabled barometers. Now whether that causes some other issue for multirotors when EST_BARO_VALID isn't set is definitely above my pay grade.

[ricardodeazambuja]

Now that I looked at the code again, even the "pt1FilterInit" doesn't seem to be needed for the BARO_MSP in any normal use because it is always initialized. I think I introduced this bug during initial tests of the BARO_MSP (on my desk) when the FC was powered, and the BARO_MSP wasn't up and running yet or the hardware generating the MSP messages slowed down for some reason, still it should not call "pt1FilterInit" mid-flight in any situation.

[M0j0Risin]

@ricardodeazambuja So it sounds like simply reverting this back to the 6.0 version might be the "easiest" solution. That's basically what I did in my tests today and at least I can say it fixed it for my fixed wing models.

[M0j0Risin]

@ricardodeazambuja Actually, I say that, but I did have to make one change which was on line 449 of the 6.0 version. Basically to account for the new simulator mode arming flags as they've been broken out for HITL and SITL

if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {

[ricardodeazambuja]

Maybe we should also keep this line posEstimator.baro.epv = positionEstimationConfig()->max_eph_epv; because if the baro goes bad it is nice to set the uncertainty to a big value, but I may be exaggerating.

[M0j0Risin]

@ricardodeazambuja I definitely think you guys will know better than I on that one. I went ahead and added that line

inav/src/main/navigation/navigation_pos_estimator.c

Line 330 in d6ce6e7

posEstimator.baro.epv = positionEstimationConfig()->max_eph_epv;

back into my test version.

I'd forgotten I'd also reverted the minor change in

inav/src/main/sensors/barometer.c

Line 345 in d6ce6e7

return sensors(SENSOR_BARO);

so I've set that back to how you have it as well.

I'll try to get out for some more test flights as soon as I can, but it might not be until later this week or the weekend.

[MrD-RC]

When you make the changes to fix this, please can you base the code on the https://github.com/iNavFlight/inav/tree/release_6.1.1 branch please.

[M0j0Risin]

@MrD-RC I'm not sure if anyone else is actively working this, but I went ahead and created a PR with what I've tested to be good. It's basically just reverting this file to the 6.0 version and adding back in the lines to account for the arming flags related to HITL and SITL.

[MrD-RC]

@M0j0Risin Were the HITL and SITL lines in 6.1?

I’ve added the 6.1.1 milestone to #9111 If @ricardodeazambuja could check that PR too. Once you’re both happy it’s working. We can get it merged 👍🏻

[M0j0Risin]

@MrD-RC Yeah, in 6.0 there was just a SIMULATOR_MODE arming flag. In 6.1 they broke those out as SIMULATOR_MODE_HITL & SIMULATOR_MODE_SITL. So any lines that were referencing just SIMULATOR_MODE would cause a compiler error. So those minor corrections had to be made to make it compatible with 6.1.

https://github.com/iNavFlight/inav/blob/6ea39d48ef2b08b847294e4a079ff2d20f7872d9/src/main/fc/runtime_config.h#L24C2-L25

The only thing I didn't add is this line that @ricardodeazambuja proposed. I just have no way to test that. Since it wasn't there in 6.0 I opted to just go with what we know works. But I'm happy to include it if you guys think it's a good idea.

inav/src/main/navigation/navigation_pos_estimator.c

Line 330 in d6ce6e7

posEstimator.baro.epv = positionEstimationConfig()->max_eph_epv;

[MrD-RC]

With regards to that evp line. I think if the baro goes bad. It would be better to handle that appropriately. Rather than set a value high.

[ricardodeazambuja]

I don't think this line is really needed. I wrote that out of precaution because I thought it was a good idea to make sure to indicate the uncertainty was very high, still it could backfire since max_eph_epv is set by the user.

inav/src/main/navigation/navigation_pos_estimator.c

Line 330 in d6ce6e7

posEstimator.baro.epv = positionEstimationConfig()->max_eph_epv;