├── .gitignore ├── requirements.txt ├── device_addresses.csv.example ├── app_logger.py ├── notifications.py ├── README.md ├── database.py ├── server.py └── blescan.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | __pycache__/* 3 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | bluepy==1.3.0 2 | numpy -------------------------------------------------------------------------------- /device_addresses.csv.example: -------------------------------------------------------------------------------- 1 | d4:ca:6e:12:34:56 2 | d4:ca:6e:12:34:57 3 | d4:ca:6e:12:34:58 4 | d4:ca:6e:12:34:59 5 | d4:ca:6e:12:34:60 -------------------------------------------------------------------------------- /app_logger.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Write logs to file specified in `settings.py` 3 | Logfile name is specifies 4 | ''' 5 | import logging 6 | 7 | 8 | class Logger(): 9 | def __init__(self, module_name): 10 | ''' 11 | Creates two loggers. One for the logfile, one for command-line output. 12 | ''' 13 | self.logger = logging.getLogger(module_name) 14 | self.logger.setLevel(logging.DEBUG) 15 | fh = logging.FileHandler("log.log") 16 | fh.setLevel(logging.DEBUG) 17 | sh = logging.StreamHandler() 18 | sh.setLevel(logging.DEBUG) 19 | logfile_formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s') 20 | user_facing_formatter = logging.Formatter('%(message)s') 21 | fh.setFormatter(logfile_formatter) 22 | sh.setFormatter(user_facing_formatter) 23 | self.logger.addHandler(fh) 24 | self.logger.addHandler(sh) -------------------------------------------------------------------------------- /notifications.py: -------------------------------------------------------------------------------- 1 | ''' 2 | This class allows a Python script to listen for data from an Xsens DOT. 3 | 4 | You can attach this "delegate" to an Xsens DOT with `Peripheral.setDelegate()`. 5 | NOTE: We give the Delegate class the address so it can print which device 6 | the data came from. 7 | For example: 8 | my_periph = Peripheral("ab:cd:ef:12:34:56") 9 | delegate = XsensNotificationDelegate(my_periph.addr) 10 | my_periph.setDelegate( 11 | ''' 12 | 13 | from bluepy import btle 14 | import numpy as np 15 | 16 | class XsensNotificationDelegate(btle.DefaultDelegate): 17 | def __init__(self, bluetooth_device_address): 18 | btle.DefaultDelegate.__init__(self) 19 | self.bluetooth_device_address = bluetooth_device_address 20 | print("XsensNotificationDelegate has been initialized") 21 | 22 | # Do something when sensor data is received. In this case, print it out. 23 | def handleNotification(self, cHandle, data): 24 | data_segments = np.dtype([('timestamp', np.uint32), ('quat_w', np.float32), ('quat_x', np.float32), 25 | ('quat_y', np.float32), ('quat_z', np.float32)]) 26 | human_readable_data = np.frombuffer(data, dtype=data_segments) 27 | formatted_data = str([x for x in human_readable_data.tolist()[0]][:-1])[1:-1] 28 | formatted_data = self.bluetooth_device_address + ", " + formatted_data 29 | print(formatted_data) 30 | 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Xsens DOT Python Server 2 | 3 | Connect to Xsens DOT wearable sensors via Python. 4 | 5 | ### Requirements 6 | * [Bluepy](https://github.com/IanHarvey/bluepy) 7 | * [Numpy](https://numpy.org/) 8 | * One or more [Xsens DOT sensors](https://www.xsens.com/xsens-dot) 9 | 10 | ### Installation 11 | 12 | Enter this in your command prompt or terminal to install necessary Python modules via Pip: 13 | `pip install -r requirements.txt` 14 | 15 | ### How to Run 16 | There are generally two steps to connecting to any Bluetooth device: Scanning for all Bluetooth devices near you, and pairing/connecting to the device. 17 | 1. Find the DOTs with `$ python blescan.py`. `blescan.py` finds all Bluetooth devices in your area, including phones, laptops, and lightbulbs, and will print them all to the console/command line/Terminal. 18 | 2. Add the addresses of only the Xsens DOTs that were found to `device_addresses.csv`. The address will look like `aa:bb:cc:12:34:56`. 19 | 3. Connect to and start getting data from the DOTs by running `$ python server.py`. DOT data will be printed to the console. 20 | 21 | ### Known Bugs 22 | There is a known issue where DOTs may send out messages that only have half the data elements they're supposed to. For example, if you turn on a mode that is supposed to give you accelerometer data and gyroscope data, you might only get accelerometer data. This may be an issue with the code in this repository or with the original Xsens code. 23 | 24 | There is an [issue open here with Xsens themselves](https://github.com/xsens/xsens_dot_server/issues/11), but they're a Node shop rather than a Python shop, so the issue was not looked into further. 25 | 26 | -------------------------------------------------------------------------------- /database.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Handle database interactions. 3 | 4 | Read and write discovered Xsens DOTs. 5 | ''' 6 | 7 | import sqlite3 8 | from app_logger import Logger 9 | 10 | 11 | logger = Logger(__name__) 12 | 13 | 14 | class XsensDOTDatabase: 15 | def __init__(self, name="database.sqlite"): 16 | logger.logger.info("Connecting to database: {}...".format(name)) 17 | 18 | self.conn = sqlite3.connect(name) 19 | self.cursor = self.conn.cursor() 20 | 21 | self._create_tables() 22 | 23 | 24 | def add_device(self, ble_address, status, signal_strength): 25 | # Add an Xsens DOT device to the database 26 | status = "Disconnected" 27 | local_name = "Xsens DOT" 28 | if self._device_already_found(ble_address): 29 | logger.logger.debug("Device with address {} already in database.".format(ble_address)) 30 | else: 31 | self.cursor.execute("INSERT INTO devices VALUES (?,?,?,?,?)", 32 | ([None, ble_address, local_name, status, signal_strength])) 33 | self.conn.commit() 34 | 35 | def _create_tables(self): 36 | if not self._table_exists("devices"): 37 | logger.logger.debug("`devices` table does not exist. Creating...") 38 | self._create_device_table() 39 | 40 | def _table_exists(self, name): 41 | # Check if a table of `name` exists. If SELECT fails, it does not. 42 | try: 43 | self.cursor.execute("SELECT * FROM {}".format(name)) 44 | except sqlite3.OperationalError as e: 45 | logger.logger.error(e) 46 | return False 47 | except Error as e: 48 | logger.logger.error(e) 49 | return False 50 | return True 51 | 52 | def _create_device_table(self): 53 | # Create a table to store scanned Bluetooth devices in 54 | try: 55 | self.cursor.execute('''CREATE TABLE devices 56 | (device_id INTEGER PRIMARY KEY, 57 | ble_address varchar(17) NOT NULL , 58 | local_name varchar(20), 59 | status varchar(20), 60 | signal_strength_dbm INTEGER)''') 61 | except sqlite3.OperationalError as e: 62 | logger.logger.error(e) 63 | 64 | def _device_already_found(self, ble_address): 65 | device_cursor = self.cursor.execute( 66 | "SELECT * FROM devices WHERE ble_address = '{}'".format(ble_address)) 67 | devices = device_cursor.fetchall() 68 | if len(devices) == 0: 69 | return False 70 | return True 71 | 72 | -------------------------------------------------------------------------------- /server.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Server that connects to and grabs data from Xsens DOT wearable IMUs. 3 | 4 | Steps involved: 5 | - Connect to devices via their Bluetooth addresses. Add these to `device_addresses.csv` 6 | - Enable the CCCD for each device 7 | - Enable the Control Characteristic on each device 8 | - Set a notification delegate for each device to get actual sensor data 9 | - Start a loop to listen for sensor data 10 | 11 | NOTES: 12 | - Handling of sensor data is determined in `XsensNotificationDelegate.handleNotification()` 13 | - CCCD = Client Characteristic Configuration Descriptor 14 | ''' 15 | 16 | 17 | from bluepy import btle 18 | import csv 19 | 20 | from notifications import XsensNotificationDelegate 21 | 22 | '''Handles and enable messages for Xsens DOTs' Bluetooth characteristics''' 23 | CONTROL_CHARACTERISTIC = 32 24 | SHORT_PAYLOAD_CHARACTERISTIC = 43 25 | SHORT_PAYLOAD_CCCD = 44 26 | MEDIUM_PAYLOAD_CHARACTERISTIC = 39 27 | MEDIUM_PAYLOAD_CCCD = 40 28 | CCCD_ENABLE_MESSAGE = b"\x01\x00" 29 | ENABLE_MESSAGE = b"\x01\x01\x06" 30 | 31 | '''Connect to Xsens DOTs via their Bluetooth addresses listed in `device_addresses.csv`. 32 | You should use a command line tool to scan for these addresses or use Bluepy's `blescan.py`. 33 | Scanning using this server is not yet supported.''' 34 | ble_address_file = csv.reader(open("device_addresses.csv")) 35 | bluetooth_addresses = [x[0] for x in ble_address_file] 36 | 37 | peripherals = [] 38 | for addr in bluetooth_addresses: 39 | try: 40 | periph = btle.Peripheral(addr) 41 | peripherals.append(periph) 42 | except btle.BTLEDisconnectError: 43 | print("ERROR: Unable to connect to device at address {}".format(periph.addr)) 44 | print("Connected_peripherals: {}".format(peripherals)) 45 | 46 | '''Enable the CCCD for each device. This is necessary in addition to enabling the 47 | measurement characteristic. The CCCD handles for medium payloads and short payloads are 48 | different.''' 49 | for periph in peripherals: 50 | periph.writeCharacteristic(SHORT_PAYLOAD_CCCD, CCCD_ENABLE_MESSAGE, withResponse=True) 51 | print("CCCD enabled for device: {}".format(periph.addr)) 52 | 53 | '''Enable the Control Characteristic on each device. This is the same whether we are 54 | doing a short or medium payload measurement''' 55 | print("ENABLE_MESSAGE: {}".format(ENABLE_MESSAGE)) 56 | for periph in peripherals: 57 | periph.writeCharacteristic(CONTROL_CHARACTERISTIC, ENABLE_MESSAGE, withResponse=True) 58 | print("Measurement enabled for device: {}".format(periph.addr)) 59 | print(periph.readCharacteristic(CONTROL_CHARACTERISTIC)) 60 | 61 | for periph in peripherals: 62 | delegate = XsensNotificationDelegate(periph.addr) 63 | periph.setDelegate(delegate) 64 | 65 | i = 1 66 | while True: 67 | for periph in peripherals: 68 | if periph.waitForNotifications(1.0): 69 | pass 70 | else: 71 | print("No notification/data from sensor {} was received".format(periph.addr)) 72 | i += 1 73 | 74 | -------------------------------------------------------------------------------- /blescan.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Scan for Bluetooth LE devices in the area. 3 | 4 | This is a slightly altered version of `blescan.py` from the IanHarvey/bluepy repository. 5 | 6 | ''' 7 | 8 | #!/usr/bin/env python 9 | import argparse 10 | import binascii 11 | import os 12 | import sys 13 | from bluepy import btle 14 | 15 | from database import XsensDOTDatabase 16 | 17 | if os.getenv('C', '1') == '0': 18 | ANSI_RED = '' 19 | ANSI_GREEN = '' 20 | ANSI_YELLOW = '' 21 | ANSI_CYAN = '' 22 | ANSI_WHITE = '' 23 | ANSI_OFF = '' 24 | else: 25 | ANSI_CSI = "\033[" 26 | ANSI_RED = ANSI_CSI + '31m' 27 | ANSI_GREEN = ANSI_CSI + '32m' 28 | ANSI_YELLOW = ANSI_CSI + '33m' 29 | ANSI_CYAN = ANSI_CSI + '36m' 30 | ANSI_WHITE = ANSI_CSI + '37m' 31 | ANSI_OFF = ANSI_CSI + '0m' 32 | 33 | 34 | def dump_services(dev): 35 | services = sorted(dev.services, key=lambda s: s.hndStart) 36 | for s in services: 37 | print ("\t%04x: %s" % (s.hndStart, s)) 38 | if s.hndStart == s.hndEnd: 39 | continue 40 | chars = s.getCharacteristics() 41 | for i, c in enumerate(chars): 42 | props = c.propertiesToString() 43 | h = c.getHandle() 44 | if 'READ' in props: 45 | val = c.read() 46 | if c.uuid == btle.AssignedNumbers.device_name: 47 | string = ANSI_CYAN + '\'' + \ 48 | val.decode('utf-8') + '\'' + ANSI_OFF 49 | elif c.uuid == btle.AssignedNumbers.device_information: 50 | string = repr(val) 51 | else: 52 | string = '' 53 | else: 54 | string = '' 55 | print ("\t%04x: %-59s %-12s %s" % (h, c, props, string)) 56 | 57 | while True: 58 | h += 1 59 | if h > s.hndEnd or (i < len(chars) - 1 and h >= chars[i + 1].getHandle() - 1): 60 | break 61 | try: 62 | val = dev.readCharacteristic(h) 63 | print ("\t%04x: <%s>" % 64 | (h, binascii.b2a_hex(val).decode('utf-8'))) 65 | except btle.BTLEException: 66 | break 67 | 68 | 69 | class ScannerDatabase(btle.DefaultDelegate): 70 | 71 | def __init__(self, opts): 72 | btle.DefaultDelegate.__init__(self) 73 | self.opts = opts 74 | 75 | self.xsens_db = XsensDOTDatabase() 76 | 77 | def handleDiscovery(self, dev, isNewDev, isNewData): 78 | if isNewDev: 79 | status = "new" 80 | elif isNewData: 81 | if self.opts.new: 82 | return 83 | status = "update" 84 | else: 85 | if not self.opts.all: 86 | return 87 | status = "old" 88 | 89 | if dev.rssi < self.opts.sensitivity: 90 | return 91 | 92 | print (' Device (%s): %s (%s), %d dBm %s' % 93 | (status, 94 | ANSI_WHITE + dev.addr + ANSI_OFF, 95 | dev.addrType, 96 | dev.rssi, 97 | ('' if dev.connectable else '(not connectable)')) 98 | ) 99 | for (sdid, desc, val) in dev.getScanData(): 100 | if sdid in [8, 9]: 101 | if dev.addr[:8] == "d4:ca:6e": # Is an Xsens DOT device 102 | self.xsens_db.add_device(dev.addr, None, dev.rssi) 103 | print ('\t' + desc + ': \'' + ANSI_CYAN + val + ANSI_OFF + '\'') 104 | else: 105 | if dev.addr[:8] == "d4:ca:6e": # Is an Xsens DOT device 106 | self.xsens_db.add_device(dev.addr, None, dev.rssi) 107 | print ('\t' + desc + ': <' + val + '>') 108 | if not dev.scanData: 109 | print ('\t(no data)') 110 | print 111 | 112 | def main(): 113 | parser = argparse.ArgumentParser() 114 | parser.add_argument('-i', '--hci', action='store', type=int, default=0, 115 | help='Interface number for scan') 116 | parser.add_argument('-t', '--timeout', action='store', type=int, default=4, 117 | help='Scan delay, 0 for continuous') 118 | parser.add_argument('-s', '--sensitivity', action='store', type=int, default=-128, 119 | help='dBm value for filtering far devices') 120 | parser.add_argument('-d', '--discover', action='store_true', 121 | help='Connect and discover service to scanned devices') 122 | parser.add_argument('-a', '--all', action='store_true', 123 | help='Display duplicate adv responses, by default show new + updated') 124 | parser.add_argument('-n', '--new', action='store_true', 125 | help='Display only new adv responses, by default show new + updated') 126 | parser.add_argument('-v', '--verbose', action='store_true', 127 | help='Increase output verbosity') 128 | arg = parser.parse_args(sys.argv[1:]) 129 | 130 | btle.Debugging = arg.verbose 131 | 132 | scanner = btle.Scanner(arg.hci).withDelegate(ScannerDatabase(arg)) 133 | 134 | print (ANSI_RED + "Scanning for devices..." + ANSI_OFF) 135 | devices = scanner.scan(arg.timeout) 136 | 137 | if arg.discover: 138 | print (ANSI_RED + "Discovering services..." + ANSI_OFF) 139 | 140 | for d in devices: 141 | if not d.connectable or d.rssi < arg.sensitivity: 142 | 143 | continue 144 | 145 | print (" Connecting to", ANSI_WHITE + d.addr + ANSI_OFF + ":") 146 | 147 | dev = btle.Peripheral(d) 148 | dump_services(dev) 149 | dev.disconnect() 150 | print 151 | 152 | if __name__ == "__main__": 153 | main() --------------------------------------------------------------------------------