›

Stream Quaternion in Python

AJM_InfAJM_Inf
in Python

Hello,

I am new to the community and also new to python. I want to stream the quaternion data from my MetaMotionC (to get the orientation of glasses). I researched several times how to do so, but I only found how to log these. Also I've read on some posts it is not possible to get quaternion with only 1 sensor (4 sensors are required). Is that true?

I have found this post for logging quaternion data. I want to stream it and don't now what to do exactly.

`from mbientlab.metawear import MetaWear, libmetawear, parse_value, cbindings, SensorFusionData, SensorFusionMode
from mbientlab.metawear import FnVoid_VoidP_UByte_Long_UByteP_UByte, FnVoid_VoidP_DataP, byref, LogDownloadHandler, FnVoid_VoidP_UInt_UInt
from mbientlab.metawear.cbindings import *

from time import sleep
from threading import Event
from ctypes import c_void_p, cast

mac_address = 'CB:4C:61:C2:62:39'
output = []
imu_processor = None

class Controller:
    def __init__(self):
        global mac_address
            self.imu = MetaWear(mac_address)
            if not self.imu.is_connected:
                try:
                    self.imu.connect()
                except ConnectionError:
                    rospy.logwarn('Cannot connect to IMU!')
            self.imu.on_disconnect = lambda status: rospy.logwarn('Disconnected to IMU!')


       # Callback function to process/parse the gyroscope data
    def imu_data_handler(self, ctx, data):
        values = parse_value(data)
        global output
        output.append(values)


    def detect_head_orientation(self):

        if not self.imu.is_connected:
            try:
                self.imu.connect()
            except ConnectionError:
                rospy.logwarn('Cannot connect to IMU!')
                return

        # Callback function pointer
        callback = FnVoid_VoidP_DataP(self.imu_data_handler)

        # Connection parameters: Board, Connection Invervall (min, max), Slave latency, Connection supervision timeout
        libmetawear.mbl_mw_settings_set_connection_parameters(self.imu.board, 7.5, 7.5, 0, 6000)
        sleep(1.5)

        # Get the gyroscope data signal
        signal = libmetawear.mbl_mw_gyro_bmi160_get_rotation_data_signal(self.imu.board)
        # Subscribe to it
        libmetawear.mbl_mw_datasignal_subscribe(signal, None, callback)

        # Enable the gyroscope
        libmetawear.mbl_gyro_bmi160_enable_rotation_sampling(self.imu.board)
        libmetawear.mbl_mw_gyro_bmi160_start(self.imu.board)

        # TODO: Get Data and do sth
        quaternion = libmetawear.mbl_mw_sensor_fusion_get_data_signal(self.imu.board, SensorFusionData.QUATERNION)
    orientation = quaternion

        # Disable the gyroscope
        libmetawear.mbl_mw_gyro_bmi160_stop(self.imu.board)
        libmetawear.mbl_mw_gyro_bmi160_disable_rotation_sampling(self.imu.board)

        # Unsubscribe to it
        libmetawear.mbl_mw_datasignal_unsubscribe(signal)

        return orientation


    def disconnect_imu(self):
        self.imu.disconnect()`

Comments

Sign In or Register to comment.