Python＋Tkinterによる9DoFセンサビューアの作成

9DoFからの計測値は数字だけ見てもなかなかどの軸で移動が検知されているのか分かりにくい。そこで、視覚的に把握できるようGUIのViewerを作った。Unityとかだと勉強しないといけないので、、Python+Tkinterで作成。普通の作り方だとボタン操作等を処理するため、Tkinterのイベントループを回すが、そうするとリアルタイムもグラフ描画はスレッド等を立てないといけなくなるので、メインループは自前で作ってTkinterの画面にはGUI等の操作系パーツは無しで描画専用とした。画面左側の3本の棒グラフはジャイロセンサの計測値（XYZ)、右側の３本の棒グラフは加速度センサの計測値（XYZ)、センサデータがプラス方向かマイナス方向か、また、強さが分かるように実装。

PC側で動作するビューアのソースは以下（急づくりで汚い）

#!/usr/bin/python3

import serial
import struct
import pdb
import tkinter


def draw_graph(val,pos_x):
   if (val > 0):
      canvas.create_rectangle(pos_x,250-val, pos_x+30, 250)
   else:
      canvas.create_rectangle(pos_x,250, pos_x + 30 ,250 - val)

#
#  bytes('F','F','E','0') -> -12 in int
#
def conv_hex2int(hex_in_bytes):
    val = int(hex_in_bytes, base=16)   # bytes ('F','F','E','4') -> 0xFFE4 in int
    hex_str = "{:04x}".format(val)      # 
    #print(hex_str)
    #pdb.set_trace()
    val = struct.unpack('>h', bytes.fromhex(hex_str))[0]   # 2byte
    return val



GRAPH_HEIGHT=200
MAX_GYRO_VALUE=22000
MAX_ACCEL_VALUE=15800


ser = serial.Serial("/dev/ttyS13", 38400)
cmd = bytes((0x53,))
ser.write(cmd)
ser.readline() # dummy read for drop gargase
ser.readline() # dummy read for drop gargase

root =  tkinter.Tk()
canvas = tkinter.Canvas(root,width=500,height=500)
canvas.pack()
root.update()

while True:
   data = ser.readline()
   #print(data)
   if len(data) < 27:
       continue
   tmp = bytes((data[5],data[6],data[2],data[3]))
   gx = conv_hex2int(tmp)
   tmp = bytes((data[11],data[12],data[8],data[9]))
   gy = conv_hex2int(tmp)
   tmp = bytes((data[17],data[18],data[14],data[15]))
   gz = conv_hex2int(tmp)

   tmp = bytes((data[26],data[27],data[23],data[24]))
   ax = conv_hex2int(tmp)
   tmp = bytes((data[32],data[33],data[29],data[30]))
   ay = conv_hex2int(tmp)
   tmp = bytes((data[38],data[39],data[35],data[36]))
   az = conv_hex2int(tmp)

   print(gx,gy,gz,ax,ay,az,end="")
   print("    ",end="")
   print(data)

   canvas.delete("all")
   canvas.create_line(30, 250, 470, 250)
   canvas.create_text(50, 20, text = "{:d}".format(gx), font = ('FixedSys', 12))
   canvas.create_text(100, 20, text = "{:d}".format(gy), font = ('FixedSys', 12))
   canvas.create_text(150, 20, text = "{:d}".format(gz), font = ('FixedSys', 12))

   draw_graph(GRAPH_HEIGHT * gx / MAX_GYRO_VALUE, 50)
   draw_graph(GRAPH_HEIGHT * gy / MAX_GYRO_VALUE, 100)
   draw_graph(GRAPH_HEIGHT * gz / MAX_GYRO_VALUE, 150)

   canvas.create_text(300, 20, text = "{:d}".format(ax), font = ('FixedSys', 12))
   canvas.create_text(350, 20, text = "{:d}".format(ay), font = ('FixedSys', 12))
   canvas.create_text(400, 20, text = "{:d}".format(az), font = ('FixedSys', 12))

   draw_graph(GRAPH_HEIGHT * ax / MAX_ACCEL_VALUE, 300)
   draw_graph(GRAPH_HEIGHT * ay / MAX_ACCEL_VALUE, 350)
   draw_graph(GRAPH_HEIGHT * az / MAX_ACCEL_VALUE, 400)

   root.update()



root.mainloop()

最終的には、計測した値を積分して姿勢推定までやりたいけど、まずは、加速度センサだけを使って機体の傾きを検知して、水平に保つ制御だけを作ってみる予定。何も工夫しないと制御が発振したりいろいろありそうだけど。。作ってみないとどう動くのか分からない。モータの出力はPWMを使って推力を調整する。制御アルゴリズムをドローン内で開発すると、試作＞コンパイル＞デバッグ＞試作のループに時間がかかりすぎるので、、初期バージョンは自立型とせず、センサデータをＰＣで取得して演算結果をドローンに戻して姿勢制御させてみる予定。そのためには、、PWMによるモータ制御部分を実装する必要がある。

■参考URL
PWM設定解説動画
STM32 TIMERS #1. PWM Output || DMA - YouTube

python - How to run a function in the background of tkinter - Stack Overflow