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)