Complementary filter equation

DB23

Hello!

I have a problem with equation for complementary filter. I'm using MPU 6050 triple axis accelerometer and gyro. I have read the data from the sensor and allready use few equations. But with this equation (kot_final) i have problems. It gives me weird result. I think the problem is with floating numbers. But i cant find the mistake. Or it's something else? Please help me with that:/

my code:

repeat     
     debug.dec(GetRX)
     debug.str(string(", "))
     debug.dec(GetRY)
     debug.str(string(", "))
     debug.dec(GetRZ)
     debug.str(string("   "))
     debug.dec(GetAX)
     debug.str(string(", "))
     debug.dec(GetAY)
     debug.str(string(", "))
     debug.dec(GetAZ)
     debug.tx(13)
     
     fXg := F.FFloat(GetRX)     ''gyro
     fYg := F.FFloat(GetRY)
     fZg := F.FFloat(GetRZ)


     fX := F.FFloat(GetAX)      ''acc
     fY := F.FFloat(GetAY)
     fZ := F.FFloat(GetAZ)
     
     kotY := F.FRound(F.FMul(F.Degrees(F.ATan2(fZ, fY)), 1000.0))
     debug.str(string("kot okrog osi Y= "))
     '''DecPoint(kotY, 1000) 
     debug.dec(kotY)
     debug.tx(13)


     kotX := F.FRound(F.FMul(F.Degrees(F.ATan2(fZ, fX)), 1000.0))
     debug.str(string("kot okrog osi X= "))
     '''DecPoint(kotX, 1000) 
     debug.dec(kotX)
     debug.tx(13)




      ''angle := (0.98)*(angle + gyro*dt) + (0.02)*(x_acc)
        
     kot_final := F.FSub(F.FMul(98.0,F.FSub(kot_final,F.FMul(fXg,1.0))), F.FMul(2.0,kotX))    [SIZE=3] [COLOR=#ff0000]<------Problem with that[/COLOR][/SIZE]


     
     debug.str(string("kot_final= "))
     '''DecPoint(kotX, 1000) 
     debug.dec(kot_final)
     debug.tx(13)

result on serial terminal:

Thank you!

Duane Degn

You're not keeping your floating point numbers and integers separated. You can only perform floating point operations on numbers encoded as floats (and expect a meaningful number).

The dec method will not display a floating point number as expected either. You either need an object to display floats or to convert the float to an integer prior to displaying the number.

I'd suggest you try some simple math with numbers you can check to learn to use the floating point object. Make sure you understand how the "FRound" method works.

HDS

Hello, I've been fighting with my recently acquired MPU-6050 for a few days without any success. Absolutely no problem connecting from an Arduino and absolutely nothing from the propeller. As someone new to i2c and Propeller Spin, I have a lot to learn... Are you able to post your MPU-6050 code or point me to the object you use? I'd love to move on with my project! Thanks in advance for any assistance you can provide.