Shop Learn
BinFloat Issue (Atan2 ?) — Parallax Forums

BinFloat Issue (Atan2 ?)

zlantzzlantz Posts: 136
edited 2021-12-27 04:19 in Propeller 2

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.

Comments

  • 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

  • pik33pik33 Posts: 1,226

    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.

  • zlantzzlantz Posts: 136
    edited 2021-12-27 17:36

    @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
    
  • 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.

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

  • ersmithersmith Posts: 5,193

    @"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.

Sign In or Register to comment.