Guest User

Untitled

a guest
Sep 4th, 2017
538
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2. # -*- coding: utf-8 -*-
  3. """
  4. :mod:`sensorfusion`
  5. ==================
  6.  
  7. Created by mgeorgi <marcus.georgi@kinemic.de>
  8. Created on 2016-02-01
  9.  
  10. """
  11.  
  12. from __future__ import division
  13. from __future__ import print_function
  14. from __future__ import absolute_import
  15.  
  16. import sys
  17.  
  18. import time
  19.  
  20. from pymetawear.discover import select_device
  21. from pymetawear.client import MetaWearClient
  22. from pymetawear.mbientlab.metawear.cbindings import SensorFusionData, SensorFusionGyroRange, SensorFusionAccRange, SensorFusionMode
  23.  
  24. #address = select_device()
  25. address = 'EC:44:5E:DD:E1:74'
  26.  
  27. reconnects = 5
  28. while reconnects >= 0:
  29.     try:
  30.         c = MetaWearClient(str(address), 'pygatt', debug=False)
  31.         break
  32.     except Exception, e:
  33.         if reconnects > 0:
  34.             print("Reconnecting...")
  35.             reconnects -= 1
  36.         else:
  37.             print("Could not connect to ", str(address))
  38.             sys.exit(0)
  39.  
  40. print("New client created: {0}".format(c))
  41.  
  42. def handle_quat(data):
  43.     # Handle a (epoch_time, (w,x,y,z)) quaternion tuple.¬
  44.     epoch = data[0]
  45.     xyzaccu = data[1]
  46.     print("QUAT [{0}] W {1}, X {2}, Y {3}, Z {4}".format(epoch, *xyzaccu))
  47.  
  48. def handle_notification(data):
  49.     # Handle a (epoch_time, (x,y,z,accuracy)) corrected acc¬
  50.     # tuple.¬
  51.     epoch = data[0]
  52.     xyzaccu = data[1]
  53.     print("ACC [{0}] X: {1}, Y: {2}, Z: {3}".format(epoch, *xyzaccu[:-1]))
  54.  
  55. def handle_gyro(data):
  56.     # Handle a (epoch_time, (x,y,z,accuracy)) corrected gyro¬
  57.     # tuple.¬
  58.     epoch = data[0]
  59.     xyzaccu = data[1]
  60.     print("GYRO [{0}] X: {1}, Y: {2}, Z: {3}".format(epoch, *xyzaccu[:-1]))
  61.  
  62. def handle_euler(data):
  63.     # Handle a (epoch_time, (heading,pitch,roll,yaw)) euler angle tuple.¬
  64.     epoch = data[0]
  65.     xyzaccu = data[1]
  66.     print("EULER [{0}] Heading: {1}, Pitch: {2}, Roll: {3}, Yaw: {4}".format(
  67.         epoch, *xyzaccu))
  68.  
  69.  
  70. print("Write Sensor Fusion settings...")
  71. c.sensorfusion.set_mode(SensorFusionMode.NDOF)
  72. c.sensorfusion.set_acc_range(SensorFusionAccRange._8G)
  73. c.sensorfusion.set_gyro_range(SensorFusionGyroRange._1000DPS)
  74.  
  75. succeded = False
  76.  
  77. while not succeded:
  78.     try:
  79.         print("Set Time Processor to limit data rate to 50Hz for each channel")
  80.         #c.sensorfusion.set_sample_delay(SensorFusionData.EULER_ANGLE, 20)
  81.         c.sensorfusion.set_sample_delay(SensorFusionData.QUATERION, 20)
  82.         c.sensorfusion.set_sample_delay(SensorFusionData.CORRECTED_ACC, 20)
  83.         c.sensorfusion.set_sample_delay(SensorFusionData.CORRECTED_GYRO, 20)
  84.  
  85.         print("Subscribing to Sensor Fusion Quaternion signal notifications...")
  86.         #c.sensorfusion.notifications(euler_angle_callback=handle_euler)
  87.         #c.sensorfusion.notifications(quaternion_callback=handle_quat)
  88.         c.sensorfusion.notifications(corrected_acc_callback=handle_notification,
  89.                                      quaternion_callback=handle_quat,
  90.                                      corrected_gyro_callback=handle_gyro)
  91.  
  92.         succeded = True
  93.     except Exception, e:
  94.         print(e)
  95.         time.sleep(0.1)
  96.  
  97. time.sleep(4.0)
  98.  
  99. print("Unsubscribe to notification...")
  100. c.sensorfusion.notifications()
  101.  
  102. time.sleep(5.0)
  103.  
  104. c.disconnect()
RAW Paste Data