Kenji Arai
BMX055 data by Madgwick Filter sends to PC and shows it by Python program
Dependencies: MadgwickFilter BMX055
Diff: Python_PC_program/quaternion_BMX055.py
- Revision:
- 1:a0ca9f62e81f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Python_PC_program/quaternion_BMX055.py Fri Feb 07 04:28:46 2020 +0000
@@ -0,0 +1,203 @@
+"""
+Created on Feb. 7th, 2020
+@author: Kenji Arai
+
+Refrence
+ http://kieranwynn.github.io/pyquaternion/
+
+ https://kumadasu.com/series/quaternion-for-embedded/
+
+ https://wgld.org/d/webgl/w031.html
+ http://marupeke296.com/DXG_No10_Quaternion.html
+ http://edom18.hateblo.jp/entry/2018/06/25/084023
+"""
+
+from pyquaternion import Quaternion
+from time import sleep
+import serial
+import serial.tools.list_ports
+import tkinter as tk
+from tkinter import *
+from tkinter import messagebox
+import numpy as np
+from matplotlib import pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from enum import Enum
+
+from check_ser_port import connect_dtlggr
+
+D_LBL_MODE_IMU = 'Operating Mode: Quaternion '
+D_BTN_MODE_IMU = ' IMU / Quarternion mode '
+D_LBL_MODE_ACC = 'Operating Mode: Accelerometer '
+D_BTN_MODE_ACC = ' ACC / Accelerometer mode '
+D_SPACE = ' '
+D_LETTER = 'UTF-8'
+
+class mode(Enum):
+ ACC = 1
+ IMU = 2
+ ALL = 3
+ RST = 4
+
+class op_mode:
+ opeartion_mode = mode.IMU
+
+ def set_mode(ser,mode):
+ if mode == mode.IMU:
+ op_mode.opeartion_mode = mode.IMU
+ ser.write(bytes('2',D_LETTER))
+ sleep(0.02)
+ ser.write(bytes('2',D_LETTER))
+ elif mode == mode.ACC:
+ op_mode.opeartion_mode = mode.ACC
+ ser.write(bytes('1',D_LETTER))
+ sleep(0.02)
+ ser.write(bytes('1',D_LETTER))
+ else:
+ ser.write(bytes('R',D_LETTER))
+ sleep(0.02)
+ ser.write(bytes('R',D_LETTER))
+
+ def get_mode():
+ return op_mode.opeartion_mode
+
+def chart_initialize(root):
+ root.title("BMX055 Quarternion")
+ root.resizable(False, False)
+ frame0 = tk.Frame(root)
+ frame1 = tk.Frame(root)
+ text_widget0 = tk.Text(frame0,height=1,width=35,fg='darkblue',bg='gray94',
+ relief='flat',font=('Arial', '12'))
+ text_widget0.grid(row=0,column=0)
+
+ def acc_action():
+ op_mode.set_mode(ser,mode.ACC)
+ text_widget0.delete('1.0','end')
+ text_widget0.insert('end',D_SPACE)
+ text_widget0.insert('end',D_LBL_MODE_ACC)
+
+ def imu_action():
+ op_mode.set_mode(ser,mode.IMU)
+ text_widget0.delete('1.0','end')
+ text_widget0.insert('end',D_SPACE)
+ text_widget0.insert('end',D_LBL_MODE_IMU)
+
+ def reset_action():
+ op_mode.set_mode(ser,mode.RST)
+ text_widget0.delete('1.0','end')
+ text_widget0.insert('end',D_SPACE)
+ text_widget0.insert('end',D_LBL_MODE_IMU)
+
+ def quit_action():
+ root.destroy()
+
+ button0 = tk.Button(frame1,text=D_BTN_MODE_ACC,command=acc_action)
+ button0.pack(anchor=tk.S,fill=tk.BOTH)
+ button1 = tk.Button(frame1,text=D_BTN_MODE_IMU,command=imu_action)
+ button1.pack(anchor=tk.S,fill=tk.BOTH)
+ button2 = tk.Button(frame1,text='RESET',command=reset_action)
+ button2.pack(anchor=tk.S,fill=tk.BOTH)
+ button3 = tk.Button(frame1,text='EXIT',command=quit_action)
+ button3.pack(anchor=tk.S,fill=tk.BOTH)
+ frame0.pack(fill='both')
+ frame1.pack(fill='both')
+
+ text_widget0.delete('1.0','end')
+ text_widget0.insert('end',D_SPACE)
+ text_widget0.insert('end',D_LBL_MODE_IMU)
+
+
+def graph_initialize():
+ global fig, lines, startpoints, endpoints
+
+ fig = plt.figure(figsize=(8,6))
+ ax = fig.add_axes([0, 0, 1, 1], projection='3d')
+ ax.set_xlabel('X(Red)',size=14, color='red')
+ ax.set_ylabel('Y(Green)',size=14, color='green')
+ ax.set_zlabel('Z(Blue)',size=14, color='blue')
+ colors = ['red', 'green', 'blue']
+ lines = sum([ax.plot([], [], linewidth = 5, c=c) for c in colors], [])
+ startpoints = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
+ endpoints = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
+ ax.set_xticks([-1.0, -0.5, 0.0, 0.5, 1.0])
+ ax.set_yticks([-1.0, -0.5, 0.0, 0.5, 1.0])
+ ax.set_zticks([-1.0, -0.5, 0.0, 0.5, 1.0])
+ ax.set_xlim((-1, 1))
+ ax.set_ylim((-1, 1))
+ ax.set_zlim((-1, 1))
+ ax.quiver(-1,0,0,1,0,0,length=2,arrow_length_ratio=0,
+ linestyles='dashed',color='red')
+ ax.quiver(0,-1,0,0,1,0,length=2,arrow_length_ratio=0,
+ linestyles='dashed',color='green')
+ ax.quiver(0,0,-1,0,0,1,length=2,arrow_length_ratio=0,
+ linestyles='dashed',color='blue')
+ ax.view_init(30, 10)
+
+def graph_update(serData,mode):
+ if mode == mode.ACC:
+ n = 0
+ for line, start, end in zip(lines, startpoints, endpoints):
+ start = [0.,0.,0.]
+ end =[0., 0., 0.]
+ end[n] = float(serData[n])
+ n += 1
+ line.set_data([start[0], end[0]], [start[1], end[1]])
+ line.set_3d_properties([start[2], end[2]])
+ elif mode == mode.IMU:
+ q = Quaternion(serData)
+ for line, start, end in zip(lines, startpoints, endpoints):
+ start = q.rotate(start)
+ end = q.rotate(end)
+ line.set_data([start[0], end[0]], [start[1], end[1]])
+ line.set_3d_properties([start[2], end[2]])
+
+ fig.canvas.draw()
+
+def show_BMX055_serial():
+
+ def every_second():
+ serData = ser.readline().strip().split(b',')
+ dt = len(serData)
+ #print(serData)
+ mode = op_mode.get_mode()
+ if mode == mode.ACC and dt != 3:
+ op_mode.set_mode(ser,mode.ACC)
+ print('ERROR @ read ser read')
+ elif mode == mode.IMU and dt != 4:
+ op_mode.set_mode(ser,mode.IMU)
+ print('ERROR @ read ser read')
+ elif mode == mode.ACC or mode == mode.IMU:
+ graph_update(serData,mode)
+ else:
+ print(serData)
+
+ root.after(1, every_second)
+
+ root = tk.Tk()
+ com_port=connect_dtlggr()
+ if com_port == None:
+ print("Serial port is not found!")
+ root.withdraw()
+ ready = messagebox.showwarning('showwaning', 'USB is not ready!!')
+ print(ready)
+ root.destroy()
+ return
+
+ global ser
+ print('checking serial')
+ #ser = serial.Serial(com_port,921600,timeout=None)
+ ser = serial.Serial(com_port,115200,timeout=None)
+ op_mode.set_mode(ser,mode.IMU)
+ chart_initialize(root)
+ graph_initialize()
+ every_second()
+ root.mainloop()
+
+ print('Push EXIT button')
+ ser.close()
+ plt.clf()
+ plt.close()
+
+if __name__ == '__main__':
+ show_BMX055_serial()
+ print("Exit")