Code:
' X = (2 * qYF * qWF) - (2 * qXF * qZF)
I2CWRITE Fpu_SDA, Fpu_SCL, Fpu_ID, 0, [SELECTA, X, LEFT, FSETI, 2, FMUL, qYF,_
FMUL, qWF, RIGHT, FSET0, LEFT, FSETI, 2, FMUL, qXF, FMUL, qZF, RIGHT, FSUB0]
' Y = 1 - ((2 * (qYF * qYF)) - (2 * (qZF * qZF)))
I2CWRITE Fpu_SDA, Fpu_SCL, Fpu_ID, 0, [SELECTA, Y, FSETI, 1, LEFT, LEFT,_
FSETI, 2, LEFT, FSET, qYF, FMUL, qYF, RIGHT, FMUL0, RIGHT, FSET0, LEFT,_
FSETI, 2, LEFT, FSET, qZF, FMUL, qZF, RIGHT, FMUL0, RIGHT, FSUB0, RIGHT,_
FSUB0]
' heading = DEGREES(ATAN2(X,Y))
I2CWRITE Fpu_SDA, Fpu_SCL, Fpu_ID, 0, [SELECTA, heading, LEFT, LEFT, FSET, X,_
ATAN2, Y, RIGHT, FSET0, DEGREES, RIGHT, FSET0]
' attitude = DEGREES(asin((2*qXF*qYF) + (2*qZF*qWF)))
I2CWRITE Fpu_SDA, Fpu_SCL, Fpu_ID, 0, [SELECTA, attitude, LEFT, LEFT, LEFT,_
FSETI, 2, FMUL, qXF, FMUL, qYF, RIGHT, FSET0, LEFT, FSETI, 2, FMUL, qZF,_
FMUL, qWF, RIGHT, FADD0, ASIN, RIGHT, FSET0, DEGREES, RIGHT, FSET0]
'
' X = (2 * qXF * qWF) - (2 * qYF * qZF)
I2CWRITE Fpu_SDA, Fpu_SCL, Fpu_ID, 0, [SELECTA, X, LEFT, FSETI, 2, FMUL, qXF,_
FMUL, qWF, RIGHT, FSET0, LEFT, FSETI, 2, FMUL, qYF, FMUL, qZF, RIGHT, FSUB0]
' Y = 1 - ((2 * (qXF * qXF)) - (2 * (qZF * qZF)))
I2CWRITE Fpu_SDA, Fpu_SCL, Fpu_ID, 0, [SELECTA, Y, FSETI, 1, LEFT, LEFT,_
FSETI, 2, LEFT, FSET, qXF, FMUL, qXF, RIGHT, FMUL0, RIGHT, FSET0, LEFT,_
FSETI, 2, LEFT, FSET, qZF, FMUL, qZF, RIGHT, FMUL0, RIGHT, FSUB0, RIGHT,_
FSUB0]
'
' banking = DEGREES(ATAN2(X,Y))
I2CWRITE Fpu_SDA, Fpu_SCL, Fpu_ID, 0, [SELECTA, banking, LEFT, LEFT, FSET, X,_
ATAN2, Y, RIGHT, FSET0, DEGREES, RIGHT, FSET0]
'
' Angle = heading * 10
I2CWRITE Fpu_SDA, Fpu_SCL, Fpu_ID, 0, [SELECTA, 0, LEFT, FSET, heading,_
FMULI, 10, RIGHT, FIX]
GOSUB Fpu_Wait
I2CWRITE Fpu_SDA, Fpu_SCL, Fpu_ID, 0, [LREAD0]
GOSUB Fpu_ReadDelay
I2CREAD Fpu_SDA, Fpu_SCL, Fpu_ID, 0, [Angle(3), Angle(2), Angle(1), Angle(0)]
There are some variables to set up too.
Bookmarks