yostlabs 2025.5.14__tar.gz → 2025.9.17__tar.gz

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (32) hide show
  1. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/PKG-INFO +1 -1
  2. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/pyproject.toml +1 -1
  3. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/communication/ble.py +75 -68
  4. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/tss3/api.py +55 -40
  5. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/.gitignore +0 -0
  6. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/Examples/embedded_2024_dec_20.xml +0 -0
  7. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/Examples/example_ble.py +0 -0
  8. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/Examples/example_commands.py +0 -0
  9. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/Examples/example_component_specific_settings_and_commands.py +0 -0
  10. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/Examples/example_firmware_upload.py +0 -0
  11. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/Examples/example_parsing_stored_binary.py +0 -0
  12. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/Examples/example_read_settings.py +0 -0
  13. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/Examples/example_streaming.py +0 -0
  14. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/Examples/example_streaming_manager.py +0 -0
  15. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/Examples/example_write_settings.py +0 -0
  16. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/LICENSE +0 -0
  17. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/README.md +0 -0
  18. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/__init__.py +0 -0
  19. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/communication/__init__.py +0 -0
  20. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/communication/base.py +0 -0
  21. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/communication/serial.py +0 -0
  22. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/math/__init__.py +0 -0
  23. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/math/quaternion.py +0 -0
  24. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/math/vector.py +0 -0
  25. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/tss3/__init__.py +0 -0
  26. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/tss3/consts.py +0 -0
  27. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/tss3/eepts.py +0 -0
  28. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/tss3/utils/__init__.py +0 -0
  29. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/tss3/utils/calibration.py +0 -0
  30. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/tss3/utils/parser.py +0 -0
  31. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/tss3/utils/streaming.py +0 -0
  32. {yostlabs-2025.5.14 → yostlabs-2025.9.17}/src/yostlabs/tss3/utils/version.py +0 -0
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: yostlabs
3
- Version: 2025.5.14
3
+ Version: 2025.9.17
4
4
  Summary: Python resources and API for 3Space sensors from Yost Labs Inc.
5
5
  Project-URL: Homepage, https://yostlabs.com/
6
6
  Project-URL: Repository, https://github.com/YostLabs/3SpacePythonPackage/tree/main
@@ -5,7 +5,7 @@ build-backend = "hatchling.build"
5
5
  [project]
6
6
  name = "yostlabs"
7
7
  #If uploading again on the same day, add a fourth number
8
- version = "2025.05.14"
8
+ version = "2025.09.17"
9
9
  authors = [
10
10
  { name="Yost Labs Inc.", email="techsupport@yostlabs.com" },
11
11
  { name="Andy Riedlinger", email="techsupport@yostlabs.com" },
@@ -1,5 +1,6 @@
1
1
  import asyncio
2
2
  import async_timeout
3
+ import threading
3
4
  import time
4
5
  from bleak import BleakScanner, BleakClient
5
6
  from bleak.backends.device import BLEDevice
@@ -24,10 +25,23 @@ MANUFACTURER_NAME_STRING_UUID = "00002a29-0000-1000-8000-00805f9b34fb"
24
25
 
25
26
  class TssBLENoConnectionError(Exception): ...
26
27
 
28
+ def ylBleEventLoopThread(loop: asyncio.AbstractEventLoop):
29
+ asyncio.set_event_loop(loop)
30
+ loop.run_forever()
31
+
27
32
  from yostlabs.communication.base import *
28
33
  class ThreespaceBLEComClass(ThreespaceComClass):
29
34
 
30
35
  DEFAULT_TIMEOUT = 2
36
+ EVENT_LOOP = None
37
+ EVENT_LOOP_THREAD = None
38
+
39
+ @classmethod
40
+ def __lazy_event_loop_init(cls):
41
+ if cls.EVENT_LOOP is None:
42
+ cls.EVENT_LOOP = asyncio.new_event_loop()
43
+ cls.EVENT_LOOP_THREAD = threading.Thread(target=ylBleEventLoopThread, args=(cls.EVENT_LOOP,), daemon=True)
44
+ cls.EVENT_LOOP_THREAD.start()
31
45
 
32
46
  def __init__(self, ble: BleakClient | BLEDevice | str, discover_name: bool = True, discovery_timeout=5, error_on_disconnect=True):
33
47
  """
@@ -39,6 +53,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
39
53
  error_on_disconnect : If trying to read while the sensor is disconnected, an exception will be generated. This may be undesirable \
40
54
  if it is expected that the sensor will frequently go in and out of range and the user wishes to preserve data (such as streaming)
41
55
  """
56
+ self.__lazy_event_loop_init()
42
57
  bleak_options = { "timeout": discovery_timeout, "disconnected_callback": self.__on_disconnect }
43
58
  if isinstance(ble, BleakClient): #Actual client
44
59
  self.client = ble
@@ -46,7 +61,8 @@ class ThreespaceBLEComClass(ThreespaceComClass):
46
61
  elif isinstance(ble, str):
47
62
  if discover_name: #Local Name stirng
48
63
  self.__lazy_init_scanner()
49
- device = ThreespaceBLEComClass.SCANNER_EVENT_LOOP.run_until_complete(BleakScanner.find_device_by_name(ble, timeout=discovery_timeout))
64
+ future = asyncio.run_coroutine_threadsafe(BleakScanner.find_device_by_name(ble, timeout=discovery_timeout), self.EVENT_LOOP)
65
+ device = future.result()
50
66
  if device is None:
51
67
  raise BleakDeviceNotFoundError(ble)
52
68
  self.client = BleakClient(device, **bleak_options)
@@ -63,26 +79,27 @@ class ThreespaceBLEComClass(ThreespaceComClass):
63
79
  self.__timeout = self.DEFAULT_TIMEOUT
64
80
 
65
81
  self.buffer = bytearray()
66
- self.event_loop: asyncio.AbstractEventLoop = None
67
82
  self.data_read_event: asyncio.Event = None
68
83
 
69
84
  #Default to 20, will update on open
70
85
  self.max_packet_size = 20
71
86
 
72
87
  self.error_on_disconnect = error_on_disconnect
73
- #is_connected is different from open.
74
- #check_open() should return is_connected as that is what the user likely wants.
75
- #open is whether or not the client will auto connect to the device when rediscovered.
76
- #This file is set up to automatically close the connection if a method is called and is_connected is False
77
- #This behavior might be specific to Windows.
88
+
89
+ #is_connected is different from open. Open is the cached idea of whether the BLE connection should be active,
90
+ #while is_connected is the actual state of the connection. Basically, opened means a connection is desired, while
91
+ #is_connected means it is actually connected.
78
92
  self.__opened = False
93
+
79
94
  #client.is_connected is really slow (noticeable when called in bulk, which happens do to the assert_connected)...
80
95
  #So instead using the disconnected callback and this variable to manage tracking the state without the delay
81
96
  self.__connected = False
97
+
82
98
  #Writing functions will naturally throw an exception if disconnected. Reading ones don't because they use notifications rather
83
99
  #then direct reads. This means reading functions will need to assert the connection status but writing does not.
84
100
 
85
101
  async def __async_open(self):
102
+ self.data_read_event = asyncio.Event()
86
103
  await self.client.connect()
87
104
  await self.client.start_notify(NORDIC_UART_TX_UUID, self.__on_data_received)
88
105
 
@@ -92,9 +109,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
92
109
  if not self.__connected and self.error_on_disconnect:
93
110
  self.close()
94
111
  return
95
- self.event_loop = asyncio.new_event_loop()
96
- self.data_read_event = asyncio.Event()
97
- self.event_loop.run_until_complete(self.__async_open())
112
+ asyncio.run_coroutine_threadsafe(self.__async_open(), self.EVENT_LOOP).result()
98
113
  self.max_packet_size = self.client.mtu_size - 3 #-3 to account for the opcode and attribute handle stored in the data packet
99
114
  self.__opened = True
100
115
  self.__connected = True
@@ -104,14 +119,12 @@ class ThreespaceBLEComClass(ThreespaceComClass):
104
119
  #the disconnect call will hang on Windows. It seems similar to this issue: https://github.com/hbldh/bleak/issues/1359
105
120
  await asyncio.sleep(0.5)
106
121
  await self.client.disconnect()
122
+ self.data_read_event = None
123
+ self.buffer.clear()
107
124
 
108
125
  def close(self):
109
126
  if not self.__opened: return
110
- self.event_loop.run_until_complete(self.__async_close())
111
- self.buffer.clear()
112
- self.event_loop.close()
113
- self.event_loop = None
114
- self.data_read_event = None
127
+ asyncio.run_coroutine_threadsafe(self.__async_close(), self.EVENT_LOOP).result()
115
128
  self.__opened = False
116
129
 
117
130
  def __on_disconnect(self, client: BleakClient):
@@ -125,28 +138,10 @@ class ThreespaceBLEComClass(ThreespaceComClass):
125
138
  raise TssBLENoConnectionError(f"{self.name} is not connected")
126
139
 
127
140
  def check_open(self):
128
- #Checking this, while slow, isn't much difference in speed as allowing the disconnect callback to update via
129
- #running the empty async function. So just going to use this here. Repeated calls to check_open are not a good
130
- #idea from a speed perspective until a fix is found. We will probably make a version of this BLEComClass that uses
131
- #a background thread for asyncio to allow for speed increases.
132
- self.__connected = self.client.is_connected
133
141
  if not self.__connected and self.__opened and self.error_on_disconnect:
134
142
  self.close()
135
143
  return self.__connected
136
144
 
137
- #Bleak does run a thread to read data on notification after calling start_notify, however on notification
138
- #it schedules a callback using loop.call_soon_threadsafe() so the actual notification can't happen unless we
139
- #run the event loop. Therefore, this async function that does nothing is used just to trigger an event loop updated
140
- #so the read callbacks __on_data_received can occur
141
- @staticmethod
142
- async def __wait_for_callbacks_async():
143
- pass
144
-
145
- def __read_all_data(self):
146
- self.event_loop.run_until_complete(self.__wait_for_callbacks_async())
147
- self.data_read_event.clear()
148
- self.__assert_connected()
149
-
150
145
  def __on_data_received(self, sender: BleakGATTCharacteristic, data: bytearray):
151
146
  self.buffer += data
152
147
  self.data_read_event.set()
@@ -155,7 +150,9 @@ class ThreespaceBLEComClass(ThreespaceComClass):
155
150
  start_index = 0
156
151
  while start_index < len(bytes):
157
152
  end_index = min(len(bytes), start_index + self.max_packet_size) #Can only send max_packet_size data per call to write_gatt_char
158
- self.event_loop.run_until_complete(self.client.write_gatt_char(NORDIC_UART_RX_UUID, bytes[start_index:end_index], response=False))
153
+ asyncio.run_coroutine_threadsafe(
154
+ self.client.write_gatt_char(NORDIC_UART_RX_UUID, bytes[start_index:end_index], response=False),
155
+ self.EVENT_LOOP).result()
159
156
  start_index = end_index
160
157
 
161
158
  async def __await_read(self, timeout_time: int):
@@ -169,19 +166,19 @@ class ThreespaceBLEComClass(ThreespaceComClass):
169
166
  return False
170
167
 
171
168
  async def __await_num_bytes(self, num_bytes: int):
172
- start_time = self.event_loop.time()
173
- while len(self.buffer) < num_bytes and self.event_loop.time() - start_time < self.timeout:
169
+ start_time = self.EVENT_LOOP.time()
170
+ while len(self.buffer) < num_bytes and self.EVENT_LOOP.time() - start_time < self.timeout:
174
171
  await self.__await_read(start_time + self.timeout)
175
172
 
176
173
  def read(self, num_bytes: int):
177
- self.event_loop.run_until_complete(self.__await_num_bytes(num_bytes))
174
+ asyncio.run_coroutine_threadsafe(self.__await_num_bytes(num_bytes), self.EVENT_LOOP).result()
178
175
  num_bytes = min(num_bytes, len(self.buffer))
179
176
  data = self.buffer[:num_bytes]
180
177
  del self.buffer[:num_bytes]
181
178
  return data
182
179
 
183
180
  def peek(self, num_bytes: int):
184
- self.event_loop.run_until_complete(self.__await_num_bytes(num_bytes))
181
+ asyncio.run_coroutine_threadsafe(self.__await_num_bytes(num_bytes), self.EVENT_LOOP).result()
185
182
  num_bytes = min(num_bytes, len(self.buffer))
186
183
  data = self.buffer[:num_bytes]
187
184
  return data
@@ -189,13 +186,13 @@ class ThreespaceBLEComClass(ThreespaceComClass):
189
186
  #Reads until the pattern is received, max_length is exceeded, or timeout occurs
190
187
  async def __await_pattern(self, pattern: bytes, max_length: int = None):
191
188
  if max_length is None: max_length = float('inf')
192
- start_time = self.event_loop.time()
193
- while pattern not in self.buffer and self.event_loop.time() - start_time < self.timeout and len(self.buffer) < max_length:
189
+ start_time = self.EVENT_LOOP.time()
190
+ while pattern not in self.buffer and self.EVENT_LOOP.time() - start_time < self.timeout and len(self.buffer) < max_length:
194
191
  await self.__await_read(start_time + self.timeout)
195
192
  return pattern in self.buffer
196
193
 
197
194
  def read_until(self, expected: bytes) -> bytes:
198
- self.event_loop.run_until_complete(self.__await_pattern(expected))
195
+ asyncio.run_coroutine_threadsafe(self.__await_pattern(expected), self.EVENT_LOOP).result()
199
196
  if expected in self.buffer: #Found the pattern
200
197
  length = self.buffer.index(expected) + len(expected)
201
198
  result = self.buffer[:length]
@@ -207,7 +204,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
207
204
  return result
208
205
 
209
206
  def peek_until(self, expected: bytes, max_length: int = None) -> bytes:
210
- self.event_loop.run_until_complete(self.__await_pattern(expected, max_length=max_length))
207
+ asyncio.run_coroutine_threadsafe(self.__await_pattern(expected, max_length=max_length), self.EVENT_LOOP).result()
211
208
  if expected in self.buffer:
212
209
  length = self.buffer.index(expected) + len(expected)
213
210
  else:
@@ -220,7 +217,6 @@ class ThreespaceBLEComClass(ThreespaceComClass):
220
217
 
221
218
  @property
222
219
  def length(self):
223
- self.__read_all_data() #Gotta update the data before knowing the length
224
220
  return len(self.buffer)
225
221
 
226
222
  @property
@@ -249,7 +245,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
249
245
  return self.client.address
250
246
 
251
247
  SCANNER = None
252
- SCANNER_EVENT_LOOP = None
248
+ SCANNER_LOCK = None
253
249
 
254
250
  SCANNER_CONTINOUS = False #Controls if scanning will continously run
255
251
  SCANNER_TIMEOUT = 5 #Controls the scanners timeout
@@ -261,13 +257,20 @@ class ThreespaceBLEComClass(ThreespaceComClass):
261
257
 
262
258
  @classmethod
263
259
  def __lazy_init_scanner(cls):
260
+ cls.__lazy_event_loop_init()
264
261
  if cls.SCANNER is None:
265
- cls.SCANNER = BleakScanner(detection_callback=cls.__detection_callback, service_uuids=[NORDIC_UART_SERVICE_UUID])
266
- cls.SCANNER_EVENT_LOOP = asyncio.new_event_loop()
262
+ cls.SCANNER_LOCK = threading.Lock()
263
+ #Scanner should be created inside of the Async Context that will use it
264
+ async def create_scanner():
265
+ cls.SCANNER = BleakScanner(detection_callback=cls.__detection_callback, service_uuids=[NORDIC_UART_SERVICE_UUID])
266
+ asyncio.run_coroutine_threadsafe(create_scanner(), cls.EVENT_LOOP).result()
267
+
268
+
267
269
 
268
270
  @classmethod
269
271
  def __detection_callback(cls, device: BLEDevice, adv: AdvertisementData):
270
- cls.discovered_devices[device.address] = {"device": device, "adv": adv, "last_found": time.time()}
272
+ with cls.SCANNER_LOCK:
273
+ cls.discovered_devices[device.address] = {"device": device, "adv": adv, "last_found": time.time()}
271
274
 
272
275
  @classmethod
273
276
  def set_scanner_continous(cls, continous: bool):
@@ -281,8 +284,10 @@ class ThreespaceBLEComClass(ThreespaceComClass):
281
284
  """
282
285
  cls.__lazy_init_scanner()
283
286
  cls.SCANNER_CONTINOUS = continous
284
- if continous: cls.SCANNER_EVENT_LOOP.run_until_complete(cls.SCANNER.start())
285
- else: cls.SCANNER_EVENT_LOOP.run_until_complete(cls.SCANNER.stop())
287
+ if continous:
288
+ asyncio.run_coroutine_threadsafe(cls.SCANNER.start(), cls.EVENT_LOOP).result()
289
+ else:
290
+ asyncio.run_coroutine_threadsafe(cls.SCANNER.stop(), cls.EVENT_LOOP).result()
286
291
 
287
292
  @classmethod
288
293
  def update_nearby_devices(cls):
@@ -291,36 +296,37 @@ class ThreespaceBLEComClass(ThreespaceComClass):
291
296
  """
292
297
  cls.__lazy_init_scanner()
293
298
  if cls.SCANNER_CONTINOUS:
294
- #Allow the callbacks for nearby devices to trigger
295
- cls.SCANNER_EVENT_LOOP.run_until_complete(cls.__wait_for_callbacks_async())
296
- #Remove expired devices
297
- cur_time = time.time()
298
- to_remove = [] #Avoiding concurrent list modification
299
- for device in cls.discovered_devices:
300
- if cur_time - cls.discovered_devices[device]["last_found"] > cls.SCANNER_EXPIRATION_TIME:
301
- to_remove.append(device)
302
- for device in to_remove:
303
- del cls.discovered_devices[device]
304
-
299
+ with cls.SCANNER_LOCK:
300
+ #Remove expired devices
301
+ cur_time = time.time()
302
+ to_remove = [] #Avoiding concurrent list modification
303
+ for device in cls.discovered_devices:
304
+ if cur_time - cls.discovered_devices[device]["last_found"] > cls.SCANNER_EXPIRATION_TIME:
305
+ to_remove.append(device)
306
+ for device in to_remove:
307
+ del cls.discovered_devices[device]
308
+ discovered = cls.discovered_devices.copy()
305
309
  else:
306
310
  #Mark all devices as invalid before searching for nearby devices
307
311
  cls.discovered_devices.clear()
308
312
  start_time = time.time()
309
313
  end_time = cls.SCANNER_TIMEOUT or float('inf')
310
314
  end_count = cls.SCANNER_FIND_COUNT or float('inf')
311
- cls.SCANNER_EVENT_LOOP.run_until_complete(cls.SCANNER.start())
315
+ asyncio.run_coroutine_threadsafe(cls.SCANNER.start(), cls.EVENT_LOOP).result()
312
316
  while time.time() - start_time < end_time and len(cls.discovered_devices) < end_count:
313
- cls.SCANNER_EVENT_LOOP.run_until_complete(cls.__wait_for_callbacks_async())
314
- cls.SCANNER_EVENT_LOOP.run_until_complete(cls.SCANNER.stop())
315
-
316
- return cls.discovered_devices
317
+ time.sleep(0)
318
+ asyncio.run_coroutine_threadsafe(cls.SCANNER.stop(), cls.EVENT_LOOP).result()
319
+ discovered = cls.discovered_devices.copy()
320
+ return discovered
317
321
 
318
322
  @classmethod
319
323
  def get_discovered_nearby_devices(cls):
320
324
  """
321
325
  A helper to get a copy of the discovered devices
322
326
  """
323
- return cls.discovered_devices.copy()
327
+ with cls.SCANNER_LOCK:
328
+ discovered = cls.discovered_devices.copy()
329
+ return discovered
324
330
 
325
331
  @staticmethod
326
332
  def auto_detect() -> Generator["ThreespaceBLEComClass", None, None]:
@@ -330,5 +336,6 @@ class ThreespaceBLEComClass(ThreespaceComClass):
330
336
  """
331
337
  cls = ThreespaceBLEComClass
332
338
  cls.update_nearby_devices()
333
- for device_info in cls.discovered_devices.values():
334
- yield(ThreespaceBLEComClass(device_info["device"]))
339
+ with cls.SCANNER_LOCK:
340
+ for device_info in cls.discovered_devices.values():
341
+ yield(ThreespaceBLEComClass(device_info["device"]))
@@ -100,7 +100,7 @@ class ThreespaceCommand:
100
100
  response = response[size:]
101
101
  else: #Strings are special, find the null terminator
102
102
  str_len = response.index(0)
103
- output.append(struct.unpack(f"<{str_len}s", response[str_len])[0])
103
+ output.append(struct.unpack(f"<{str_len}s", response[:str_len])[0])
104
104
  response = response[str_len + 1:] #+1 to skip past the null terminator character too
105
105
  else: #Fast parse because no strings
106
106
  output.extend(struct.unpack(f"<{self.out_format}", response[:self.info.out_size]))
@@ -115,37 +115,42 @@ class ThreespaceCommand:
115
115
  raw = bytearray([])
116
116
  if self.info.num_out_params == 0: return None, raw
117
117
  output = []
118
- for c in self.out_format:
119
- if c != 's':
120
- format_str = f"<{c}"
121
- size = struct.calcsize(format_str)
122
- response = com.read(size)
123
- raw += response
124
- if len(response) != size:
125
- if verbose:
126
- print(f"Failed to read {c} type. Aborting...")
127
- return None
128
- output.append(struct.unpack(format_str, response)[0])
129
- else: #Strings are special, find the null terminator
130
- response = com.read(1)
131
- raw += response
132
- if len(response) != 1:
133
- if verbose:
134
- print(f"Failed to read string. Aborting...")
135
- return None
136
- byte = chr(response[0])
137
- string = ""
138
- while byte != '\0':
139
- string += byte
140
- #Get next byte
141
- response = com.read(1)
118
+
119
+ if not math.isnan(self.info.out_size):
120
+ #Fast read and parse
121
+ response = com.read(self.info.out_size)
122
+ raw += response
123
+ if len(response) != self.info.out_size:
124
+ if verbose:
125
+ print(f"Failed to read {self.info.name} {len(response)} / {self.info.out_size}. Aborting...")
126
+ output.extend(struct.unpack(f"<{self.out_format}", response))
127
+ else:
128
+ #There is a string, so go element by element
129
+ i = 0
130
+ while i < len(self.out_format):
131
+ c = self.out_format[i]
132
+ if c != 's':
133
+ end_index = self.out_format.find('s', i)
134
+ if end_index == -1: end_index = len(self.out_format)
135
+ format_str = f"<{self.out_format[i:end_index]}"
136
+ size = struct.calcsize(format_str)
137
+ response = com.read(size)
142
138
  raw += response
143
- if len(response) != 1:
139
+ if len(response) != size:
140
+ if verbose:
141
+ print(f"Failed to read {c} type. Aborting...")
142
+ return None
143
+ output.append(struct.unpack(format_str, response)[0])
144
+ i = end_index
145
+ else:
146
+ response = com.read_until(b'\0')
147
+ raw += response
148
+ if response[-1] != 0:
144
149
  if verbose:
145
150
  print(f"Failed to read string. Aborting...")
146
151
  return None
147
- byte = chr(response[0])
148
- output.append(string)
152
+ output.append(response[:-1].decode())
153
+ i += 1
149
154
 
150
155
  if self.info.num_out_params == 1:
151
156
  return output[0], raw
@@ -162,7 +167,7 @@ class ThreespaceGetStreamingBatchCommand(ThreespaceCommand):
162
167
  def set_stream_slots(self, streaming_slots: list[ThreespaceCommand]):
163
168
  self.commands = streaming_slots
164
169
  self.out_format = ''.join(slot.out_format for slot in streaming_slots if slot is not None)
165
- self.info.out_size = struct.calcsize(f"<{self.out_format}")
170
+ self.info.out_size = sum(slot.info.out_size for slot in streaming_slots if slot is not None)
166
171
 
167
172
  def parse_response(self, response: bytes):
168
173
  data = []
@@ -180,9 +185,8 @@ class ThreespaceGetStreamingBatchCommand(ThreespaceCommand):
180
185
  raw_response = bytearray([])
181
186
  for command in self.commands:
182
187
  if command is None: continue
183
- binary = com.read(command.info.out_size)
184
- raw_response += binary
185
- out = command.parse_response(binary)
188
+ out, raw = command.read_command(com, verbose=verbose)
189
+ raw_response += raw
186
190
  response.append(out)
187
191
 
188
192
  return response, raw_response
@@ -393,6 +397,8 @@ class StreamableCommands(Enum):
393
397
  GetCorrectedAccelVec = 55
394
398
  GetCorrectedMagVec = 56
395
399
 
400
+ GetDateTime = 63
401
+
396
402
  GetRawGyroRate = 65
397
403
  GetRawAccelVec = 66
398
404
  GetRawMagVec = 67
@@ -401,8 +407,10 @@ class StreamableCommands(Enum):
401
407
  GetEeptsNewestStep = 71
402
408
  GetEeptsNumStepsAvailable = 72
403
409
 
410
+ GetDateTimeString = 93
404
411
  GetTimestamp = 94
405
412
 
413
+ GetBatteryCurrent = 200
406
414
  GetBatteryVoltage = 201
407
415
  GetBatteryPercent = 202
408
416
  GetBatteryStatus = 203
@@ -609,14 +617,14 @@ class ThreespaceSensor:
609
617
  self.streaming_packet_size = 0
610
618
  self._force_stop_streaming()
611
619
 
612
- #Now reinitialize the cached settings
613
- self.__cache_header_settings()
614
- self.__cache_streaming_settings()
615
-
616
620
  self.__cache_serial_number(int(self.get_settings("serial_number"), 16))
617
621
  self.__empty_debug_cache()
618
622
  self.immediate_debug = int(self.get_settings("debug_mode")) == 1 #Needed for some startup processes when restarting
619
623
 
624
+ #Now reinitialize the cached settings
625
+ self.__cache_header_settings()
626
+ self.__cache_streaming_settings()
627
+
620
628
  def __initialize_commands(self):
621
629
  self.commands: list[ThreespaceCommand] = [None] * 256
622
630
  self.getStreamingBatchCommand: ThreespaceGetStreamingBatchCommand = None
@@ -670,6 +678,9 @@ class ThreespaceSensor:
670
678
 
671
679
  setattr(self, command.info.name, method)
672
680
 
681
+ def has_command(self, command: ThreespaceCommand):
682
+ return self.commands[command.info.num] is not None
683
+
673
684
  def __get_command(self, command_name: str):
674
685
  for command in self.commands:
675
686
  if command is None: continue
@@ -1007,6 +1018,7 @@ class ThreespaceSensor:
1007
1018
  header = ThreespaceHeader.from_bytes(header, self.header_info)
1008
1019
  return header
1009
1020
 
1021
+ #TODO: max_data_length is not sufficient. Need MINIMUM length as well. Can have a situation where len = 0 and checksum = 0
1010
1022
  def __peek_checksum(self, header: ThreespaceHeader, max_data_length=4096):
1011
1023
  """
1012
1024
  Using a header that contains the checksum and data length, calculate the checksum of the expected
@@ -1100,7 +1112,7 @@ class ThreespaceSensor:
1100
1112
  self.misaligned = False
1101
1113
  return THREESPACE_UPDATE_COMMAND_PARSED
1102
1114
  elif self.is_log_streaming and header.echo == THREESPACE_FILE_READ_BYTES_COMMAND_NUM:
1103
- if not blocking:
1115
+ if not blocking:
1104
1116
  expected_output_size = len(header.raw_binary) + min(header.length, THREESPACE_LIVE_LOG_STREAM_MAX_PACKET_SIZE)
1105
1117
  if self.com.length < expected_output_size: return THREESPACE_UPDATE_COMMAND_NOT_ENOUGH_DATA
1106
1118
  if checksum_match := self.__peek_checksum(header, max_data_length=THREESPACE_LIVE_LOG_STREAM_MAX_PACKET_SIZE):
@@ -1204,9 +1216,9 @@ class ThreespaceSensor:
1204
1216
 
1205
1217
  def startStreaming(self) -> ThreespaceCmdResult[None]: ...
1206
1218
  def __startStreaming(self) -> ThreespaceCmdResult[None]:
1207
- if self.is_data_streaming: return
1208
- self.streaming_packets.clear()
1209
- self.__cache_streaming_settings()
1219
+ if not self.is_data_streaming:
1220
+ self.streaming_packets.clear()
1221
+ self.__cache_streaming_settings()
1210
1222
 
1211
1223
  result = self.execute_command(self.commands[THREESPACE_START_STREAMING_COMMAND_NUM])
1212
1224
  self.is_data_streaming = True
@@ -1632,6 +1644,7 @@ class ThreespaceSensor:
1632
1644
  def getCorrectedMagVec(self, id: int) -> ThreespaceCmdResult[list[float]]: ...
1633
1645
  def enableMSC(self) -> ThreespaceCmdResult[None]: ...
1634
1646
  def disableMSC(self) -> ThreespaceCmdResult[None]: ...
1647
+ def getDateTimeString(self) -> ThreespaceCmdResult[str]: ...
1635
1648
  def getTimestamp(self) -> ThreespaceCmdResult[int]: ...
1636
1649
  def getBatteryVoltage(self) -> ThreespaceCmdResult[float]: ...
1637
1650
  def getBatteryPercent(self) -> ThreespaceCmdResult[int]: ...
@@ -1760,6 +1773,7 @@ _threespace_commands: list[ThreespaceCommand] = [
1760
1773
  ThreespaceCommand("stopStreaming", THREESPACE_STOP_STREAMING_COMMAND_NUM, "", "", custom_func=ThreespaceSensor._ThreespaceSensor__stopStreaming),
1761
1774
  ThreespaceCommand("pauseLogStreaming", 87, "b", ""),
1762
1775
 
1776
+ ThreespaceCommand("getDateTimeString", 93, "", "S"),
1763
1777
  ThreespaceCommand("getTimestamp", 94, "", "U"),
1764
1778
 
1765
1779
  ThreespaceCommand("tareWithCurrentOrientation", 96, "", ""),
@@ -1788,6 +1802,7 @@ _threespace_commands: list[ThreespaceCommand] = [
1788
1802
  ThreespaceCommand("fileStartStream", 180, "", "U", custom_func=ThreespaceSensor._ThreespaceSensor__fileStartStream),
1789
1803
  ThreespaceCommand("fileStopStream", 181, "", "", custom_func=ThreespaceSensor._ThreespaceSensor__fileStopStream),
1790
1804
 
1805
+ ThreespaceCommand("getBatteryCurrent", 200, "", "I"),
1791
1806
  ThreespaceCommand("getBatteryVoltage", 201, "", "f"),
1792
1807
  ThreespaceCommand("getBatteryPercent", 202, "", "b"),
1793
1808
  ThreespaceCommand("getBatteryStatus", 203, "", "b"),
File without changes
File without changes
File without changes