M5GOの加速度・角速度センサーから算出した回転角度のリアルタイムグラフ表示

概要

前の記事「M5GOの加速度・角速度センサーでBlenderの3Dモデルを動かす」でM5GOの加速度・角速度センサーから算出した回転角度を MQTT Broker へ送信するプログラムを作ったので、同時にMQTT Brokerから取得したデータをリアルタイムにグラフ化するプログラムです。
（データは100ms間隔で送信されていますが、完全には追随できていないようで、動かし続けると表示が遅延してしまいます）

環境

• M5GO (Firmware 0.15)
• iMac (macOS High Sierra)
• MQTT Broker (mosquitto version 1.4.15)
• matplotlib (2.2.3)

M5GOでBlenderの3Dモデルを動かすのと同時に、回転角度のリアルタイムグラフ表示https://t.co/yr8vwqtHl7#M5GO #M5Stack pic.twitter.com/gQ9igXBoQg

— 稲澤祐一 (@inasawa) 2018年9月4日

リアルタイムグラフ表示プログラム

plot_angles.py

from matplotlib import pyplot as plt
import math
import paho.mqtt.client as mqtt
import time
import signal, os


MQTT_BROKER = 'MQTTBroker' # MQTTBroker のアドレスを設定
MQTT_TOPIC  = 'M5GO/MPU6500'


# CTRL/C 押下時の停止用ハンドラー
# sys.exit では停止しない為、os._exit を使用
def signal_handler(signal, frame):
    print('\nExit by KeyboardInterrupt')
    os._exit(0)

# コネクト時コールバック
def on_connect(client, userdata, flags, respons_code):
    print('MQTT Broker Connected. Status {}'.format(respons_code))
    client.subscribe(MQTT_TOPIC)

# メッセージ受信時コールバック
def on_message(client, userdata, msg):
    # 以下の8種類の値を取得
    #   加速度から算出した回転角 (roll, pitch)
    #   角速度から算出した回転角 (roll, pitch, yaw)
    #   角速度と加速度の両方から算出した回転角 (roll, pitch)
    #   角度センサーから取得した回転角  (yaw)
    try:
        a_roll, a_pitch, \
        g_roll, g_pitch, g_yaw, \
        ga_roll, ga_pitch, \
        angle_sensor_val = \
                [float(n) for n in msg.payload.decode('utf-8').split(',')]
    except Exception as e:
        print('Exception: ', e)
        return
    
    times.append(time.time() - start_time)
    times.pop(0)
    
    values_a_roll.append(a_roll)
    values_a_roll.pop(0)
    values_g_roll.append(g_roll)
    values_g_roll.pop(0)
    values_ga_roll.append(ga_roll)
    values_ga_roll.pop(0)
    
    values_a_pitch.append(a_pitch)
    values_a_pitch.pop(0)
    values_g_pitch.append(g_pitch)
    values_g_pitch.pop(0)
    values_ga_pitch.append(ga_pitch)
    values_ga_pitch.pop(0)
    
    values_g_yaw.append(g_yaw)
    values_g_yaw.pop(0)
    values_angle_sensor.append(angle_sensor_val)
    values_angle_sensor.pop(0)
    
    line_a_roll.set_data(times, values_a_roll)
    line_g_roll.set_data(times, values_g_roll)
    line_ga_roll.set_data(times, values_ga_roll)
    
    line_a_pitch.set_data(times, values_a_pitch)
    line_g_pitch.set_data(times, values_g_pitch)
    line_ga_pitch.set_data(times, values_ga_pitch)
    
    line_g_yaw.set_data(times, values_g_yaw)
    line_angle_sensor.set_data(times, values_angle_sensor)
    
    plt.xlim(min(times), max(times))
    plt.draw()
    plt.pause(0.001)


# CTRL/C 押下時の停止用ハンドラーを設定
signal.signal(signal.SIGINT, signal_handler)

times               = [0 for i in range(100)]
values_a_roll       = [0 for i in range(100)]
values_a_pitch      = [0 for i in range(100)]
values_g_roll       = [0 for i in range(100)]
values_g_pitch      = [0 for i in range(100)]
values_ga_roll      = [0 for i in range(100)]
values_ga_pitch     = [0 for i in range(100)]
values_g_yaw        = [0 for i in range(100)]
values_angle_sensor = [0 for i in range(100)]

start_time = time.time()

# interactive mode on
plt.ion()

fig = plt.figure(figsize=(12, 6))
fig.suptitle('M5GO Roll/Pitch/Yaw Angles')

# Roll 用グラフ
ax1 = fig.add_subplot(311)
ax1.set_ylim(-180, 180)
ax1.set_yticks(range(-180, 181, 60))
ax1.grid(True)
ax1.set_ylabel('Roll (Deg)')

# Pitch 用グラフ
ax2 = fig.add_subplot(312, sharex=ax1, sharey=ax1)
ax2.set_ylim(-180, 180)
ax2.grid(True)
ax2.set_ylabel('Pitch (Deg)')

# Yaw 用グラフ
ax3 = fig.add_subplot(313, sharex=ax1, sharey=ax1)
ax3.set_ylim(-180, 180)
ax3.grid(True)
ax3.set_ylabel('Yaw (Deg)')

# Roll 用グラフ
line_a_roll,   = ax1.plot(times, values_a_roll)
line_g_roll,   = ax1.plot(times, values_g_roll)
line_ga_roll,  = ax1.plot(times, values_ga_roll)
ax1.legend([line_g_roll, line_a_roll, line_ga_roll],['Gyro', 'Acc', 'G+A'], loc='upper left')

# Pitch 用グラフ
line_a_pitch,  = ax2.plot(times, values_a_pitch)
line_g_pitch,  = ax2.plot(times, values_g_pitch)
line_ga_pitch, = ax2.plot(times, values_ga_pitch)
ax2.legend([line_g_pitch, line_a_pitch, line_ga_pitch],['Gyro', 'Acc', 'G+A'], loc='upper left')

# Yaw 用グラフ
line_g_yaw,        = ax3.plot(times, values_g_yaw)
line_angle_sensor, = ax3.plot(times, values_angle_sensor)
ax3.legend([line_g_yaw, line_angle_sensor],['Gyro', 'Angle Sensor'], loc='upper left')

plt.pause(0.001)


# MQTT Client セットアップ
client = mqtt.Client(protocol=mqtt.MQTTv311)

# コネクト時・メッセージ受信時のコールバックを指定
client.on_connect = on_connect
client.on_message = on_message

# MQTT Broker に接続しメッセージ受信待ち
client.connect(MQTT_BROKER)
client.loop_forever()