BinFloat Issue (Atan2 ?)

zlantz · 2021-12-27 03:22

I recently wrote a MPU6050 driver for spin2 and get great data from it, however, whenever I go to calculate the Pitch & Roll angles from the gForce, it returns unexpected values.

When I compare the code in Visual Basic it gives the correct angles so I wrote a simple example spin2 program with hard coded values to calculate the angles laying flat on a table. It was expected that a gforce [X,Y,Z] of [0,0,1] would return 0 deg Pitch and 0 deg Roll, however the output value for each was 14.323943 degrees.

Both Pitch and Roll use the Atan2 function and it would seem the issue is there. This project uses a v2 of BinFloat acquired via another post as the OBEX version was out-dated and will not compile without modifications.

Also sending -1.0 as a float with BinFloat to serial comes out as -4. A few issues reside in this module.

Phil Pilgrim (PhiPi) · 2021-12-27 04:43

You obviously live in one of those places known as a Mystery Spot, where gravitational/inertial anomalies play tricks on the unsuspecting. Should you ever quit your day job, you could make a fortune attracting tourists to your place.

-Phil

pik33 · 2021-12-27 08:17

As I can see you are mixing Spin2 native floats with the library written earlier than these floats was added to Spin2. This may cause problems.
The atan2 function in the library has a "work in progress" comment in it.
The function uses CORDIC module, which operates on 32bit integer values. This, along with single precision float to fixed point conversion, can cause precision problems when the angles are near n*90 degrees. Maybe someone can give a link to the topic where it is described and worked around.

zlantz · 2021-12-27 15:34

@pik33 said:
As I can see you are mixing Spin2 native floats with the library written earlier than these floats was added to Spin2. This may cause problems.
The atan2 function in the library has a "work in progress" comment in it.
The function uses CORDIC module, which operates on 32bit integer values. This, along with single precision float to fixed point conversion, can cause precision problems when the angles are near n*90 degrees. Maybe someone can give a link to the topic where it is described and worked around.

I have tried it both ways, I only used spin2 math to simplify the function visually for the example. I have also tried this with the xypol function as shown in the isp_click_mpu_9dof.spin2 library as an example for the MPU-9250. It returns 90 degrees for an expected 0 degree input. There is very little description as to how it works and no mention as to if it needs to be float or dec so I tried both and get the same result either way.

Without the ability to calculate roll and pitch angles, it would seem there is a need to revert back to the P1 for sensor input then feed that into the P2 for further calculations? Not really what I had in mind for an upgrade.

This chip is blazing fast and love seeing it cycle over 500x a second! The P1 never got past 50x per second with my functions.

Edit: I kind of got the xypol function to work, you have to shift the output around and re-scale it before it can be utilized. xypol requires decimal input. It would appear one function is square root and the other performs the atan2.

' // Round Complementary Filter Output for Angle Calculation
CtrlX := fRound(cAngX)
CtrlY := fRound(cAngY)
CtrlZ := fRound(cAngZ)

' // Pitch Calculation (+/-90 Deg)
hypotYZ, _ := xypol(CtrlY, CtrlZ)
_, anglePitch := xypol(CtrlX, hypotYZ)
currPitch := anglePitch +/ kONE_DEGREE

' // Roll Calculation (+/-180 Deg)
_, angleRoll := xypol(CtrlY, CtrlZ)
currRoll := angleRoll +/ kONE_DEGREE

' // Correct for currRoll Offset
Roll := currRoll + 90
if (Roll < 0)
  Roll := Roll + 360
elseif (Roll > 359)
  Roll := Roll - 360
Roll := ScaleValueLong(Roll, 0, 359, -180, 180)

' // Correct for currPitch Offset
Pitch := currPitch - 90

Pri ScaleValueLong(scaleVal, fromMin, fromMax, toMin, toMax) : retVal
    retVal := (scaleVal - fromMin) * (toMax - toMin) / (fromMax - fromMin) + toMin

con
    kONE_DEGREE = 1 frac 360

ersmith · 2021-12-27 17:52

BinFloat's trig routines can work in radians, degrees, or "fractions of a circle"; the default is "fractions of a circle", which is what the CORDIC uses. To get radians you'll have to use f.SetRadians() before calling f.atan2.

Also, as noted in the BinFloat docs, the call is f.atan2(X, Y), whereas some languages (e.g. BASIC or C) use atan2(Y, X). I think BinFloat's order is for compatibility with other Spin2 trig packages.

Mike Green · 2022-01-08 21:55

When warnings are turned on, BinFloat 1052 warns "nopoint is possibly used before initialization"

ersmith · 2022-01-09 01:07

@"Mike Green" said:
When warnings are turned on, BinFloat 1052 warns "nopoint is possibly used before initialization"

Whoops, that's bad! It should be initialized to 1 somewhere at the top of the function.

BinFloat Issue (Atan2 ?)

Comments