├── RepeatedTimer.py ├── LICENSE ├── README.md ├── Crc16.py ├── BluetoothService.py ├── SpatialSensorManager.py ├── buds_to_unity.py ├── Headtracking.py └── getBudsUDP.cs /RepeatedTimer.py: -------------------------------------------------------------------------------- 1 | import time 2 | from threading import Event, Thread 3 | 4 | class RepeatedTimer: 5 | 6 | """Repeat `function` every `interval` seconds.""" 7 | 8 | def __init__(self, interval, function, *args, **kwargs): 9 | self.interval = interval 10 | self.function = function 11 | self.args = args 12 | self.kwargs = kwargs 13 | self.start = time.time() 14 | self.event = Event() 15 | self.thread = Thread(target=self._target) 16 | self.thread.start() 17 | 18 | def _target(self): 19 | while not self.event.wait(self._time): 20 | self.function(*self.args, **self.kwargs) 21 | 22 | @property 23 | def _time(self): 24 | return self.interval - ((time.time() - self.start) % self.interval) 25 | 26 | def stop(self): 27 | self.event.set() 28 | self.thread.join() -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Tim Schneeberger 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Galaxy Buds Pro Headtracking 2 | Stream head-tracking data from the Samsung Galaxy Buds Pro in real-time 3 | 4 | ## Requirements 5 | 6 | ### Windows 7 | 8 | On Windows, make sure to install version 0.22 of PyBluez. 9 | The latest version (v0.23) does not work properly and fails to enumerate Bluetooth devices. 10 | ``` 11 | pip install PyBluez==0.22 12 | ``` 13 | 14 | ### Linux 15 | 16 | On Linux, you can just go ahead and install the latest version of PyBluez. 17 | ``` 18 | pip install PyBluez 19 | ``` 20 | 21 | ## Usage 22 | ``` 23 | python Headtracking.py --help 24 | ``` 25 | ``` 26 | usage: Headtracking.py [-h] [-v] [-t] mac-address 27 | 28 | Stream head-tracking data from the Galaxy Buds Pro 29 | 30 | positional arguments: 31 | mac-address MAC-Address of your Buds 32 | 33 | optional arguments: 34 | -h, --help show this help message and exit 35 | -v, --verbose Print debug information 36 | -t, --trace Trace Bluetooth serial traffic 37 | 38 | ``` 39 | Dump raw head-tracking data as a quaternion (4D vector): 40 | ``` 41 | python Headtracking.py 64:03:7f:2e:2b:3a 42 | ``` 43 | ``` 44 | x=0.0159, y=0.0096, z=0.0134, w=0.0245 45 | x=0.0179, y=0.0171, z=0.0127, w=0.0251 46 | x=0.0178, y=0.0172, z=0.0146, w=0.0000 47 | x=0.0168, y=0.0103, z=0.0056, w=0.0226 48 | x=0.0165, y=0.0123, z=0.0149, w=0.0252 49 | x=0.0164, y=0.0122, z=0.0151, w=0.0252 50 | x=0.0153, y=0.0085, z=0.0181, w=0.0000 51 | x=0.0172, y=0.0162, z=0.0178, w=0.0006 52 | x=0.0170, y=0.0156, z=0.0182, w=0.0007 53 | x=0.0177, y=0.0167, z=0.0141, w=0.0255 54 | x=0.0164, y=0.0120, z=0.0148, w=0.0252 55 | x=0.0167, y=0.0143, z=0.0182, w=0.0006 56 | ``` 57 | -------------------------------------------------------------------------------- /Crc16.py: -------------------------------------------------------------------------------- 1 | import array 2 | 3 | Crc16Tab = [0, 4129, 8258, 12387, 16516, 20645, 24774, 28903, 33032, 37161, 41290, 4 | 45419, 49548, 53677, 57806, 61935, 4657, 528, 12915, 8786, 21173, 17044, 29431, 25302, 5 | 37689, 33560, 45947, 41818, 54205, 50076, 62463, 58334, 9314, 13379, 1056, 5121, 25830, 6 | 29895, 17572, 21637, 42346, 46411, 34088, 38153, 58862, 62927, 50604, 54669, 13907, 7 | 9842, 5649, 1584, 30423, 26358, 22165, 18100, 46939, 42874, 38681, 34616, 63455, 59390, 8 | 55197, 51132, 18628, 22757, 26758, 30887, 2112, 6241, 10242, 14371, 51660, 55789, 9 | 59790, 63919, 35144, 39273, 43274, 47403, 23285, 19156, 31415, 27286, 6769, 2640, 10 | 14899, 10770, 56317, 52188, 64447, 60318, 39801, 35672, 47931, 43802, 27814, 31879, 11 | 19684, 23749, 11298, 15363, 3168, 7233, 60846, 64911, 52716, 56781, 44330, 48395, 12 | 36200, 40265, 32407, 28342, 24277, 20212, 15891, 11826, 7761, 3696, 65439, 61374, 13 | 57309, 53244, 48923, 44858, 40793, 36728, 37256, 33193, 45514, 41451, 53516, 49453, 14 | 61774, 57711, 4224, 161, 12482, 8419, 20484, 16421, 28742, 24679, 33721, 37784, 41979, 15 | 46042, 49981, 54044, 58239, 62302, 689, 4752, 8947, 13010, 16949, 21012, 25207, 29270, 16 | 46570, 42443, 38312, 34185, 62830, 58703, 54572, 50445, 13538, 9411, 5280, 1153, 29798, 17 | 25671, 21540, 17413, 42971, 47098, 34713, 38840, 59231, 63358, 50973, 55100, 9939, 18 | 14066, 1681, 5808, 26199, 30326, 17941, 22068, 55628, 51565, 63758, 59695, 39368, 19 | 35305, 47498, 43435, 22596, 18533, 30726, 26663, 6336, 2273, 14466, 10403, 52093, 20 | 56156, 60223, 64286, 35833, 39896, 43963, 48026, 19061, 23124, 27191, 31254, 2801, 21 | 6864, 10931, 14994, 64814, 60687, 56684, 52557, 48554, 44427, 40424, 36297, 31782, 22 | 27655, 23652, 19525, 15522, 11395, 7392, 3265, 61215, 65342, 53085, 57212, 44955, 23 | 49082, 36825, 40952, 28183, 32310, 20053, 24180, 11923, 16050, 3793, 7920] 24 | 25 | 26 | def crc16_ccitt(data): 27 | i2 = 0 28 | for i3 in range(0, len(data)): 29 | out = Crc16Tab[((i2 >> 8) ^ data[i3]) & 255] 30 | i2 = out ^ (i2 << 8) 31 | 32 | return 65535 & i2 33 | -------------------------------------------------------------------------------- /BluetoothService.py: -------------------------------------------------------------------------------- 1 | import struct 2 | from threading import Thread 3 | import Crc16 4 | 5 | 6 | class BluetoothService: 7 | 8 | def __init__(self, socket, message_callback=None, debug=False): 9 | # Constants 10 | self.MSG_SIZE_CRC = 2 11 | self.MSG_SIZE_ID = 1 12 | self.MSG_SIZE_MARKER = 1 13 | self.MSG_SIZE_TYPE = 1 14 | self.MSG_SIZE_BYTES = 1 15 | self.MSG_SOM_MARKER = 0xFD 16 | self.MSG_EOM_MARKER = 0xDD 17 | 18 | # Member variables 19 | self.message_cb = message_callback 20 | self.debug = debug 21 | self.socket = socket 22 | self.isDisposing = False 23 | self.thread = Thread(target=self.__run) 24 | self.thread.start() 25 | 26 | def __run(self): 27 | try: 28 | while not self.isDisposing: 29 | data = self.socket.recv(1024) 30 | if len(data) == 0: 31 | continue 32 | 33 | if self.debug: 34 | print(">> INCOMING: " + bytearray(data).hex(sep=' ')) 35 | 36 | if data[0] != self.MSG_SOM_MARKER: 37 | print("Warning: Invalid SOM received, corrupted message") 38 | continue 39 | 40 | if self.message_cb is not None: 41 | self.message_cb(data) 42 | 43 | except IOError: 44 | pass 45 | 46 | def __size(self, payload_length): 47 | return self.MSG_SIZE_ID + payload_length + self.MSG_SIZE_CRC 48 | 49 | def __totalPacketSize(self, payload_length): 50 | return self.__size(payload_length) + (self.MSG_SIZE_MARKER * 2) + self.MSG_SIZE_TYPE + self.MSG_SIZE_BYTES 51 | 52 | def close(self): 53 | self.isDisposing = True 54 | self.socket.close() 55 | 56 | def sendPacket(self, msg_id, payload, is_response=False, is_fragment=False): 57 | b = bytearray(self.__totalPacketSize(len(payload))) 58 | b[0] = self.MSG_SOM_MARKER 59 | 60 | header_b = 0 61 | if is_fragment: 62 | header_b = (header_b | 32) 63 | 64 | if is_response: 65 | header_b = (header_b | 16) 66 | 67 | b[1] = self.__size(len(payload)) 68 | b[2] = header_b 69 | b[3] = msg_id 70 | 71 | b[4:4 + len(payload)] = payload 72 | 73 | crc_data = bytearray(self.__size(len(payload)) - self.MSG_SIZE_CRC) 74 | crc_data[0] = msg_id 75 | crc_data[1:1 + len(payload)] = payload 76 | 77 | crc16 = Crc16.crc16_ccitt(crc_data) 78 | b[len(payload) + 4] = crc16 & 255 79 | b[len(payload) + 5] = (crc16 >> 8) & 255 80 | b[len(payload) + 6] = self.MSG_EOM_MARKER 81 | 82 | if self.debug: 83 | print("<< OUTGOING: " + bytearray(b).hex(sep=' ')) 84 | 85 | self.socket.send(bytes(b)) 86 | -------------------------------------------------------------------------------- /SpatialSensorManager.py: -------------------------------------------------------------------------------- 1 | from BluetoothService import BluetoothService 2 | from RepeatedTimer import RepeatedTimer 3 | 4 | import struct 5 | 6 | 7 | class SpatialSensorManager: 8 | def __init__(self, socket, data_callback, verbose=False, debug=False): 9 | # Constants 10 | self.CID_ATTACH = 0 11 | self.CID_DETACH = 1 12 | self.CID_ATTACH_ACK = 2 13 | self.CID_DETACH_ACK = 3 14 | self.CID_KEEP_ALIVE = 4 15 | self.CID_WEAR_ON_OFF = 5 16 | 17 | self.DID_BUDGRV = 32 18 | self.DID_GYROCAL = 35 19 | self.DID_SENSOR_STUCK = 36 20 | self.DID_WEAR_OFF = 34 21 | self.DID_WEAR_ON = 33 22 | 23 | self.MSG_SPATIAL_AUDIO_ENABLE = 124 24 | self.MSG_SPATIAL_AUDIO_DATA = 194 25 | self.MSG_SPATIAL_AUDIO_CONTROL = 195 26 | 27 | # Member variables 28 | self.data_cb = data_callback 29 | self.verbose = verbose 30 | self.service = BluetoothService(socket, self.__onMessageReceived, debug) 31 | self.timer = None 32 | 33 | def attach(self): 34 | self.service.sendPacket(self.MSG_SPATIAL_AUDIO_ENABLE, bytes(1)) 35 | self.service.sendPacket(self.MSG_SPATIAL_AUDIO_CONTROL, bytes(self.CID_ATTACH)) 36 | self.timer = RepeatedTimer(2, self.__keepAlive) 37 | 38 | def detach(self): 39 | self.service.sendPacket(self.MSG_SPATIAL_AUDIO_CONTROL, bytes(self.CID_DETACH)) 40 | self.service.sendPacket(self.MSG_SPATIAL_AUDIO_ENABLE, bytes(0)) 41 | self.timer.stop() 42 | 43 | def __keepAlive(self): 44 | self.service.sendPacket(self.MSG_SPATIAL_AUDIO_CONTROL, bytes(self.CID_KEEP_ALIVE)) 45 | 46 | def __extract_data(self, byte, i, i2, z): 47 | if len(byte) >= i + i2: 48 | i4 = 0 49 | i3 = 0 50 | if z: 51 | while i4 < i2: 52 | i3 += (byte[i + i4] & 255) << (((i2 - 1) - i4) * 8) 53 | i4 += 1 54 | else: 55 | while i4 < i2: 56 | i3 += (byte[i + i4] & 255) << (i4 * 8) 57 | i4 += 1 58 | return i3 59 | else: 60 | return -2 61 | 62 | def __onMessageReceived(self, data): 63 | if data[3] == self.MSG_SPATIAL_AUDIO_CONTROL: 64 | cid = data[4] 65 | if self.verbose and cid == self.CID_ATTACH_ACK: 66 | print("Attach successful (ACK)") 67 | elif self.verbose and cid == self.CID_DETACH_ACK: 68 | print("Detach successful (ACK)") 69 | elif data[3] == self.MSG_SPATIAL_AUDIO_DATA: 70 | event = data[4] 71 | if event == self.DID_SENSOR_STUCK and self.verbose: 72 | print("Warning: Sensor stuck") 73 | elif event == self.DID_GYROCAL and self.verbose: 74 | # For more information refer to https://github.com/ThePBone/GalaxyBudsClient/blob/master/GalaxyBudsClient/Message/Decoder/SpatialAudioDataParser.cs#L56 75 | print("Received gyro bias info from device") 76 | elif event == self.DID_BUDGRV: 77 | 78 | payload = data[5:len(data) - 3] 79 | input = payload[:-1] 80 | if len(input) == 8: 81 | quaternion = [one / 10000.0 for one in struct.unpack('hhhh', payload[:-1])] 82 | if self.data_cb is not None: 83 | self.data_cb(quaternion, self) 84 | else: 85 | if self.verbose: 86 | print("diff length: ", len(input)) 87 | -------------------------------------------------------------------------------- /buds_to_unity.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jun 22 23:13:48 2021 4 | 5 | @author: JY 6 | """ 7 | 8 | import time 9 | import socket 10 | 11 | import bluetooth 12 | import sys 13 | import argparse 14 | 15 | from SpatialSensorManager import SpatialSensorManager 16 | 17 | SENDING_STR = "0.00,0.00,0.00,0.00" 18 | import numpy as np 19 | 20 | def quaternion_to_euler_angle_vectorized1(w, x, y, z): 21 | ysqr = y * y 22 | 23 | t0 = +2.0 * (w * x + y * z) 24 | t1 = +1.0 - 2.0 * (x * x + ysqr) 25 | X = np.degrees(np.arctan2(t0, t1)) 26 | 27 | t2 = +2.0 * (w * y - z * x) 28 | t2 = np.where(t2>+1.0,+1.0,t2) 29 | #t2 = +1.0 if t2 > +1.0 else t2 30 | 31 | t2 = np.where(t2<-1.0, -1.0, t2) 32 | #t2 = -1.0 if t2 < -1.0 else t2 33 | Y = np.degrees(np.arcsin(t2)) 34 | 35 | t3 = +2.0 * (w * z + x * y) 36 | t4 = +1.0 - 2.0 * (ysqr + z * z) 37 | Z = np.degrees(np.arctan2(t3, t4)) 38 | 39 | return X, Y, Z 40 | 41 | 42 | def __spatial_sensor_callback(quaternion): 43 | global SENDING_STR 44 | # This parameter is a float list describing a raw quaternion (4D vector) 45 | # The values are ordered like this: x, y, z, w 46 | 47 | euler_ = quaternion_to_euler_angle_vectorized1(quaternion[3], quaternion[0], quaternion[1], quaternion[2]) 48 | SENDING_STR = f"{euler_[0]:0.03f},{euler_[1]:0.03f},{euler_[2]:0.03f} | " 49 | SENDING_STR += f"{quaternion[0]},{quaternion[1]},{quaternion[2]},{quaternion[3]}" 50 | print(SENDING_STR) 51 | 52 | # Conversion examples (C#): https://github.com/ThePBone/GalaxyBudsClient/blob/master/GalaxyBudsClient/Utils/QuaternionExtensions.cs#L48 53 | 54 | # UDP part 55 | UDP_IP = "127.0.0.1" 56 | UDP_PORT = 12562 57 | MESSAGE = "Hello, World!" 58 | 59 | print("UDP target IP:", UDP_IP) 60 | print("UDP target port:", UDP_PORT) 61 | 62 | data_type = "dffffffffffqd" 63 | 64 | udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP 65 | 66 | 67 | # buds part 68 | mac = "64:03:7F:85:D3:0C" 69 | verbose = True 70 | trace = False 71 | 72 | 73 | if verbose: 74 | print(str(bluetooth.lookup_name(mac))) 75 | print("Searching for RFCOMM interface...") 76 | 77 | service_matches = bluetooth.find_service(uuid="00001101-0000-1000-8000-00805F9B34FB", address=mac) 78 | 79 | port = host = None 80 | for match in service_matches: 81 | if match["name"] == b"GEARMANAGER": 82 | port = match["port"] 83 | host = match["host"] 84 | break 85 | 86 | if port is None or host is None: 87 | print("Couldn't find the proprietary RFCOMM service") 88 | sys.exit(1) 89 | 90 | if verbose: 91 | print("RFCOMM interface found. Establishing connection...") 92 | 93 | sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM) 94 | sock.connect((host, port)) 95 | 96 | if verbose: 97 | print("Connected to device.") 98 | 99 | sensor = None 100 | try: 101 | sensor = SpatialSensorManager(sock, __spatial_sensor_callback, verbose, trace) 102 | sensor.attach() 103 | 104 | while True: 105 | udp_sock.sendto(bytes(SENDING_STR, "utf-8"), (UDP_IP, UDP_PORT)) 106 | time.sleep(0.01) 107 | 108 | except KeyboardInterrupt: 109 | if sensor is not None: 110 | sensor.detach() 111 | 112 | #%% 113 | # simple inquiry example 114 | import bluetooth 115 | 116 | nearby_devices = bluetooth.discover_devices(lookup_names=True) 117 | print("Found {} devices.".format(len(nearby_devices))) 118 | 119 | for addr, name in nearby_devices: 120 | print(" {} - {}".format(addr, name)) -------------------------------------------------------------------------------- /Headtracking.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | A python script to stream head-tracking data from the Samsung Galaxy Buds Pro 5 | """ 6 | 7 | # License: MIT 8 | # Author: @ThePBone 9 | # 04/26/2021 10 | import os 11 | import signal 12 | import time 13 | 14 | import bluetooth 15 | import sys 16 | import argparse 17 | 18 | from SpatialSensorManager import SpatialSensorManager 19 | 20 | timeAtStart = -1 21 | timeAtLastEvent = -1 22 | benchmarkHistory = [] 23 | doBenchmark = False 24 | doBenchmarkAfterNIterations = 0 25 | benchmarkCount = 100 26 | benchmarkIteration = 0 27 | 28 | 29 | def __spatial_sensor_callback(quaternion, sensor_manager): 30 | global timeAtLastEvent, benchmarkHistory, doBenchmark, benchmarkCount,\ 31 | benchmarkIteration, doBenchmarkAfterNIterations, timeAtStart 32 | if sensor_manager.service.isDisposing: 33 | return 34 | 35 | # This parameter is a float list describing a raw quaternion (4D vector) 36 | # The values are ordered like this: x, y, z, w 37 | # Conversion examples (C#): https://github.com/ThePBone/GalaxyBudsClient/blob/master/GalaxyBudsClient/Utils/QuaternionExtensions.cs#L48 38 | 39 | if not doBenchmark: 40 | print(f"x={quaternion[0]}, y={quaternion[1]}, z={quaternion[2]}, w={quaternion[3]}") 41 | return 42 | 43 | if timeAtStart < 0 and benchmarkIteration >= doBenchmarkAfterNIterations - 1: 44 | timeAtStart = time.time_ns() 45 | 46 | if timeAtLastEvent >= 0 and benchmarkIteration >= doBenchmarkAfterNIterations: 47 | benchmarkHistory.append((time.time_ns() - timeAtLastEvent) / 1e+6) 48 | 49 | if len(benchmarkHistory) >= benchmarkCount - 1: 50 | total_duration = round((time.time_ns() - timeAtStart) / 1e+9, 6) 51 | print("====== BENCHMARK DONE ======") 52 | if doBenchmarkAfterNIterations > 0: 53 | print("Benchmark launched after skipping " + str(doBenchmarkAfterNIterations) + " frames") 54 | print("Motion frames received: " + str(len(benchmarkHistory) + 1)) 55 | print("Average time between frames: " + str(round(sum(benchmarkHistory) / len(benchmarkHistory), 6)) + "ms") 56 | print("Minimum time between frames: " + str(round(min(benchmarkHistory), 6)) + "ms") 57 | print("Maximum time between frames: " + str(round(max(benchmarkHistory), 6)) + "ms") 58 | print("Total benchmark duration: " + str(total_duration) + "s") 59 | 60 | sensor_manager.detach() 61 | sensor_manager.service.close() 62 | 63 | benchmarkIteration += 1 64 | timeAtLastEvent = time.time_ns() 65 | 66 | 67 | def main(): 68 | global timeAtLastEvent, benchmarkHistory, doBenchmark, benchmarkCount, doBenchmarkAfterNIterations 69 | parser = argparse.ArgumentParser(description='Stream head-tracking data from the Galaxy Buds Pro') 70 | parser.add_argument('mac', metavar='mac-address', type=str, nargs=1, 71 | help='MAC-Address of your Buds') 72 | parser.add_argument('-b', '--benchmark', action='store_true', help="Perform benchmark") 73 | parser.add_argument('--benchmark-count', metavar="n", default=[benchmarkCount], nargs=1, type=int, 74 | help="Stop benchmark after benchmarking N frames") 75 | parser.add_argument('--benchmark-delay', metavar="n", default=[doBenchmarkAfterNIterations], nargs=1, type=int, 76 | help="Start benchmark after receiving/skipping N frames (to wait until the connection stabilizes)") 77 | parser.add_argument('-v', '--verbose', action='store_true', help="Print debug information") 78 | parser.add_argument('-t', '--trace', action='store_true', help="Trace Bluetooth serial traffic") 79 | args = parser.parse_args() 80 | 81 | doBenchmark = args.benchmark 82 | benchmarkCount = args.benchmark_count[0] 83 | doBenchmarkAfterNIterations = args.benchmark_delay[0] 84 | verbose = args.verbose 85 | trace = args.trace 86 | 87 | if verbose: 88 | print(str(bluetooth.lookup_name(args.mac[0]))) 89 | print("Searching for RFCOMM interface...") 90 | 91 | service_matches = bluetooth.find_service(uuid="00001101-0000-1000-8000-00805F9B34FB", address=str(args.mac[0])) 92 | 93 | port = host = None 94 | for match in service_matches: 95 | if match["name"] == "GEARMANAGER" or match["name"] == b"GEARMANAGER" or match["name"] == b"FACTORY" or match["name"] == "FACTORY": 96 | port = match["port"] 97 | host = match["host"] 98 | break 99 | 100 | if port is None or host is None: 101 | print("Couldn't find the proprietary RFCOMM service") 102 | sys.exit(1) 103 | 104 | if verbose: 105 | print("RFCOMM interface found. Establishing connection...") 106 | 107 | sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM) 108 | sock.connect((host, port)) 109 | 110 | if verbose: 111 | print("Connected to device.") 112 | 113 | sensor = None 114 | try: 115 | sensor = SpatialSensorManager(sock, __spatial_sensor_callback, verbose, trace) 116 | sensor.attach() 117 | 118 | while not sensor.service.isDisposing: 119 | time.sleep(1) 120 | 121 | except KeyboardInterrupt: 122 | if sensor is not None: 123 | sensor.detach() 124 | 125 | 126 | if __name__ == "__main__": 127 | main() 128 | -------------------------------------------------------------------------------- /getBudsUDP.cs: -------------------------------------------------------------------------------- 1 | using System.Collections; 2 | using System.Collections.Generic; 3 | using UnityEngine; 4 | /* 5 | * Modified from........ 6 | * 7 | * Author: Hyung-il Kim 8 | * M.S. Student, KAIST UVR Lab. 9 | * 10 | * Receives rotation vector as quaternion, by UDP comm. 11 | * x, y, z, w 12 | * 13 | * - Recenter rotation using space bar 14 | */ 15 | 16 | using System; 17 | using System.Text; 18 | using System.Net; 19 | using System.Net.Sockets; 20 | using System.Threading; 21 | 22 | public class getBudsUDP : MonoBehaviour { 23 | 24 | // receiving Thread 25 | Thread receiveThread; 26 | 27 | // udpclient object 28 | UdpClient client; 29 | 30 | // port number 31 | public String TARGET_IP = "127.0.0.1"; 32 | public int TARGET_PORT = 12563; 33 | 34 | public int RECEIVING_PORT = 12562; // define > init 35 | 36 | public float ID = -1; 37 | public Vector3 gyro = new Vector3(.0f,.0f,.0f); 38 | public Vector3 acc = new Vector3(.0f,.0f,.0f); 39 | public Vector4 rot = new Vector4(.0f,.0f,.0f,.0f); 40 | 41 | public int sent_num = 0; 42 | 43 | 44 | 45 | public getBudsUDP(int port){ 46 | this.RECEIVING_PORT = port; 47 | } 48 | 49 | IEnumerator Start() 50 | { 51 | Debug.Log("UDPSendReceive: Starting"); 52 | this.init(); 53 | 54 | yield return new WaitForSeconds(1.0f); 55 | 56 | } 57 | void Update() 58 | { 59 | if (Input.GetKeyDown("space")) 60 | { 61 | print("space key was pressed"); 62 | sent_num += 1; 63 | 64 | UdpClient udpClient = new UdpClient(TARGET_IP, TARGET_PORT); 65 | Debug.Log("UDPSendReceive: sending"+TARGET_IP+":"+TARGET_PORT); 66 | 67 | 68 | byte[] sendBytes2 = ConvertDoubleToByte(new double[]{sent_num, sent_num*0.3, sent_num*0.5}); 69 | 70 | try{ 71 | // Byte[] sendBytes = Encoding.ASCII.GetBytes("sending test"+sent_num); 72 | // udpClient.Send(sendBytes, sendBytes.Length); 73 | 74 | 75 | udpClient.Send(sendBytes2, sendBytes2.Length); 76 | } 77 | catch ( Exception e ){ 78 | Console.WriteLine( e.ToString()); 79 | } 80 | } 81 | transform.eulerAngles = new Vector3(rot.x,rot.y,rot.z); 82 | // transform.Rotate(rot, Space.Self); 83 | // Debug.Log("UDPSendReceive: receiveThread background=" + receiveThread.IsBackground); 84 | } 85 | 86 | // OnDestroy 87 | public void OnDestroy() 88 | { 89 | receiveThread.Abort(); 90 | client.Close(); 91 | Debug.Log("UDPSendReceive: OnDestroy"); 92 | } 93 | 94 | // init 95 | private void init() 96 | { 97 | // Define local endpoint (where messages are received). 98 | // Create a new thread for the reception create incoming messages. 99 | receiveThread = new Thread(new ThreadStart(ReceiveData)); 100 | receiveThread.IsBackground = false; 101 | receiveThread.Start(); 102 | } 103 | 104 | 105 | 106 | public static float[] ConvertByteToFloat(byte[] array) 107 | { 108 | float[] floatArr = new float[10]; 109 | for (int i = 0; i < floatArr.Length; i++) 110 | { 111 | // if (BitConverter.IsLittleEndian) 112 | // { 113 | // Array.Reverse(array, i * 4, 4); 114 | // } 115 | floatArr[i] = BitConverter.ToSingle(array, i * 4); 116 | } 117 | return floatArr; 118 | } 119 | public static byte[] ConvertDoubleToByte(double[] array) 120 | { 121 | byte[] byte_arr = new byte[8*array.Length]; 122 | for (int i = 0; i < array.Length; i++) 123 | { 124 | byte[] bytes = BitConverter.GetBytes( array[i]); 125 | Array.Copy(bytes, 0, byte_arr , 8*i, bytes.Length); 126 | } 127 | return byte_arr; 128 | } 129 | 130 | 131 | 132 | // receive thread 133 | private void ReceiveData() 134 | { 135 | //char[] delim = {'#'}; 136 | 137 | client = new UdpClient(RECEIVING_PORT); 138 | Debug.Log("UDPSendReceive: listening at port.... " + RECEIVING_PORT); 139 | while (true) 140 | { 141 | try 142 | { 143 | // Bytes received. 144 | IPEndPoint anyIP = new IPEndPoint(IPAddress.Any, 0); 145 | // Debug.Log("UDPSendReceive: get input"); 146 | byte[] data = client.Receive(ref anyIP); 147 | String receive_str = Encoding.UTF8.GetString(data); 148 | // Debug.Log("UDPSendReceive: " + receive_str); 149 | 150 | string[] subs = receive_str.Split(','); 151 | rot.x = float.Parse(subs[0]); 152 | rot.y = float.Parse(subs[1]); 153 | rot.z = float.Parse(subs[2]); 154 | // rot.w = float.Parse(subs[3]); 155 | 156 | // Array.Reverse(data, 0, 4); 157 | // int num_of_input_set = BitConverter.ToInt16(data, 0); 158 | // Debug.Log("UDPSendReceive: num_of_input_set:" + num_of_input_set); 159 | 160 | 161 | 162 | 163 | // for (int i = 0; i < num_of_input_set; i++) 164 | // { 165 | // byte[] oneset_arr = new byte[52]; 166 | 167 | // Array.Copy(data, 4+52*i, oneset_arr, 0, 52); 168 | // // convert received byte array to float array. 169 | // float[] values = ConvertByteToFloat(oneset_arr); // Gx,Gy,Gz, ACCx,ACCy,ACCz, ROTx,ROTy,ROTz,ROTw 170 | 171 | // // Array.Reverse(oneset_arr, 40, 8); 172 | // // Array.Reverse(oneset_arr, 48, 4); 173 | 174 | // double device_time = BitConverter.ToInt64(oneset_arr, 40); 175 | // int dev_id_int = BitConverter.ToInt16(oneset_arr, 48); 176 | // Debug.Log("UDPSendReceive: device_time:" + device_time); 177 | // Debug.Log("UDPSendReceive: dev_id:" + dev_id_int); 178 | 179 | // // these values can be modified.. 180 | // gyro[0] = values[0]; 181 | // gyro[1] = values[1]; 182 | // gyro[2] = values[2]; 183 | 184 | // acc[0] = values[3]; 185 | // acc[1] = values[4]; 186 | // acc[2] = values[5]; 187 | 188 | // rot.x = values[6]; 189 | // rot.y = values[7]; 190 | // rot.z = values[8]; 191 | // rot.w = values[9]; 192 | // Debug.Log("UDPSendReceive: sensorval:" + values[0]+","+ values[1]+","+ values[2]+"|"+ 193 | // values[3] + "," + values[4] + "," + values[5]+"|"+ 194 | // values[6] + "," + values[7] + "," + values[8] + "," + values[9]); 195 | 196 | 197 | // } 198 | 199 | 200 | } 201 | catch (Exception err) 202 | { 203 | print(err.ToString()); 204 | } 205 | } 206 | } 207 | 208 | } --------------------------------------------------------------------------------