Pic FPU

[Darrel Taylor]

That's exactly what the uM-FPU does ... and a lot more.
It's made from a dsPIC.

They are about $20 each.
But if you consider the time it will take you to recreate one (several months) then they're extremely cheap.

It's pretty cool, because it has a utility that writes your PBP side code for you.

I've only used it once for a Quaternion to Euler Angle convertor, but it worked great.

You enter math like ...

X = (2 * qYF * qWF) - (2 * qXF * qZF)
Y = 1 - ((2 * (qYF * qYF)) - (2 * (qZF * qZF)))
heading = DEGREES(ATAN2(X,Y))
attitude = DEGREES(asin((2*qXF*qYF) + (2*qZF*qWF))) 

X = (2 * qXF * qWF) - (2 * qYF * qZF)
Y = 1 - ((2 * (qXF * qXF)) - (2 * (qZF * qZF)))

banking = DEGREES(ATAN2(X,Y))

Angle = heading * 10

And it spits out PBP code like this ...

  ' 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.
The chip does the rest.