Tracking angles and the "NAN" problem...
zlantz
Posts: 136
Here we are again, the "NAN" problem and keeping track of angles. This time it is with the gyro angle. Typically, you would track gyro angle by finding the degrees per second on an axis and multiplying by the loop time (dT) to find the rate, then summing this value with a current angle variable over time.
All values leading to the angle tracking function are seemingly correct, but once fed into the tracking function it goes all haywire. The tracked angle vs real angle are extremely different and the drift is 1000x worse than on the P1. I had nearly zero drift and it kept angle great - was in fact difficult to drift but now it drifts constantly and cannot get a good read from it. I have used this code for many years without issue w/P1.
If a number ever results as a "NAN" value on P2, it won't reset to a valid number once available, so I came up with a NANcheck which requires two longs to track one value. It uses the previous non-nan value when it detects a nan value. It is an effective solution, but consumes longs, code space, and is messy. If this is not used, randomly one of the three cAng and/or three gAng values will become NAN and not return to a valid number once a valid number is present and will "crash" the [long].
Its been a rough transition from the P1 to P2. It is extremely fast and reads sensors great but has a hard time with basic functions afterwards and can easily lock out a value with a NAN result. I keep thinking I need to couple the P1 to the P2 to get proper functionality as everything I am having problems with works just fine on the P1 but is extremely problematic or simply won't work on the P2.
This is a fairly advanced use of the MPU9250 as there are 2 sensors, each with their own address, that are setup to blend the data between the two into a third output (MPUc), and are aligned so that each positive axis averages to the opposite negative axis. I have flown many drones with this design and functions flawlessly (P1). The output is also one step behind in time so that it matches the dT calculation for accurate tracking.
Upon rotation, the gAng values become useless and the angle of rotation cannot be tracked to even the slightest degree with the gyro rate output. For verification, DegSecC and gForceC are displayed to show it is functioning properly up to the angle tracking function. MPUc should match the non-inverted MPU output (as it does).
On week 3 of trying to get my P1 code working on the P2. The frustration is strong but I don't want to give up.
Edit: To use this code with only one MPU, change to the following:
aX := Float(aXa) 'Float(lXa)
aY := Float(aYa) 'Float(lYa)
aZ := Float(aZa) 'Float(lZa)
gX := Float(gXa) 'Float(lXg)
gY := Float(gYa) 'Float(lYg)
gZ := Float(gZa) 'Float(lZg)
Comments
NaN propagation is a normal thing that happens on every IEEE compliant floating point implementation.
That said, storing angles as floats isn't really a good idea to begin with. Integers and their wrapping behaviour naturally lend themselves to angle computations and the P2's vector math instructions input and output integer angles.
I don't think zlantz has your same patience to start over from scratch.
From a top level, how quickly do you think the loop calculation loop is running on the P1? How many times a second? How quickly do you think this code is running on the P2.
Where I am headed, is that by relying on a time delta (which isn't a bad approach), there may be a certain sweet spot for these calculations. Calculate too quickly and the less significant bits fall off (despite even using floating point)?
Also considering the update rate of the accelerometer, filters might have turned out to be better matched for the rate the code was running on the P1?
Just some higher level thoughts. What would happen if you stabilize the update loop to a certain minimum time interval? For example by waiting at least 500 uS (or what you feel appropriate)?
Anyways my point is that a few tweaks and measurements could get this code functional on the P2 without a rewrite.
Also, you'll probably find the BinFloat functions (F_Add, F_Sub, and so forth) more stable than the built in functions like +. and -. The P2 interpreter still has some floating point bugs, particularly around the handling of NaN and Inf.
I started with BinFloat and that was where the problem first identified itself. I had then transitioned to P2 float math for simplicity/view-ability, plus it provided the same results just much much faster. I am now very fond of the built in float functions and is a tremendous improvement over the P1.
I have tried adding delays to slow everything down to the same dT as the P1 and it still has the same issue. You can't really get a gyro rate without knowing the difference in time it was moving for as they are relative. If the read time was 1 second it would equal the degrees per second but the read time is much much less than 1 second so it must match that time. It is possible the dT calculation is failing to function accurately, though its a super simple function that has worked for years, hard to see it as the problem. And if it is the problem, then how do you accurately time anything with the P2 via getct() or is it not possible?