diff --git a/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.mp4 b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.mp4 index b3e74462f86954630f6a4e40658544276f6ae500..35e02370ec8cd4cadffc8cde68417b593a686b3f 100644 Binary files a/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.mp4 and b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.mp4 differ diff --git a/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.py b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.py index ea567b2607399c7c79b069836e9a9f640a966c9a..03bef6a87fc90e5ac7434c7d434781b6da142a03 100644 --- a/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.py +++ b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.py @@ -16,13 +16,17 @@ import serial,sys,math WIDTH = 750 # window width NBARS = 12 # number of bar graphs BARHEIGHT = 70 # bar graph height -BORDER = 10 # bar graph border HEIGHT = NBARS*BARHEIGHT # window height +BORDER = 10 # bar graph border FONTSIZE = 28 # font size EPS = 0.2 # smoothing filter filter = {} +gx = gy = 0 +gz = 1 +barcount = 0 # def idle(parent,canvas): + global gx,gy,gz # # read a line # @@ -32,16 +36,25 @@ def idle(parent,canvas): # line = line.split(':') match line[0]: - case "ax ay az": + case "ax ay az": # acceleration try: line = line[1].split(',') line = list(map(int,line)) - update_bar_graph("ax",line[0],-10000,10000) - update_bar_graph("ay",line[1],-10000,10000) - update_bar_graph("az",line[2],-10000,10000) + ax = line[0] + ay = line[1] + az = line[2] + ax *= 2/32768 # normalize 2g full scale + ay *= 2/32768 # normalize 2g full scale + az *= 2/32768 # normalize 2g full scale + ax -= gx # subtract gravity + ay -= gy # subtract gravity + az -= gz # subtract gravity + update_bar_graph("ax(g)",ax,-1,1) + update_bar_graph("ay(g)",ay,-1,1) + update_bar_graph("az(g)",az,-1,1) except: pass - case "gx gy gz": + case "gx gy gz": # gravity try: line = line[1].split(',') line = list(map(int,line)) @@ -49,24 +62,30 @@ def idle(parent,canvas): gy = line[1] gz = line[2] norm = math.sqrt(gx*gx+gy*gy+gz*gz) - gx /= norm - gy /= norm - gz /= norm - update_bar_graph("gx",gx,-1,1) - update_bar_graph("gy",gy,-1,1) - update_bar_graph("gz",gz,-1,1) + gx /= norm # normalize to 1g + gy /= norm # normalize to 1g + gz /= norm # normalize to 1g + update_bar_graph("gx(g)",gx,-1,1) + update_bar_graph("gy(g)",gy,-1,1) + update_bar_graph("gz(g)",gz,-1,1) except: pass - case "rx ry rz": + case "rx ry rz": # rotation try: line = line[1].split(',') line = list(map(int,line)) - update_bar_graph("rx",line[0],-10000,10000) - update_bar_graph("ry",line[1],-10000,10000) - update_bar_graph("rz",line[2],-10000,10000) + rx = line[0] + ry = line[1] + rz = line[2] + rx *= 1000/32768 # normalize to 1000 dps full scale + ry *= 1000/32768 # normalize to 1000 dps full scale + rz *= 1000/32768 # normalize to 2000 dps full scale + update_bar_graph("rx(dps)",rx,-1000,1000) + update_bar_graph("ry(dps)",ry,-1000,1000) + update_bar_graph("rz(dps)",rz,-1000,1000) except: pass - case "qw qx qy qz": + case "qw qx qy qz": # quaternions try: line = line[1].split(',') line = list(map(float,line)) @@ -85,9 +104,9 @@ def idle(parent,canvas): yaw = 180*yaw/math.pi pitch = 180*pitch/math.pi roll = 180*roll/math.pi - update_bar_graph("yaw",yaw,-90,90) - update_bar_graph("pitch",pitch,-180,180) - update_bar_graph("roll",roll,-180,180) + update_bar_graph("yaw(deg)",yaw,-90,90) + update_bar_graph("pitch(deg)",pitch,-180,180) + update_bar_graph("roll(deg)",roll,-180,180) except: pass # @@ -104,7 +123,10 @@ def update_bar_graph(label,value,vmin,vmax): tuple[2] = BORDER+(WIDTH-2*BORDER)*(filt-vmin)/(vmax-vmin) canvas.coords(label+'r',tuple) # -def create_bar_graph(label,n): +def create_bar_graph(label): + global barcount + n = barcount + barcount += 1 canvas.create_rectangle(BORDER,n*BARHEIGHT+BORDER,WIDTH-BORDER,(n+1)*BARHEIGHT-BORDER,tags=label+'b', fill='#0000b0') canvas.create_rectangle(BORDER,n*BARHEIGHT+BORDER,BORDER,(n+1)*BARHEIGHT-BORDER,tags=label+'r', fill='#b00000') canvas.create_text(WIDTH/2,(n+0.5)*BARHEIGHT,text=label,font=("Helvetica",FONTSIZE),tags=label,fill="#FFFFFF",anchor=CENTER) @@ -128,21 +150,21 @@ root.title('hello.LSM6DSV16X.fusion.py (q to exit)') root.bind('q','exit') canvas = Canvas(root,width=WIDTH,height=HEIGHT,background='white') # -create_bar_graph("ax",0) -create_bar_graph("ay",1) -create_bar_graph("az",2) +create_bar_graph("ax(g)") +create_bar_graph("ay(g)") +create_bar_graph("az(g)") # -create_bar_graph("gx",3) -create_bar_graph("gy",4) -create_bar_graph("gz",5) +create_bar_graph("gx(g)") +create_bar_graph("gy(g)") +create_bar_graph("gz(g)") # -create_bar_graph("rx",6) -create_bar_graph("ry",7) -create_bar_graph("rz",8) +create_bar_graph("rx(dps)") +create_bar_graph("ry(dps)") +create_bar_graph("rz(dps)") # -create_bar_graph("roll",9) -create_bar_graph("pitch",10) -create_bar_graph("yaw",11) +create_bar_graph("roll(deg)") +create_bar_graph("pitch(deg)") +create_bar_graph("yaw(deg)") # canvas.pack() #