概要
前の記事「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()