Matthias Grob / Mbed 2 deprecated IMU_10DOF

An fully working IMU-Filter and Sensor drivers for the 10DOF-Board over I2C. All in one simple class. Include, calibrate sensors, call read, get angles. (3D Visualisation code for Python also included) Sensors: L3G4200D, ADXL345, HMC5883, BMP085

Dependencies:   mbed

3D_IMU_10DOF.py.txt@4:f62337b907e5, 2013-08-29 (annotated)

Committer:
maetugr
Date:
Thu Aug 29 13:52:30 2013 +0000
Revision:
4:f62337b907e5
Parent:
2:ca8fd72a8f2b 
The Altitude Sensor is now implemented, it's really 10DOF now ;); TODO: Autocalibration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 2:ca8fd72a8f2b 1 # Test for IMU_10DOF
maetugr 2:ca8fd72a8f2b 2 # MaEtUgR @2013
maetugr 2:ca8fd72a8f2b 3 # This script needs VPhyton, pyserial and pywin modules
maetugr 2:ca8fd72a8f2b 4
maetugr 2:ca8fd72a8f2b 5 # First Install Python 2.6.4
maetugr 2:ca8fd72a8f2b 6 # Install pywin from http://sourceforge.net/projects/pywin32/
maetugr 2:ca8fd72a8f2b 7 # Install pyserial from http://sourceforge.net/projects/pyserial/files/
maetugr 2:ca8fd72a8f2b 8 # Install Vphyton from http://vpython.org/contents/download_windows.html
maetugr 2:ca8fd72a8f2b 9
maetugr 2:ca8fd72a8f2b 10 from visual import *
maetugr 2:ca8fd72a8f2b 11 import serial
maetugr 2:ca8fd72a8f2b 12 import string
maetugr 2:ca8fd72a8f2b 13 import math
maetugr 2:ca8fd72a8f2b 14
maetugr 2:ca8fd72a8f2b 15 from time import time
maetugr 2:ca8fd72a8f2b 16
maetugr 2:ca8fd72a8f2b 17 grad2rad = 3.141592/180.0
maetugr 2:ca8fd72a8f2b 18
maetugr 2:ca8fd72a8f2b 19 # Check your COM port and baud rate
maetugr 2:ca8fd72a8f2b 20 ser = serial.Serial(port='COM3',baudrate=921600, timeout=1)
maetugr 2:ca8fd72a8f2b 21
maetugr 2:ca8fd72a8f2b 22 # Main scene
maetugr 2:ca8fd72a8f2b 23 scene=display(title="3D Visualisation")
maetugr 2:ca8fd72a8f2b 24 scene.range=(1.2,1.2,1.2)
maetugr 2:ca8fd72a8f2b 25 #scene.forward = (0,-1,-0.25)
maetugr 2:ca8fd72a8f2b 26 scene.forward = (1,0,-0.25)
maetugr 2:ca8fd72a8f2b 27 scene.up=(0,0,1)
maetugr 2:ca8fd72a8f2b 28
maetugr 2:ca8fd72a8f2b 29 # Second scene (Roll, Pitch, Yaw)
maetugr 2:ca8fd72a8f2b 30 scene2 = display(title='2D Visualisation',x=0, y=0, width=500, height=200,center=(0,0,0), background=(0,0,0))
maetugr 2:ca8fd72a8f2b 31 scene2.range=(1,1,1)
maetugr 2:ca8fd72a8f2b 32 scene.width=500
maetugr 2:ca8fd72a8f2b 33 scene.y=200
maetugr 2:ca8fd72a8f2b 34
maetugr 2:ca8fd72a8f2b 35 scene2.select()
maetugr 2:ca8fd72a8f2b 36 #Roll, Pitch, Yaw
maetugr 2:ca8fd72a8f2b 37 cil_roll = cylinder(pos=(-0.4,0,0),axis=(0.2,0,0),radius=0.01,color=color.red)
maetugr 2:ca8fd72a8f2b 38 cil_roll2 = cylinder(pos=(-0.4,0,0),axis=(-0.2,0,0),radius=0.01,color=color.red)
maetugr 2:ca8fd72a8f2b 39 cil_pitch = cylinder(pos=(0.1,0,0),axis=(0.2,0,0),radius=0.01,color=color.green)
maetugr 2:ca8fd72a8f2b 40 cil_pitch2 = cylinder(pos=(0.1,0,0),axis=(-0.2,0,0),radius=0.01,color=color.green)
maetugr 2:ca8fd72a8f2b 41 #cil_course = cylinder(pos=(0.6,0,0),axis=(0.2,0,0),radius=0.01,color=color.blue)
maetugr 2:ca8fd72a8f2b 42 #cil_course2 = cylinder(pos=(0.6,0,0),axis=(-0.2,0,0),radius=0.01,color=color.blue)
maetugr 2:ca8fd72a8f2b 43 arrow_course = arrow(pos=(0.6,0,0),color=color.cyan,axis=(-0.2,0,0), shaftwidth=0.02, fixedwidth=1)
maetugr 2:ca8fd72a8f2b 44
maetugr 2:ca8fd72a8f2b 45 #Roll,Pitch,Yaw labels
maetugr 2:ca8fd72a8f2b 46 label(pos=(-0.4,0.3,0),text="Roll",box=0,opacity=0)
maetugr 2:ca8fd72a8f2b 47 label(pos=(0.1,0.3,0),text="Pitch",box=0,opacity=0)
maetugr 2:ca8fd72a8f2b 48 label(pos=(0.55,0.3,0),text="Yaw",box=0,opacity=0)
maetugr 2:ca8fd72a8f2b 49 label(pos=(0.6,0.22,0),text="N",box=0,opacity=0,color=color.yellow)
maetugr 2:ca8fd72a8f2b 50 label(pos=(0.6,-0.22,0),text="S",box=0,opacity=0,color=color.yellow)
maetugr 2:ca8fd72a8f2b 51 label(pos=(0.38,0,0),text="W",box=0,opacity=0,color=color.yellow)
maetugr 2:ca8fd72a8f2b 52 label(pos=(0.82,0,0),text="E",box=0,opacity=0,color=color.yellow)
maetugr 2:ca8fd72a8f2b 53 label(pos=(0.75,0.15,0),height=7,text="NE",box=0,color=color.yellow)
maetugr 2:ca8fd72a8f2b 54 label(pos=(0.45,0.15,0),height=7,text="NW",box=0,color=color.yellow)
maetugr 2:ca8fd72a8f2b 55 label(pos=(0.75,-0.15,0),height=7,text="SE",box=0,color=color.yellow)
maetugr 2:ca8fd72a8f2b 56 label(pos=(0.45,-0.15,0),height=7,text="SW",box=0,color=color.yellow)
maetugr 2:ca8fd72a8f2b 57
maetugr 2:ca8fd72a8f2b 58 L1 = label(pos=(-0.4,0.22,0),text="-",box=0,opacity=0)
maetugr 2:ca8fd72a8f2b 59 L2 = label(pos=(0.1,0.22,0),text="-",box=0,opacity=0)
maetugr 2:ca8fd72a8f2b 60 L3 = label(pos=(0.7,0.3,0),text="-",box=0,opacity=0)
maetugr 2:ca8fd72a8f2b 61
maetugr 2:ca8fd72a8f2b 62 # Main scene objects
maetugr 2:ca8fd72a8f2b 63 scene.select()
maetugr 2:ca8fd72a8f2b 64 # Reference axis (x,y,z)
maetugr 2:ca8fd72a8f2b 65 arrow(color=color.green,axis=(1,0,0), shaftwidth=0.02, fixedwidth=1)
maetugr 2:ca8fd72a8f2b 66 arrow(color=color.green,axis=(0,-1,0), shaftwidth=0.02 , fixedwidth=1)
maetugr 2:ca8fd72a8f2b 67 arrow(color=color.green,axis=(0,0,-1), shaftwidth=0.02, fixedwidth=1)
maetugr 2:ca8fd72a8f2b 68 # labels
maetugr 2:ca8fd72a8f2b 69 label(pos=(0,0,0.8),text="MaEtUgR's IMU test",box=0,opacity=0)
maetugr 2:ca8fd72a8f2b 70 label(pos=(1,0,0),text="X",box=0,opacity=0)
maetugr 2:ca8fd72a8f2b 71 label(pos=(0,-1,0),text="Y",box=0,opacity=0)
maetugr 2:ca8fd72a8f2b 72 label(pos=(0,0,-1),text="Z",box=0,opacity=0)
maetugr 2:ca8fd72a8f2b 73 # IMU object
maetugr 2:ca8fd72a8f2b 74 platform = box(length=1, height=0.05, width=1, color=color.red)
maetugr 2:ca8fd72a8f2b 75 p_line = box(length=1,height=0.08,width=0.1,color=color.yellow)
maetugr 2:ca8fd72a8f2b 76 plat_arrow = arrow(color=color.green,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1)
maetugr 2:ca8fd72a8f2b 77
maetugr 2:ca8fd72a8f2b 78
maetugr 2:ca8fd72a8f2b 79 f = open("Serial"+str(time())+".txt", 'w')
maetugr 2:ca8fd72a8f2b 80
maetugr 2:ca8fd72a8f2b 81 roll=0
maetugr 2:ca8fd72a8f2b 82 pitch=0
maetugr 2:ca8fd72a8f2b 83 yaw=0
maetugr 2:ca8fd72a8f2b 84 while 1:
maetugr 2:ca8fd72a8f2b 85 rate(100) # for speed issues with 3D not refreshing or loosing Serial data
maetugr 2:ca8fd72a8f2b 86 line = ser.readline()
maetugr 2:ca8fd72a8f2b 87 #line = line.replace("!ANG:","") # Delete "!ANG:"
maetugr 2:ca8fd72a8f2b 88 print line
maetugr 2:ca8fd72a8f2b 89 f.write(line) # Write to the output log file
maetugr 2:ca8fd72a8f2b 90 words = string.split(line,",") # Fields split
maetugr 2:ca8fd72a8f2b 91 if len(words) > 2:
maetugr 2:ca8fd72a8f2b 92 try:
maetugr 2:ca8fd72a8f2b 93 roll = -float(words[0])*grad2rad
maetugr 2:ca8fd72a8f2b 94 pitch = float(words[1])*grad2rad
maetugr 2:ca8fd72a8f2b 95 yaw = -float(words[2])*grad2rad
maetugr 2:ca8fd72a8f2b 96
maetugr 2:ca8fd72a8f2b 97 axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch))
maetugr 2:ca8fd72a8f2b 98 up=(sin(roll)*sin(yaw)+cos(roll)*sin(pitch)*cos(yaw),sin(roll)*cos(yaw)-cos(roll)*sin(pitch)*sin(yaw),-cos(roll)*cos(pitch))
maetugr 2:ca8fd72a8f2b 99 platform.axis=axis
maetugr 2:ca8fd72a8f2b 100 platform.up=up
maetugr 2:ca8fd72a8f2b 101 platform.length=1.0
maetugr 2:ca8fd72a8f2b 102 platform.width=0.65
maetugr 2:ca8fd72a8f2b 103 plat_arrow.axis=axis
maetugr 2:ca8fd72a8f2b 104 plat_arrow.up=up
maetugr 2:ca8fd72a8f2b 105 plat_arrow.length=0.8
maetugr 2:ca8fd72a8f2b 106 p_line.axis=axis
maetugr 2:ca8fd72a8f2b 107 p_line.up=up
maetugr 2:ca8fd72a8f2b 108 cil_roll.axis=(0.2*cos(roll),0.2*sin(roll),0)
maetugr 2:ca8fd72a8f2b 109 cil_roll2.axis=(-0.2*cos(roll),-0.2*sin(roll),0)
maetugr 2:ca8fd72a8f2b 110 cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0)
maetugr 2:ca8fd72a8f2b 111 cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0)
maetugr 2:ca8fd72a8f2b 112 arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0)
maetugr 2:ca8fd72a8f2b 113 L1.text = str(float(words[0]))
maetugr 2:ca8fd72a8f2b 114 L2.text = str(float(words[1]))
maetugr 2:ca8fd72a8f2b 115 L3.text = str(float(words[2]))
maetugr 2:ca8fd72a8f2b 116 except:
maetugr 2:ca8fd72a8f2b 117 print "Invalid line"
maetugr 2:ca8fd72a8f2b 118
maetugr 2:ca8fd72a8f2b 119 ser.close
maetugr 2:ca8fd72a8f2b 120 f.close