Util.py

#Util.py - Responsible for hoding utiity functions that may be used by multiple modules

#Import the math module to use trig functions
import math

#fromQuaternion - Responsble for converting from a given quaternion to roll, pitch, and yaw
def fromQuaternion(o):
    #Normalize Quaternion

    #Calculate requried intermediate products
    i0 = 2.0 * (o[3] * o[0] + o[1] * o[2])
    i1 = 1.0 - 2.0 * (o[0] ** 2 + o[1] ** 2)
    i2 = 2.0 * (o[3] * o[1] - o[2] * o[0])
    i3 = 2.0 * (o[3] * o[2] - o[0] * o[1])
    i4 = 1.0 - 2.0 * (o[1] * o[1] + o[2] * o[2])

    #Normalizing o2
    if i2 > +1.0:
            i2 = +1.0
    else:
            i2 = i2

    if i2 < -1.0:
            i2 = -1.0
    else:
            i2 = i2

    #Compute the roll, pitch, and yaw, convert to degrees
    roll = math.atan2(i0, i1) * (180 / math.pi)
    pitch = math.asin(i2) * (180 / math.pi)
    yaw = math.atan2(i3, i4) * (180 / math.pi)

    #return as a tuple (read-only)
    return (roll, pitch, yaw)