inav
inav copied to clipboard
iNavFlight
Fixed althold issue in surface mode for multirotors
Some people, including me, were experiencing problems with maintaining altitude in surface mode using mtf-01 sensor.
It seems that the problem was caused by high vibration levels which decreased the accelerometer weight down to 0.3, which is the minimum. But then, this value is squared in navigation_pos_estimator_agl.c, which means it goes down to 0.3*0.3=0.09 which is so low that the accelerometer almost becomes unused. Therefore, it was only using the rangefinder measurements but they have so much delay that the drone becomes unable to maintain a good altitude.
https://github.com/iNavFlight/inav/blob/b5e8b2bf6817268e04086d1cf8a190e794ea6d1c/src/main/navigation/navigation_pos_estimator.c#L360
https://github.com/iNavFlight/inav/blob/b5e8b2bf6817268e04086d1cf8a190e794ea6d1c/src/main/navigation/navigation_pos_estimator_agl.c#L153
I fixed this issue primarily by disabling the dynamic accelerometer weight and setting it to fixed 1.0 ( which is the value it should get when there are normal vibration levels )
I added a new setting to do that, acc_weight. By default, it is set to zero and that means that its value is determined automatically (as normal). But you can set it to 1.0 and that means the accelerometer weight will always be 1.0.
I particularly tested this version and works well.
Surface mode with 0.3 accelerometer weight:
Surface mode with 1.0 accelerometer weight:
@breadoven what do you think about this one?
Didn't have too good a look at it so can't really be sure at the moment.
I can guess that this is not the main issue with rangefinder because GNSS is much slower, but work fine. For my main reason is sensor fusion. It kick off sensor from calculation if it is far enough from estimated position. For baro and GNSS it is natural to drift. A couple of times surface mode work fine on my setup immediately after take off, but nether after some flight. I would make the sensors unequal for position estimation and use the main sensor if it is available and the others as needed. This is already done with baro and GNSS For velocity estimation fusion work fine.
I had literally given up hope on getting my MTF-01P working properly. So really hope this is somehow related.
I can guess that this is not the main issue with rangefinder because GNSS is much slower, but work fine.
@and-sh Correct. I wrote about the main related issue here https://github.com/iNavFlight/inav/issues/10567
@mmosca Besides some conflicts that need resolving. Is there any reason why this fix for multiple issues hasn't been added to the 8.1.0 milestone?
There are unresolved reviews and conflicts. There also seems to be more fundamental discussions.
There also seems to be more fundamental discussions.
What fundamental discussions are those? The basic issues have been explained by the contributor and myself.
However I do thank you for the quick reply. While the rest of the development team appears to have ignored this fix for months. This maybe why the contributor has abandoned the pull request and just uses the fix in his own development build. Its a hard ask for a person to add something for the good of the community, when that work is ignored for an extended period.
To do something, you need a person who is interested in doing it. You just need to fly and correct errors. For now, I think that RTK is more promising, at least for open space. If you are really interested, find a programmer and do it together. The code is quite simple, so a super professional is generally not needed. In reality, the entire althold code will fit on a couple of A4 pages. The problem is that few programmers fly.
@breadoven Could you please find some time to review this pull request, so the conflicts can be resolved and it can be added to the 8.1.0 milestone. The issues this will fix are still prevalent.
Seems I might have an interest in looking at this or general issues related to contradictions between estimated Z position and Z velocities after crashing a quad yesterday. The crash was probably due to not recalibrating the compass after some changes and although it seemed to hold position fine initially after changing heading by > 90 degrees it started toilet bowling. However, after hitting RTH (probably not a great idea) the drone ended up trying to land and although the estimated Z velocity was showing -0.5 m/s the Baro and GPS had it climbing at around 2 m/s with the estimated Z position also increasing.
It's clearly nonsense to have a complete contradiction between the estimated Z velocity and the Z position. The Z velocity error might have been caused by the fact the Z acc was constantly less than 1 which is wrong if you're climbing. Should either be > 1 if accelerating up or 1 if the climb rate is constant. Not sure why the Z Acc was wrong, maybe related to the temperature compensation being used. Either way the lack of consistency between Z position and Velocity needs resolving in some way given it seems to cause problems far to often. There's enough information from Baro and GPS sensors to know if something doesn't make sense and correct the estimations accordingly, just a question of working out how best to do it.
Having looked at this PR I can't help but think it would be better if the new setting was just applicable for use in Surface mode rather than be applied in all cases. Dynamically reducing Acc weight when there's clipping and vibration is intended to prevent moon shot problems. With this change as it is you lose that protection in all cases if you use the new setting to get Surface to work which doesn't make sense. Limit it to Surface mode only to fix issues with sluggish sensors and it at least avoids problems when using other Nav modes.
Checking the rest of the position estimation code for Z axis there seem to be a lot of changes over time that don't obviously make sense including code that could have been tidied up better when changes were made. Needs more checking though.
As to the problem I had above I can't work out why the Acc Z was consistently < 1g even though the quad was climbing at 6m/s at times. I can only assume it was Acc Z biasing that got screwed up during the toilet bowling. It did start to recover when the quad settled down trying to land but then decided to disarm thinking it had landed (it settled down because the landing heading was accurate so no toilet bowling issues). The low Acc Z was the main reason why it thought it was descending with the Z vel corrections having insufficient affect to counter the low Acc Z measurements. The Z vel corrections need fixing, they don't work as they are. Also found some debugs in the log that showed the temperature compensation for the accelerometer was working OK so that had nothing to do with it. And a new compass seems to have fixed the heading problems. The old BN-880 just wouldn't hold a consistent heading through the full 360 degrees no matter how much fiddling you tried.
This is rather strange. I have no clipping even using 8g range of ACC for precision.
It was about -15C then. I've been studying the aldhold code for a while. I didn't see any serious errors. It is important to understand that the rangefinder is always used if its readings are reliable. So there is no need to turn on the surface mode, it only complicates everything. First, you need to ensure that the drone's behavior does not change when the rangefinder is connected. I guess there is a problem with the rangefinder pos and vel weights.
@breadoven did some changes in #10818 which may be related.
I do think this PR is on to something. I'm not sure if the changes in #10818 fixed it? Or if the addtional understanding since then might shed more light, so we get the best fix possible?
