MetaWear linear acceleration reading example (requires matplotlib for plotting)

data_collector.py

#!/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)

data_plotter.py

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")