yostlabs 2025.2.17__tar.gz → 2025.4.28__tar.gz

yostlabs-2025.4.28/Examples/example_ble.py

@@ -0,0 +1,23 @@
+from yostlabs.tss3.api import ThreespaceSensor
+from yostlabs.communication.ble import ThreespaceBLEComClass
+
+auto_detect = False
+
+if auto_detect:
+    #Create a sensor by auto detecting a ThreespaceBLEComClass.
+    #It does this by attempting to connect to a device with the Nordic Uart Service
+    sensor = ThreespaceSensor(ThreespaceBLEComClass)
+else:
+    #PUT YOUR SENSORS BLE_NAME HERE
+    ble_name = "YL-TSS-####" #Defaults to the lowest 4 hex digits of the sensors serial number
+    print("Attempting to discover and connect to a sensor with the name:", ble_name)
+    com_class = ThreespaceBLEComClass(ble_name)
+    sensor = ThreespaceSensor(com_class)
+
+ble_name = sensor.get_settings("ble_name")
+print("Connected to:", ble_name)
+
+result = sensor.getPrimaryCorrectedAccelVec()
+print(result)
+
+sensor.cleanup()

{yostlabs-2025.2.17 → yostlabs-2025.4.28}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: yostlabs
-Version: 2025.2.17
+Version: 2025.4.28
 Summary: Python resources and API for 3Space sensors from Yost Labs Inc.
 Project-URL: Homepage, https://yostlabs.com/
 Project-URL: Repository, https://github.com/YostLabs/3SpacePythonPackage/tree/main
@@ -13,6 +13,7 @@ Classifier: License :: OSI Approved :: MIT License
 Classifier: Operating System :: OS Independent
 Classifier: Programming Language :: Python :: 3
 Requires-Python: >=3.10
+Requires-Dist: bleak
 Requires-Dist: numpy
 Requires-Dist: pyserial
 Description-Content-Type: text/markdown
@@ -25,6 +26,8 @@ Description-Content-Type: text/markdown
 
 ## Basic Usage
 
+#### USB
+
 ```Python
 from yostlabs.tss3.api import ThreespaceSensor
 
@@ -37,6 +40,24 @@ print(result)
 sensor.cleanup()
 ```
 
+#### BLE
+
+```Python
+from yostlabs.tss3.api import ThreespaceSensor
+from yostlabs.communication.ble import ThreespaceBLEComClass
+
+#PUT YOUR SENSORS BLE_NAME HERE
+ble_name = "YL-TSS-####" #Defaults to the lowest 4 hex digits of the sensors serial number
+com_class = ThreespaceBLEComClass(ble_name)
+sensor = ThreespaceSensor(com_class)
+#sensor = ThreespaceSensor(ThreespaceBLEComClass) #Attempt to auto discover nearby sensor
+
+result = sensor.getPrimaryCorrectedAccelVec()
+print(result)
+
+sensor.cleanup()
+```
+
 Click [here](https://github.com/YostLabs/3SpacePythonPackage/tree/main/Examples) for more examples.
 
 ## Communication

{yostlabs-2025.2.17 → yostlabs-2025.4.28}/README.md

@@ -6,6 +6,8 @@
 
 ## Basic Usage
 
+#### USB
+
 ```Python
 from yostlabs.tss3.api import ThreespaceSensor
 
@@ -18,6 +20,24 @@ print(result)
 sensor.cleanup()
 ```
 
+#### BLE
+
+```Python
+from yostlabs.tss3.api import ThreespaceSensor
+from yostlabs.communication.ble import ThreespaceBLEComClass
+
+#PUT YOUR SENSORS BLE_NAME HERE
+ble_name = "YL-TSS-####" #Defaults to the lowest 4 hex digits of the sensors serial number
+com_class = ThreespaceBLEComClass(ble_name)
+sensor = ThreespaceSensor(com_class)
+#sensor = ThreespaceSensor(ThreespaceBLEComClass) #Attempt to auto discover nearby sensor
+
+result = sensor.getPrimaryCorrectedAccelVec()
+print(result)
+
+sensor.cleanup()
+```
+
 Click [here](https://github.com/YostLabs/3SpacePythonPackage/tree/main/Examples) for more examples.
 
 ## Communication

{yostlabs-2025.2.17 → yostlabs-2025.4.28}/pyproject.toml

@@ -5,7 +5,7 @@ build-backend = "hatchling.build"
 [project]
 name = "yostlabs"
 #If uploading again on the same day, add a fourth number
-version = "2025.02.17"
+version = "2025.04.28"
 authors = [
   { name="Yost Labs Inc.", email="techsupport@yostlabs.com" },
   { name="Andy Riedlinger", email="techsupport@yostlabs.com" },
@@ -22,7 +22,8 @@ classifiers = [
 keywords = ["3space", "threespace", "yost"]
 dependencies = [
   "pyserial",
-  "numpy"
+  "numpy",
+  "bleak"
 ]
 
 [project.urls]

yostlabs-2025.4.28/src/yostlabs/communication/ble.py

@@ -0,0 +1,325 @@
+import asyncio
+import async_timeout
+import time
+from bleak import BleakScanner, BleakClient
+from bleak.backends.device import BLEDevice
+from bleak.backends.scanner import AdvertisementData
+from bleak.backends.characteristic import BleakGATTCharacteristic
+from bleak.exc import BleakDeviceNotFoundError
+
+#Services
+NORDIC_UART_SERVICE_UUID = "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
+
+#Characteristics
+NORDIC_UART_RX_UUID = "6e400002-b5a3-f393-e0a9-e50e24dcca9e"
+NORDIC_UART_TX_UUID = "6e400003-b5a3-f393-e0a9-e50e24dcca9e"
+
+DEVICE_NAME_UUID = "00002a00-0000-1000-8000-00805f9b34fb"
+APPEARANCE_UUID = "00002a01-0000-1000-8000-00805f9b34fb"
+
+FIRMWARE_REVISION_STRING_UUID = "00002a26-0000-1000-8000-00805f9b34fb"
+HARDWARE_REVISION_STRING_UUID = "00002a27-0000-1000-8000-00805f9b34fb"
+SERIAL_NUMBER_STRING_UUID = "00002a25-0000-1000-8000-00805f9b34fb"
+MANUFACTURER_NAME_STRING_UUID = "00002a29-0000-1000-8000-00805f9b34fb"
+
+class TssBLENoConnectionError(Exception): ...
+
+from yostlabs.communication.base import *
+class ThreespaceBLEComClass(ThreespaceComClass):
+
+    DEFAULT_TIMEOUT = 2
+
+    def __init__(self, ble: BleakClient | BLEDevice | str, discover_name: bool = True, discovery_timeout=5, error_on_disconnect=True):
+        """
+        Parameters
+        ----------
+        ble : Can be either a BleakClient, BleakDevice, MacAddress String, or localName string
+        discover_name : If true, a string ble parameter is interpreted as a localName, else as a MacAddress
+        discovery_timeout : Max amount of time in seconds to discover the BLE device for the corresponding MacAddress/localName
+        error_on_disconnect : If trying to read while the sensor is disconnected, an exception will be generated. This may be undesirable \
+        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)
+        """
+        bleak_options = { "timeout": discovery_timeout, "disconnected_callback": self.__on_disconnect }
+        if isinstance(ble, BleakClient):    #Actual client
+            self.client = ble
+            self.__name = ble.address
+        elif isinstance(ble, str): 
+            if discover_name: #Local Name stirng
+                self.__lazy_init_scanner()
+                device = ThreespaceBLEComClass.SCANNER_EVENT_LOOP.run_until_complete(BleakScanner.find_device_by_name(ble, timeout=discovery_timeout))
+                if device is None:
+                    raise BleakDeviceNotFoundError(ble)
+                self.client = BleakClient(device, **bleak_options)
+                self.__name = ble
+            else: #Address string
+                self.client = BleakClient(ble, **bleak_options)
+                self.__name = self.client.address
+        elif isinstance(ble, BLEDevice):
+            self.client = BleakClient(ble, **bleak_options)
+            self.__name = ble.name #Use the local name instead of the address
+        else:
+            raise TypeError("Invalid type for creating a ThreespaceBLEComClass:", type(ble), ble)
+
+        self.__timeout = self.DEFAULT_TIMEOUT
+
+        self.buffer = bytearray()
+        self.event_loop: asyncio.AbstractEventLoop = None
+        self.data_read_event: asyncio.Event = None
+
+        #Default to 20, will update on open
+        self.max_packet_size = 20
+        
+        self.error_on_disconnect = error_on_disconnect
+        #is_connected is different from open.
+        #check_open() should return is_connected as that is what the user likely wants.
+        #open is whether or not the client will auto connect to the device when rediscovered.
+        #This file is set up to automatically close the connection if a method is called and is_connected is False
+        #This behavior might be specific to Windows.
+        self.__opened = False
+        #client.is_connected is really slow (noticeable when called in bulk, which happens do to the assert_connected)... 
+        #So instead using the disconnected callback and this variable to manage tracking the state without the delay
+        self.__connected = False
+        #Writing functions will naturally throw an exception if disconnected. Reading ones don't because they use notifications rather
+        #then direct reads. This means reading functions will need to assert the connection status but writing does not.
+
+    async def __async_open(self):
+        await self.client.connect()
+        await self.client.start_notify(NORDIC_UART_TX_UUID, self.__on_data_received)
+
+    def open(self):
+        #If trying to open while already open, this infinitely loops
+        if self.__opened: 
+            if not self.__connected and self.error_on_disconnect:
+                self.close()
+            return
+        self.event_loop = asyncio.new_event_loop()
+        self.data_read_event = asyncio.Event()
+        self.event_loop.run_until_complete(self.__async_open())
+        self.max_packet_size = self.client.mtu_size - 3 #-3 to account for the opcode and attribute handle stored in the data packet
+        self.__opened = True
+        self.__connected = True
+
+    async def __async_close(self):
+        #There appears to be a bug where if you call close too soon after is_connected returns false,
+        #the disconnect call will hang on Windows. It seems similar to this issue: https://github.com/hbldh/bleak/issues/1359
+        await asyncio.sleep(0.5)
+        await self.client.disconnect()
+
+    def close(self):
+        if not self.__opened: return
+        self.event_loop.run_until_complete(self.__async_close())
+        self.buffer.clear()
+        self.event_loop.close()
+        self.event_loop = None
+        self.data_read_event = None
+        self.__opened = False
+
+    def __on_disconnect(self, client: BleakClient):
+        self.__connected = False
+
+    #Goal is that this is always called after something that would have already performed an async callback
+    #to prevent needing to run the event loop. Running the event loop frequently is slow. Which is also why this
+    #comclass will eventually have a threaded asyncio version.
+    def __assert_connected(self):
+        if not self.__connected and self.error_on_disconnect:
+            raise TssBLENoConnectionError(f"{self.name} is not connected")
+
+    def check_open(self):
+        #Checking this, while slow, isn't much difference in speed as allowing the disconnect callback to update via
+        #running the empty async function. So just going to use this here. Repeated calls to check_open are not a good
+        #idea from a speed perspective until a fix is found. We will probably make a version of this BLEComClass that uses
+        #a background thread for asyncio to allow for speed increases.
+        self.__connected = self.client.is_connected
+        if not self.__connected and self.__opened and self.error_on_disconnect:
+            self.close()
+        return self.__connected
+
+    #Bleak does run a thread to read data on notification after calling start_notify, however on notification
+    #it schedules a callback using loop.call_soon_threadsafe() so the actual notification can't happen unless we
+    #run the event loop. Therefore, this async function that does nothing is used just to trigger an event loop updated
+    #so the read callbacks __on_data_received can occur
+    @staticmethod
+    async def __wait_for_callbacks_async():
+        pass
+
+    def __read_all_data(self):
+        self.event_loop.run_until_complete(self.__wait_for_callbacks_async())
+        self.data_read_event.clear()
+        self.__assert_connected()
+
+    def __on_data_received(self, sender: BleakGATTCharacteristic, data: bytearray):
+        self.buffer += data
+        self.data_read_event.set()
+
+    def write(self, bytes: bytes):
+        start_index = 0
+        while start_index < len(bytes):
+            end_index = min(len(bytes), start_index + self.max_packet_size) #Can only send max_packet_size data per call to write_gatt_char
+            self.event_loop.run_until_complete(self.client.write_gatt_char(NORDIC_UART_RX_UUID, bytes[start_index:end_index], response=False))
+            start_index = end_index
+    
+    async def __await_read(self, timeout_time: int):
+        self.__assert_connected()
+        try:
+            async with async_timeout.timeout_at(timeout_time):
+                await self.data_read_event.wait()
+            self.data_read_event.clear()
+            return True
+        except:
+            return False
+
+    async def __await_num_bytes(self, num_bytes: int):
+        start_time = self.event_loop.time()
+        while len(self.buffer) < num_bytes and self.event_loop.time() - start_time < self.timeout:
+            await self.__await_read(start_time + self.timeout)
+
+    def read(self, num_bytes: int):
+        self.event_loop.run_until_complete(self.__await_num_bytes(num_bytes))
+        num_bytes = min(num_bytes, len(self.buffer))
+        data = self.buffer[:num_bytes]
+        del self.buffer[:num_bytes]
+        return data
+
+    def peek(self, num_bytes: int):
+        self.event_loop.run_until_complete(self.__await_num_bytes(num_bytes))
+        num_bytes = min(num_bytes, len(self.buffer))
+        data = self.buffer[:num_bytes]
+        return data        
+    
+    #Reads until the pattern is received, max_length is exceeded, or timeout occurs
+    async def __await_pattern(self, pattern: bytes, max_length: int = None):
+        if max_length is None: max_length = float('inf')
+        start_time = self.event_loop.time()
+        while pattern not in self.buffer and self.event_loop.time() - start_time < self.timeout and len(self.buffer) < max_length:
+            await self.__await_read(start_time + self.timeout)
+        return pattern in self.buffer
+
+    def read_until(self, expected: bytes) -> bytes:
+        self.event_loop.run_until_complete(self.__await_pattern(expected))
+        if expected in self.buffer: #Found the pattern
+            length = self.buffer.index(expected) + len(expected)
+            result = self.buffer[:length]
+            del self.buffer[:length]
+            return result
+        #Failed to find the pattern, just return whatever is there
+        result = self.buffer.copy()
+        self.buffer.clear()
+        return result
+
+    def peek_until(self, expected: bytes, max_length: int = None) -> bytes:
+        self.event_loop.run_until_complete(self.__await_pattern(expected, max_length=max_length))
+        if expected in self.buffer:
+            length = self.buffer.index(expected) + len(expected)
+        else:
+            length = len(self.buffer)
+
+        if max_length is not None and length > max_length:
+            length = max_length
+
+        return self.buffer[:length]
+
+    @property
+    def length(self):
+        self.__read_all_data() #Gotta update the data before knowing the length
+        return len(self.buffer) 
+
+    @property
+    def timeout(self) -> float:
+        return self.__timeout
+    
+    @timeout.setter
+    def timeout(self, timeout: float):
+        self.__timeout = timeout    
+
+    @property
+    def reenumerates(self) -> bool:
+        return False
+    
+    @property
+    def name(self) -> str:
+        return self.__name
+
+    SCANNER = None
+    SCANNER_EVENT_LOOP = None
+
+    SCANNER_CONTINOUS = False   #Controls if scanning will continously run
+    SCANNER_TIMEOUT = 5         #Controls the scanners timeout
+    SCANNER_FIND_COUNT = 1      #When continous=False, will stop scanning after at least this many devices are found. Set to None to search the entire timeout.
+    SCANNER_EXPIRATION_TIME = 5 #Controls the timeout for detected BLE sensors. If a sensor hasn't been detected again in this amount of time, its removed from discovered devices
+
+    #Format: Address - dict = { device: ..., adv: ..., last_found: ... }
+    discovered_devices: dict[str,dict] = {}
+
+    @classmethod
+    def __lazy_init_scanner(cls):
+        if cls.SCANNER is None:
+            cls.SCANNER = BleakScanner(detection_callback=cls.__detection_callback, service_uuids=[NORDIC_UART_SERVICE_UUID])
+            cls.SCANNER_EVENT_LOOP = asyncio.new_event_loop()
+
+    @classmethod
+    def __detection_callback(cls, device: BLEDevice, adv: AdvertisementData):
+        cls.discovered_devices[device.address] = {"device": device, "adv": adv, "last_found": time.time()}
+    
+    @classmethod
+    def set_scanner_continous(cls, continous: bool):
+        """
+        If not using continous mode, functions like update_nearby_devices and auto_detect are blocking with the following rules:
+        - Will search for at most SCANNER_TIMEOUT time
+        - Will stop searching immediately once SCANNER_FIND_COUNT is reached
+
+        If using continous mode, no scanning functions are blocking. However, the user must continously call 
+        update_nearby_devices to ensure up to date information.
+        """
+        cls.__lazy_init_scanner()
+        cls.SCANNER_CONTINOUS = continous
+        if continous: cls.SCANNER_EVENT_LOOP.run_until_complete(cls.SCANNER.start())
+        else: cls.SCANNER_EVENT_LOOP.run_until_complete(cls.SCANNER.stop())
+
+    @classmethod
+    def update_nearby_devices(cls):
+        """
+        Updates ThreespaceBLEComClass.discovered_devices using the current configuration.
+        """
+        cls.__lazy_init_scanner()
+        if cls.SCANNER_CONTINOUS:
+            #Allow the callbacks for nearby devices to trigger
+            cls.SCANNER_EVENT_LOOP.run_until_complete(cls.__wait_for_callbacks_async())
+            #Remove expired devices
+            cur_time = time.time()
+            to_remove = [] #Avoiding concurrent list modification
+            for device in cls.discovered_devices:
+                if cur_time - cls.discovered_devices[device]["last_found"] > cls.SCANNER_EXPIRATION_TIME:
+                    to_remove.append(device) 
+            for device in to_remove:
+                del cls.discovered_devices[device]
+
+        else:
+            #Mark all devices as invalid before searching for nearby devices
+            cls.discovered_devices.clear()
+            start_time = time.time()
+            end_time = cls.SCANNER_TIMEOUT or float('inf')
+            end_count = cls.SCANNER_FIND_COUNT or float('inf')
+            cls.SCANNER_EVENT_LOOP.run_until_complete(cls.SCANNER.start())
+            while time.time() - start_time < end_time and len(cls.discovered_devices) < end_count:
+                cls.SCANNER_EVENT_LOOP.run_until_complete(cls.__wait_for_callbacks_async())
+            cls.SCANNER_EVENT_LOOP.run_until_complete(cls.SCANNER.stop())
+        
+        return cls.discovered_devices
+    
+    @classmethod
+    def get_discovered_nearby_devices(cls):
+        """
+        A helper to get a copy of the discovered devices
+        """
+        return cls.discovered_devices.copy()
+
+    @staticmethod
+    def auto_detect() -> Generator["ThreespaceBLEComClass", None, None]:
+        """
+        Returns a list of com classes of the same type called on nearby.
+        These ports will start unopened. This allows the caller to get a list of ports without having to connect.
+        """
+        cls = ThreespaceBLEComClass
+        cls.update_nearby_devices()
+        for device_info in cls.discovered_devices.values():
+            yield(ThreespaceBLEComClass(device_info["device"]))

{yostlabs-2025.2.17 → yostlabs-2025.4.28}/src/yostlabs/communication/serial.py

@@ -1,6 +1,7 @@
 from yostlabs.communication.base import *
 import serial
 import serial.tools.list_ports
+import time
 
 
 class ThreespaceSerialComClass(ThreespaceComClass):
@@ -107,7 +108,10 @@ class ThreespaceSerialComClass(ThreespaceComClass):
     
     @timeout.setter
     def timeout(self, timeout: float):
-        self.ser.timeout = timeout    
+        self.ser.timeout = timeout
+        #There is a bug in Windows drivers that requires a delay after setting timeout
+        #When using certain Serial Interfaces
+        time.sleep(0.01) 
 
     @property
     def reenumerates(self) -> bool:

{yostlabs-2025.2.17 → yostlabs-2025.4.28}/src/yostlabs/math/quaternion.py

@@ -12,16 +12,15 @@ def quat_mul(a: list[float], b: list[float]):
 
 #Rotates quaternion b by quaternion a, it does not combine them
 def quat_rotate(a: list[float], b: list[float]):
-    inv = a.copy()
-    inv[0] *= -1
-    inv[1] *= -1
-    inv[2] *= -1
+    inv = quat_inverse(a)
     axis = [b[0], b[1], b[2], 0]
     halfway = quat_mul(a, axis)
     final = quat_mul(halfway, inv)
     return [*final[:3], b[3]]
 
 def quat_inverse(quat: list[float]):
+    #Note: While technically negating just the W is rotationally equivalent, this is not a good idea
+    #as it will conflict with rotating vectors, which are not rotations, by quaternions
     return [-quat[0], -quat[1], -quat[2], quat[3]]
 
 def quat_rotate_vec(quat: list[float], vec: list[float]):
@@ -43,7 +42,6 @@ def angles_to_quaternion(angles: list[float], order: str, degrees=True):
         imaginary = math.sin(angle / 2)
         unit_vec = [v * imaginary for v in unit_vec]
         angle_quat = [*unit_vec, w]
-        #print(f"{axis} {angle} {angle_quat}")
         quat = quat_mul(quat, angle_quat)
     return quat
 
@@ -134,6 +132,37 @@ def quaternion_global_to_local(quat, vec):
 def quaternion_local_to_global(quat, vec):
     return quat_rotate_vec(quat, vec)
 
+def quaternion_swap_axes(quat: list, old_order: str, new_order: str):
+    return quaternion_swap_axes_fast(quat, _vec.parse_axis_string_info(old_order), _vec.parse_axis_string_info(new_order))
+
+def quaternion_swap_axes_fast(quat: list, old_parsed_order: list[list, list, bool], new_parsed_order: list[list, list, bool]):
+    """
+    Like quaternion_swap_axes but uses the inputs of parsing the axis strings to avoid having to recompute
+    the storage types.
+
+    each order should be a sequence of [order, mults, right_handed]
+    """
+    old_order, old_mults, old_right_handed = old_parsed_order
+    new_order, new_mults, new_right_handed = new_parsed_order
+
+    #Undo the old negations
+    base_quat = quat.copy()
+    for i, mult in enumerate(old_mults):
+        base_quat[i] *= mult
+    
+    #Now swap the positions and apply new multipliers
+    new_quat = base_quat.copy()
+    for i in range(3):
+        new_quat[i] = base_quat[old_order.index(new_order[i])]
+        new_quat[i] *= new_mults[i]
+    
+    if old_right_handed != new_right_handed:
+        #Different handed systems rotate opposite directions. So to maintain the same rotation,
+        #invert the quaternion
+        new_quat = quat_inverse(new_quat)
+
+    return new_quat
+
 #https://splines.readthedocs.io/en/latest/rotation/slerp.html
 def slerp(a, b, t):
     dot = _vec.vec_dot(a, b)

yostlabs-2025.4.28/src/yostlabs/math/vector.py

@@ -0,0 +1,81 @@
+def vec_len(vector: list[float|int]):
+    return sum(v ** 2 for v in vector) ** 0.5
+
+def vec_dot(a: list[float], b: list[float]):
+    return sum(a[i] * b[i] for i in range(len(a)))
+
+def vec_cross(a: list[float], b: list[float]):
+    cross = [0, 0, 0]
+    cross[0] = a[1] * b[2] - a[2] * b[1]
+    cross[1] = a[2] * b[0] - a[0] * b[2]
+    cross[2] = a[0] * b[1] - a[1] * b[0]
+    return cross
+
+def vec_normalize(vec: list[float]):
+    l = vec_len(vec)
+    if l == 0:
+        return vec
+    return [v / l for v in vec]
+
+def vec_is_right_handed(order: str, negations: list[bool] = None):
+    order = order.lower()
+    if negations is None: #Must build the negation list
+        base_order = order.replace('-', '')
+        num_negations = order.count('-')
+    else:
+        base_order = order
+        num_negations = sum(negations)
+
+    right_handed = base_order in ("xzy", "yxz", "zyx")
+    if num_negations & 1: #Odd number of negations causes handedness to swap
+        right_handed = not right_handed
+    
+    return right_handed
+
+def axis_to_unit_vector(axis: str):
+    axis = axis.lower()
+    if axis == 'x' or axis == 0: return [1, 0, 0]
+    if axis == 'y' or axis == 1: return [0, 1, 0]
+    if axis == 'z' or axis == 2: return [0, 0, 1]
+
+def parse_axis_string(axis: str):
+    """
+    Given an axis order string, convert it to an array representing the axis order and negations/multipliers
+    """
+    axis = axis.lower()
+    order = [0, 1, 2]
+    multipliers = [1, 1, 1]
+    if 'x' in axis: #Using XYZ notation
+        index = 0
+        for c in axis:
+            if c == '-':
+                multipliers[index] = -1
+                continue
+            order[index] = ord(c) - ord('x')
+            index += 1
+    else:           #Using N/S E/W U/D notation
+        axis_lookup = {'e': 0, 'w': 0, 'u': 1, 'd': 1, 'n': 2, 's': 2}
+        negative_axes = "wds"
+        for i, c in enumerate(axis):
+            order[i] = axis_lookup[c]
+            if c in negative_axes:
+                multipliers[i] = -1
+    
+    return order, multipliers
+
+def parse_axis_string_info(axis: str):
+    order, mult = parse_axis_string(axis)
+    right_handed = axis_is_righthanded(order, mult)
+    return [order, mult, right_handed]
+
+def axis_is_righthanded(order: list[int], negations: list[int]):
+    num_swaps = 0
+    for i in range(3):
+        if i != order[i]:
+            num_swaps += 1
+    
+    right_handed = num_swaps == 2 #Defaults to left handed, but if a singular swap occured, the handedness swaps
+    if negations.count(-1) & 1: #Odd number of negations causes handedness to swap
+        right_handed = not right_handed
+    
+    return right_handed

{yostlabs-2025.2.17 → yostlabs-2025.4.28}/src/yostlabs/tss3/api.py

@@ -419,6 +419,10 @@ THREESPACE_AWAIT_COMMAND_FOUND = 0
 THREESPACE_AWAIT_COMMAND_TIMEOUT = 1
 THREESPACE_AWAIT_BOOTLOADER = 2
 
+THREESPACE_UPDATE_COMMAND_PARSED = 0
+THREESPACE_UPDATE_COMMAND_NOT_ENOUGH_DATA = 1
+THREESPACE_UPDATE_COMMAND_MISALIGNED = 2
+
 T = TypeVar('T')
 
 @dataclass
@@ -464,34 +468,43 @@ class ThreespaceBootloaderInfo:
 THREESPACE_REQUIRED_HEADER = THREESPACE_HEADER_ECHO_BIT | THREESPACE_HEADER_CHECKSUM_BIT | THREESPACE_HEADER_LENGTH_BIT
 class ThreespaceSensor:
     
-    def __init__(self, com = None, timeout=2, verbose=False):
+    def __init__(self, com = None, timeout=2, verbose=False, initial_clear_timeout=None):
         if com is None: #Default to attempting to use the serial com class if none is provided
             com = ThreespaceSerialComClass
-        
+        self.verbose = verbose
+
+        manually_opened_com = False
         #Auto discover using the supplied com class type
         if inspect.isclass(com) and issubclass(com, ThreespaceComClass):
             new_com = None
+            self.log("Auto-Discovering Sensor")
             for serial_com in com.auto_detect():
                 new_com = serial_com
                 break #Exit after getting 1
             if new_com is None:
-                raise RuntimeError("Failed to auto discover com port")
+                raise RuntimeError("Failed to auto discover com port")   
             self.com = new_com
+            manually_opened_com = True
             self.com.open()
         #The supplied com already was a com class, nothing to do
         elif inspect.isclass(type(com)) and issubclass(type(com), ThreespaceComClass):
             self.com = com
+            if not self.com.check_open():
+                self.com.open()
+                manually_opened_com = True
         else: #Unknown type, try making a ThreespaceSerialComClass out of this
             try:
                 self.com = ThreespaceSerialComClass(com)
             except:
                 raise ValueError("Failed to create default ThreespaceSerialComClass from parameter:", type(com), com)
 
+        self.restart_delay = 0.5
+
+        self.log("Configuring sensor communication")
         self.immediate_debug = True #Assume it is on from the start. May cause it to take slightly longer to initialize, but prevents breaking if it is on
         #Callback gives the debug message and sensor object that caused it
         self.__debug_cache: list[str] = [] #Used for storing startup debug messages until sensor state is confirmed
         
-        self.verbose = verbose
         self.debug_callback: Callable[[str, ThreespaceSensor],None] = self.__default_debug_callback
         self.misaligned = False
         self.dirty_cache = False
@@ -502,7 +515,15 @@ class ThreespaceSensor:
         self.is_data_streaming = False
         self.is_log_streaming = False
         self.is_file_streaming = False
+        self.log("Stopping potential streaming")
         self._force_stop_streaming()
+        #Clear out the buffer to allow faster initializing
+        #Ex: If a large buffer build up due to streaming, especially if using a slower interface like BLE,
+        #it may take a while before the entire garbage data can be parsed when checking for bootloader, causing a timeout
+        #even though it would have eventually succeeded   
+        self.log("Clearing com")     
+        self.__clear_com(initial_clear_timeout)
+
 
         #Used to ensure connecting to the correct sensor when reconnecting
         self.serial_number = None
@@ -514,12 +535,24 @@ class ThreespaceSensor:
         self.getStreamingBatchCommand: ThreespaceGetStreamingBatchCommand = None
         self.funcs = {}
 
-        self.__cached_in_bootloader = self.__check_bootloader_status()
-        if not self.in_bootloader:
-            self.__firmware_init()
-        else:
-            self.__cache_serial_number(self.bootloader_get_sn())
-            self.__empty_debug_cache()
+        self.log("Checking firmware status")
+        try:
+            self.__cached_in_bootloader = self.__check_bootloader_status()
+            if not self.in_bootloader:
+                self.log("Initializing firmware")
+                self.__firmware_init()
+            else:
+                self.log("Initializing bootloader")
+                self.__cache_serial_number(self.bootloader_get_sn())
+                self.__empty_debug_cache()
+        #This is just to prevent a situation where instantiating the API creates and fails to release a com class on failure when user catches the exception
+        #If user provides the com class, it is up to them to handle its state on error
+        except Exception as e:
+            self.log("Failed to initialize sensor")
+            if manually_opened_com:
+                self.com.close()
+            raise e
+        self.log("Successfully initialized sensor")
 
     #Just a helper for outputting information
     def log(self, *args):
@@ -528,6 +561,17 @@ class ThreespaceSensor:
 
 #-----------------------INITIALIZIATION & REINITIALIZATION-----------------------------------
 
+    def __clear_com(self, refresh_timeout=None):
+        data = self.com.read_all()
+        if refresh_timeout is None: return
+        while len(data) > 0: #Continue until all data is cleared
+            self.log(f"Refresh clear Length: {len(data)}")
+            start_time = time.time()
+            while time.time() - start_time < refresh_timeout: #Wait up to refresh time for a new message
+                data = self.com.read_all()
+                if len(data) > 0:
+                    break #Refresh the start time and wait for more data
+
     def __firmware_init(self):
         """
         Should only be called when not streaming and known in firmware.
@@ -619,8 +663,8 @@ class ThreespaceSensor:
             method = types.MethodType(command.custom_func, self)
         else:
             #Build the actual method for executing the command
-            code = f"def {command.info.name}(self, *args):\n"
-            code += f"    return self.execute_command(self.commands[{command.info.num}], *args)"
+            code = f"def {command.info.name}(self, *args, **kwargs):\n"
+            code += f"    return self.execute_command(self.commands[{command.info.num}], *args, **kwargs)"
             exec(code, globals(), self.funcs)
             method = types.MethodType(self.funcs[command.info.name], self)
 
@@ -734,6 +778,17 @@ class ThreespaceSensor:
 
 #-----------------------------------------------BASE SETTINGS PROTOCOL------------------------------------------------
 
+    #Helper for converting python types to strings that set_settings can understand
+    def __internal_str(self, value):
+        if isinstance(value, float):
+            return f"{value:.10f}"
+        elif isinstance(value, bool):
+            return int(value)
+        elif isinstance(value, Enum):
+            return str(value.value)
+        else:
+            return str(value)        
+
     #Can't just do if "header" in string because log_header_enabled exists and doesn't actually require cacheing the header
     HEADER_KEYS = ["header", "header_status", "header_timestamp", "header_echo", "header_checksum", "header_serial", "header_length"]
     def set_settings(self, param_string: str = None, **kwargs):
@@ -745,10 +800,10 @@ class ThreespaceSensor:
         
         for key, value in kwargs.items():
             if isinstance(value, list):
-                value = [str(v) for v in value]
+                value = [self.__internal_str(v) for v in value]
                 value = ','.join(value)
-            elif isinstance(value, bool):
-                value = int(value)
+            else:
+                value = self.__internal_str(value)
             params.append(f"{key}={value}")
         cmd = f"!{';'.join(params)}\n"
 
@@ -757,8 +812,7 @@ class ThreespaceSensor:
             return 0xFF, 0xFF
 
         #For dirty check
-        keys = cmd[1:-1].split(';')
-        keys = [v.split('=')[0] for v in keys]
+        param_dict = threespaceSetSettingsStringToDict(cmd[1:-1])
 
         #Must enable this before sending the set so can properly handle reading the response
         if "debug_mode=1" in cmd:
@@ -786,14 +840,14 @@ class ThreespaceSensor:
         #Handle updating state variables based on settings
         #If the user modified the header, need to cache the settings so the API knows how to interpret responses
         if "header" in cmd.lower(): #First do a quick check
-            if any(v in keys for v in ThreespaceSensor.HEADER_KEYS): #Then do a longer check
+            if any(v in param_dict.keys() for v in ThreespaceSensor.HEADER_KEYS): #Then do a longer check
                 self.__cache_header_settings()
         
         if "stream_slots" in cmd.lower():
             self.__cache_streaming_settings()
         
         #All the settings changed, just need to mark dirty
-        if any(v in keys for v in ("default", "reboot")):
+        if any(v in param_dict.keys() for v in ("default", "reboot")):
             self.set_cached_settings_dirty()
 
         if err:
@@ -965,11 +1019,18 @@ class ThreespaceSensor:
         """
         header_len = len(header.raw_binary)
         if header.length > max_data_length:
-            self.log("DATA TOO BIG:", header.length)
+            if not self.misaligned:
+                self.log("DATA TOO BIG:", header.length)
             return False
         data = self.com.peek(header_len + header.length)[header_len:]
-        if len(data) != header.length: return False
+        if len(data) != header.length: 
+            if not self.misaligned:
+                self.log(f"Data Length Mismatch - Got: {len(data)} Expected: {header.length}")
+            return False
         checksum = sum(data) % 256
+        if checksum != header.checksum and not self.misaligned:
+            self.log(f"Checksum Mismatch - Got: {checksum} Expected: {header.checksum}")
+            self.log(f"Data: {data}")
         return checksum == header.checksum
 
     def __await_command(self, cmd: ThreespaceCommand, timeout=2):
@@ -1006,7 +1067,7 @@ class ThreespaceSensor:
 
 #------------------------------BASE INPUT PARSING--------------------------------------------
 
-    def __internal_update(self, header: ThreespaceHeader = None):
+    def __internal_update(self, header: ThreespaceHeader = None, blocking=True):
         """
         Manages checking the datastream for asynchronous responses (Streaming, Immediate Debug Messages).
         If no data is found to match these responses, the data buffer will be considered corrupted/misaligned
@@ -1021,29 +1082,39 @@ class ThreespaceSensor:
 
         Returns
         --------
-        False : Misalignment
-        True : Internal Data Found/Parsed
+        0 : Internal Data Found/Parsed
+        1 : Not enough data (Only possible when blocking == False)
+        2 : Misalignment
         """
         checksum_match = False #Just for debugging
 
         if header is not None:
             #NOTE: FOR THIS TO WORK IT IS REQUIRED THAT THE HEADER DOES NOT CHANGE WHILE STREAMING ANY FORM OF DATA.
             #IT IS UP TO THE API TO ENFORCE NOT ALLOWING HEADER CHANGES WHILE ANY OF THOSE THINGS ARE HAPPENING
-            if self.is_data_streaming and header.echo == THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM: #Its a streaming packet, so update streaming
+            if self.is_data_streaming and header.echo == THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM:
+                if not blocking:
+                    expected_output_size = len(header.raw_binary) + self.getStreamingBatchCommand.info.out_size
+                    if self.com.length < expected_output_size: return THREESPACE_UPDATE_COMMAND_NOT_ENOUGH_DATA
                 if checksum_match := self.__peek_checksum(header, max_data_length=self.getStreamingBatchCommand.info.out_size):
                     self.__update_base_streaming()
                     self.misaligned = False
-                    return True
+                    return THREESPACE_UPDATE_COMMAND_PARSED
             elif self.is_log_streaming and header.echo == THREESPACE_FILE_READ_BYTES_COMMAND_NUM:
-                if checksum_match := self.__peek_checksum(header, max_data_length=2560): #TODO: Confirm this number can be 2048 and then pound define it. Currently set to 2560 because I know that is big enough, but not optimal
+                if not blocking: 
+                    expected_output_size = len(header.raw_binary) + min(header.length, THREESPACE_LIVE_LOG_STREAM_MAX_PACKET_SIZE)
+                    if self.com.length < expected_output_size: return THREESPACE_UPDATE_COMMAND_NOT_ENOUGH_DATA
+                if checksum_match := self.__peek_checksum(header, max_data_length=THREESPACE_LIVE_LOG_STREAM_MAX_PACKET_SIZE):
                     self.__update_log_streaming()
                     self.misaligned = False
-                    return True
+                    return THREESPACE_UPDATE_COMMAND_PARSED
             elif self.is_file_streaming and header.echo == THREESPACE_FILE_READ_BYTES_COMMAND_NUM:
+                if not blocking:
+                    expected_output_size = len(header.raw_binary) + min(header.length, THREESPACE_FILE_STREAMING_MAX_PACKET_SIZE)
+                    if self.com.length < expected_output_size: return THREESPACE_UPDATE_COMMAND_NOT_ENOUGH_DATA
                 if checksum_match := self.__peek_checksum(header, max_data_length=THREESPACE_FILE_STREAMING_MAX_PACKET_SIZE):
                     self.__update_file_streaming()
                     self.misaligned = False
-                    return True
+                    return THREESPACE_UPDATE_COMMAND_PARSED
         
         #Debug messages are possible and there is enough data to potentially be a debug message
         #NOTE: Firmware should avoid putting more then one \r\n in a debug message as they will be treated as unprocessed/misaligned characters
@@ -1058,7 +1129,7 @@ class ThreespaceSensor:
                     message = self.com.readline() #Read out the whole message!
                     self.debug_callback(message.decode('ascii'), self)
                     self.misaligned = False
-                    return True
+                    return THREESPACE_UPDATE_COMMAND_PARSED
 
         #The response didn't match any of the expected asynchronous streaming API responses, so assume a misalignment
         if header is not None:
@@ -1069,7 +1140,7 @@ class ThreespaceSensor:
             msg = "Possible Misalignment or corruption/debug message"
         #self.log("Misaligned:", self.com.peek(1))
         self.__handle_misalignment(msg)
-        return False
+        return THREESPACE_UPDATE_COMMAND_MISALIGNED
 
     def __handle_misalignment(self, message: str = None):
         if not self.misaligned and message is not None:
@@ -1167,12 +1238,21 @@ class ThreespaceSensor:
         self.streaming_packets.clear()
     
     #This is called for all streaming types
-    def updateStreaming(self, max_checks=float('inf')):
+    def updateStreaming(self, max_checks=float('inf'), timeout=None, blocking=False):
         """
         Returns true if any amount of data was processed whether valid or not. This is called for all streaming types.
+
+        Parameters
+        ----------
+        max_checks : Will only attempt to read up to max_checks packets
+        timeout : Will only attempt to read packets for this duration. It is possible for this function to take longer then this timeout \
+        if blocking = True, in which case it could take up to timeout + com.timeout 
+        blocking : If False, will immediately stop when not enough data is available. If true, will immediately stop if not enough data \
+        for a header, but will block when trying to retrieve the data associated with that header. For most com classes, this does not matter. \
+        But for communication such as BLE where the header and data may be split between different packets, this will have a clear effect.
         """
         if not self.is_streaming: return False
-
+        if timeout is None: timeout = float('inf')
         #I may need to make this have a max num bytes it will process before exiting to prevent locking up on slower machines
         #due to streaming faster then the program runs
         num_checks = 0
@@ -1182,12 +1262,17 @@ class ThreespaceSensor:
                 return data_processed
             
             #Get header
+
             header = self.com.peek(self.header_info.size)
 
             #Get the header and send it to the internal update
             header = ThreespaceHeader.from_bytes(header, self.header_info)
-            self.__internal_update(header)
-            data_processed = True #Internal update always processes data. Either reads a streaming message, or advances buffer due to misalignment
+            result = self.__internal_update(header, blocking=blocking)
+            if result == THREESPACE_UPDATE_COMMAND_PARSED:
+                data_processed = True
+            elif result == THREESPACE_UPDATE_COMMAND_NOT_ENOUGH_DATA:
+                return data_processed
+            
             num_checks += 1
         
         return data_processed
@@ -1309,7 +1394,7 @@ class ThreespaceSensor:
         cmd.send_command(self.com)
         self.com.close()
         #TODO: Make this actually wait instead of an arbitrary sleep length
-        time.sleep(0.5) #Give it time to restart
+        time.sleep(self.restart_delay) #Give it time to restart
         self.com.open()
         self.__firmware_init()
 
@@ -1320,7 +1405,7 @@ class ThreespaceSensor:
         cmd = self.commands[THREESPACE_ENTER_BOOTLOADER_COMMAND_NUM]
         cmd.send_command(self.com)
         #TODO: Make this actually wait instead of an arbitrary sleep length
-        time.sleep(0.5) #Give it time to boot into bootloader
+        time.sleep(self.restart_delay) #Give it time to boot into bootloader
         if self.com.reenumerates:
             self.com.close()
             success = self.__attempt_rediscover_self()
@@ -1384,7 +1469,7 @@ class ThreespaceSensor:
     def bootloader_boot_firmware(self):
         if not self.in_bootloader: return
         self.com.write("B".encode())
-        time.sleep(0.5) #Give time to boot into firmware
+        time.sleep(self.restart_delay) #Give time to boot into firmware
         if self.com.reenumerates:
             self.com.close()
             success = self.__attempt_rediscover_self()
@@ -1401,13 +1486,14 @@ class ThreespaceSensor:
         This may take a long time
         """
         self.com.write('S'.encode())
-        if timeout is not None:
-            cached_timeout = self.com.timeout
-            self.com.timeout = timeout
-        response = self.com.read(1)[0]
-        if timeout is not None:
-            self.com.timeout = cached_timeout
-        return response
+
+        start_time = time.perf_counter()
+        response = []
+        while len(response) == 0 and time.perf_counter() - start_time < timeout:
+            response = self.com.read(1)
+        if len(response) == 0:
+            return -1
+        return response[0]
     
     def bootloader_get_info(self):
         self.com.write('I'.encode())
@@ -1417,14 +1503,20 @@ class ThreespaceSensor:
         bootversion = struct.unpack(f">{_3space_format_to_external('I')}", self.com.read(2))[0]
         return ThreespaceBootloaderInfo(memstart, memend, pagesize, bootversion)
 
-    def bootloader_prog_mem(self, bytes: bytearray):
+    def bootloader_prog_mem(self, bytes: bytearray, timeout=5):
         memsize = len(bytes)
         checksum = sum(bytes)
         self.com.write('C'.encode())
         self.com.write(struct.pack(f">{_3space_format_to_external('I')}", memsize))
         self.com.write(bytes)
         self.com.write(struct.pack(f">{_3space_format_to_external('B')}", checksum & 0xFFFF))
-        return self.com.read(1)[0]
+        start_time = time.perf_counter()
+        result = []
+        while len(result) == 0 and time.perf_counter() - start_time < timeout:
+            result = self.com.read(1)
+        if len(result) > 0:
+            return result[0]
+        return -1
 
     def bootloader_get_state(self):
         self.com.write('OO'.encode()) #O is sent twice to compensate for a bug in some versions of the bootloader where the next character is ignored (except for R, do NOT send R after O, it will erase all settings)
@@ -1436,20 +1528,26 @@ class ThreespaceSensor:
         self.com.write("RR".encode())
 
     def cleanup(self):
-        if not self.in_bootloader:
-            if self.is_data_streaming:
-                self.stopStreaming()
-            if self.is_file_streaming:
-                self.fileStopStream()
-            if self.is_log_streaming:
-                self.stopDataLogging()
-
-            #The sensor may or may not have this command registered. So just try it
-            try:
-                #May not be opened, but also not cacheing that so just attempt to close.
-                self.closeFile()
-            except: pass
-        self.com.close()
+        error = None
+        try:
+            if not self.in_bootloader:
+                if self.is_data_streaming:
+                    self.stopStreaming()
+                if self.is_file_streaming:
+                    self.fileStopStream()
+                if self.is_log_streaming:
+                    self.stopDataLogging()
+
+                #The sensor may or may not have this command registered. So just try it
+                try:
+                    #May not be opened, but also not cacheing that so just attempt to close.
+                    self.closeFile()
+                except: pass
+        except Exception as e:
+            error = e
+        self.com.close() #Ensuring the close gets called, that way com ports can't get stuck open. Also makes calling cleanup() "safe" even after disconnect
+        if error:
+            raise error
 
 #-------------------------START ALL PROTOTYPES------------------------------------
 
@@ -1738,4 +1836,17 @@ def threespaceGetHeaderLabels(header_info: ThreespaceHeaderInfo):
         order.append("serial#")
     if header_info.length_enabled:
         order.append("len")
-    return order
+    return order
+
+def threespaceSetSettingsStringToDict(setting_string: str):
+    d = {}
+    for item in setting_string.split(';'):
+        result = item.split('=')
+        key = result[0]
+        if len(result) == 1:
+            value = None
+        else:
+            value = '='.join(result[1:]) #In case = was part of the value, do a join
+        
+        d[key] = value
+    return d

{yostlabs-2025.2.17 → yostlabs-2025.4.28}/src/yostlabs/tss3/consts.py

@@ -29,6 +29,7 @@ THREESPACE_GET_SETTINGS_ERROR_RESPONSE = "<KEY_ERROR>"
 THREESPACE_SETTING_KEY_INVALID_CHARS = " :;"
 
 THREESPACE_FILE_STREAMING_MAX_PACKET_SIZE = 512
+THREESPACE_LIVE_LOG_STREAM_MAX_PACKET_SIZE = 2048
 
 THREESPACE_SN_FAMILY_POS = 14 * 4
 THREESPACE_SN_VARIATION_POS = 11 * 4

yostlabs-2025.2.17/src/yostlabs/math/vector.py

@@ -1,31 +0,0 @@
-def vec_len(vector: list[float|int]):
-    return sum(v ** 2 for v in vector) ** 0.5
-
-def vec_dot(a: list[float], b: list[float]):
-    return sum(a[i] * b[i] for i in range(len(a)))
-
-def vec_cross(a: list[float], b: list[float]):
-    cross = [0, 0, 0]
-    cross[0] = a[1] * b[2] - a[2] * b[1]
-    cross[1] = a[2] * b[0] - a[0] * b[2]
-    cross[2] = a[0] * b[1] - a[1] * b[0]
-    return cross
-
-def vec_normalize(vec: list[float]):
-    l = vec_len(vec)
-    if l == 0:
-        return vec
-    return [v / l for v in vec]
-
-def vec_is_right_handed(order: str, negations: list[bool]):
-    right_handed = order.lower() in ("xzy", "yxz", "zyx")
-    for i in range(3):
-        if negations[i]:
-            right_handed = not right_handed
-    return right_handed
-
-def axis_to_unit_vector(axis: str):
-    axis = axis.lower()
-    if axis == 'x' or axis == 0: return [1, 0, 0]
-    if axis == 'y' or axis == 1: return [0, 1, 0]
-    if axis == 'z' or axis == 2: return [0, 0, 1]