Last active
August 29, 2020 13:33
-
-
Save hbldh/58013845a63e93c697fb631d47c8a30a to your computer and use it in GitHub Desktop.
MetaWear linear acceleration reading example (requires matplotlib for plotting)
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python | |
# -*- coding: utf-8 -*- | |
from __future__ import division | |
from __future__ import print_function | |
from __future__ import absolute_import | |
import time | |
from pymetawear.discover import select_device | |
from pymetawear.client import MetaWearClient | |
from mbientlab.metawear.cbindings import SensorFusionData, SensorFusionGyroRange, SensorFusionAccRange, SensorFusionMode | |
def handle_notification(data): | |
print("{0}".format(data)) | |
def run_metawear_client(address, stream=True, callback=handle_notification, callback_2=handle_notification): | |
if not address: | |
address = select_device() | |
c = MetaWearClient(address, debug=True) | |
# Sensor Fusion Modes (https://mbientlab.com/cppdocs/latest/sensor_fusion.html#sensor-fusion) | |
# NDOF Calculates absolute orientation from accelerometer, gyro, and magnetometer | |
# IMU_PLUS Calculates relative orientation in space from accelerometer and gyro data | |
# COMPASS Determines geographic direction from th Earth’s magnetic field | |
# M4G Similar to IMUPlus except rotation is detected with the magnetometer | |
c.sensorfusion.set_mode(SensorFusionMode.NDOF) | |
#c.sensorfusion.set_acc_range(SensorFusionAccRange._8G) | |
#c.sensorfusion.set_gyro_range(SensorFusionGyroRange._1000DPS) | |
if stream: | |
c.sensorfusion.notifications( | |
linear_acc_callback=callback, | |
quaternion_callback=callback_2 | |
) | |
else: | |
# Logging, but this does not work currently... It will be in version 0.12.0 of PyMetaWear | |
raise NotImplementedError() | |
c.sensorfusion.start_logging() | |
time.sleep(10.0) | |
if stream: | |
# Deactivate notifications. | |
c.sensorfusion.notifications() | |
else: | |
c.sensorfusion.stop_logging() | |
time.sleep(5.0) | |
c.disconnect() | |
if __name__ == '__main__': | |
run_metawear_client(address="D3:D3:47:EA:2F:23", stream=True, callback=handle_notification) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import datetime | |
import os | |
from queue import Queue | |
import matplotlib.pyplot as plt | |
import numpy as np | |
from scipy.integrate import cumtrapz | |
import data_collector | |
def run_data_plotter(address): | |
q = Queue() | |
q2 = Queue() | |
def enqueue_data(data): | |
q.put(data) | |
def enqueue_data_2(data): | |
q2.put(data) | |
data_collector.run_metawear_client(**{'address': address, 'stream': True, 'callback': enqueue_data, 'callback_2': enqueue_data_2}) | |
xs = [] | |
ys = [] | |
xs_q = [] | |
ys_q = [] | |
while True: | |
try: | |
data = q.get(timeout=0.1) | |
xs.append(datetime.datetime.fromtimestamp(data.get('epoch') / 1000)) | |
ys.append([data.get('value').x, data.get('value').y, data.get('value').z]) | |
except: | |
break | |
while True: | |
try: | |
data = q2.get(timeout=0.1) | |
xs_q.append(datetime.datetime.fromtimestamp(data.get('epoch') / 1000)) | |
ys_q.append([data.get('value').w, data.get('value').x, data.get('value').y, data.get('value').z]) | |
except: | |
break | |
# Create figure for plotting | |
fig = plt.figure() | |
ax = plt.subplot2grid((2, 4), (0, 0), colspan=4) | |
# ax = plt.subplot2grid((3, 3), (0, 0), colspan=3) | |
ax.plot(xs, ys) | |
# Format plot | |
plt.xticks(rotation=45, ha='right') | |
plt.subplots_adjust(hspace=0.50) | |
plt.title('Linear Acceleration over time') | |
plt.ylabel('Acceleration (g)') | |
plt.grid(True) | |
plt.legend(["X", "Y", "Z"]) | |
# Quaternion plots | |
ys_q = np.array(ys_q) | |
for i in range(4): | |
ax = plt.subplot2grid((2, 4), (1, i)) | |
ax.plot(xs_q, ys_q[:, i]) | |
plt.title('Quat ' + ['W', 'X', 'Y', 'Z'][i]) | |
plt.ylim((-1, 1)) | |
plt.grid(True) | |
plt.xticks(rotation=45, ha='right') | |
#plt.ylabel('Magnitude') | |
# Straight up integration of acceleration to get velocity and position. | |
# Works horribly bad. | |
# ys = np.array(ys) | |
# xs = np.array([x.timestamp() for x in xs]) | |
# # Velocity plots | |
# ax = plt.subplot2grid((3, 3), (1, 0)) | |
# ax.plot(xs[:-1], cumtrapz(ys[:, 0], xs)) | |
# plt.title('X Velocity over time') | |
# plt.ylabel('Velocity (m/s)') | |
# | |
# ax = plt.subplot2grid((3, 3), (1, 1)) | |
# ax.plot(xs[:-1], cumtrapz(ys[:, 1], xs)) | |
# plt.title('Y Velocity over time') | |
# plt.ylabel('Velocity (m/s)') | |
# | |
# ax = plt.subplot2grid((3, 3), (1, 2)) | |
# ax.plot(xs[:-1], cumtrapz(ys[:, 2], xs)) | |
# plt.title('Z Velocity over time') | |
# plt.ylabel('Velocity (m/s)') | |
# | |
# # Position plots | |
# | |
# ax = plt.subplot2grid((3, 3), (2, 0)) | |
# ax.plot(xs[:-2], cumtrapz(cumtrapz(ys[:, 0], xs), xs[:-1])) | |
# plt.title('Z Position over time') | |
# plt.ylabel('Position (m)') | |
# | |
# ax = plt.subplot2grid((3, 3), (2, 1)) | |
# ax.plot(xs[:-2], cumtrapz(cumtrapz(ys[:, 1], xs), xs[:-1])) | |
# plt.title('Y Position over time') | |
# plt.ylabel('Position (m)') | |
# | |
# ax = plt.subplot2grid((3, 3), (2, 2)) | |
# ax.plot(xs[:-2], cumtrapz(cumtrapz(ys[:, 2], xs), xs[:-1])) | |
# plt.title('Z Position over time') | |
# plt.ylabel('Position (m)') | |
plt.show() | |
try: | |
os.remove('linear_acceleration.png') | |
except: | |
pass | |
fig.savefig('linear_acceleration.png', bbox_inches='tight') | |
print("Saved figure to {0}".format(os.path.join(os.path.dirname(os.path.abspath(__file__)), "linear_acceleration.png"))) | |
plt.close(fig) | |
if __name__ == '__main__': | |
run_data_plotter(address="D3:D3:47:EA:2F:23") |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment