from magum import Magum
from array import *
import time
# enable Gyro, Accelerometer and Magnetometer
magum = Magum(2000, 0, 2, 1) # G_scaleRange = 2000 | FsDouble = 0 (disabled) | A_scaleRange = 2 | lnoise = 1 (on)
while True:
axis = magum.readAData("deg")
print str(axis[0]) + "," + str(axis[1]) + "," + str(axis[2])
time.sleep(0.900)
# enable Gyro, Accelerometer and Magnetometer
magum = Magum(2000,0,2,1) # G_scaleRange = 2000 | FsDouble = 0 (disabled) | A_scaleRange = 2 | lnoise = 1 (on)
axisOffset = magum.calibrateSens(1000) # calibration (not necessary with read Data functions)
if axisOffset[0] >= 32768:
axisOffset[0] -= 65536
if axisOffset[1] >= 32768:
axisOffset[1] -= 65536
if axisOffset[2] >= 32768:
axisOffset[2] -= 65536
aOffsetX = (axisOffset[0]/4) * 0.244
aOffsetY = (axisOffset[1]/4) * 0.244
aOffsetZ = (axisOffset[2]/4) * 0.244
aOffsetX2 = aOffsetX * aOffsetX
aOffsetY2 = aOffsetY * aOffsetY
aOffsetZ2 = aOffsetZ * aOffsetZ
# getting angles
aOffsetX = math.atan(aOffsetX/math.sqrt(aOffsetY2+aOffsetZ2))*(180/math.pi)
aOffsetY = math.atan(aOffsetY/math.sqrt(aOffsetX2+aOffsetZ2))*(180/math.pi)
aOffsetZ = math.atan(aOffsetZ/math.sqrt(aOffsetX2+aOffsetY2))*(180/math.pi)
while True:
axis = magum.readAData('deg')
print str(axis[0]-aOffsetX) + ',' + str(axis[1]-aOffsetY) + ',' + str(axis[2]-aOffsetZ)
time.sleep(.900)
# Integrate to yield quaternion
q1 += qDot1 * deltat
q2 += qDot2 * deltat
q3 += qDot3 * deltat
q4 += qDot4 * deltat
norm = math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) # normalize quaternion
norm = 1.0/norm
q.insert(0,q1 * norm)
q.insert(1,q2 * norm)
q.insert(2,q3 * norm)
q.insert(3,q4 * norm)
return q
magum = Magum(250,0,2,1)
i = 0
ar = array('f',[])
while True:
aCompArray = magum.readAData('gcomp')
gCompArray = magum.readGData('rads')
mCompArray = magum.readMData('ut')
ar = madgwickQuaternionFilter(aCompArray,gCompArray,mCompArray)
if i%20 == 0:
print madgwickQuaternionFilter(aCompArray,gCompArray,mCompArray)
i += 1
from magum import Magum
from array import *
import time
# enable Gyro, Accelerometer and Magnetometer
magum = Magum(
2000, 0, 2, 1
) # G_scaleRange = 2000 | FsDouble = 0 (disabled) | A_scaleRange = 2 | lnoise = 1 (on)
while True:
axis = magum.readAData('deg')
print str(axis[0]) + ',' + str(axis[1]) + ',' + str(axis[2])
time.sleep(.900)
