yostlabs 2025.3.6__tar.gz → 2025.4.28__tar.gz

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

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: yostlabs
-Version: 2025.3.6
+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

{yostlabs-2025.3.6 → 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.03.6"
+version = "2025.04.28"
 authors = [
   { name="Yost Labs Inc.", email="techsupport@yostlabs.com" },
   { name="Andy Riedlinger", email="techsupport@yostlabs.com" },

{yostlabs-2025.3.6 → yostlabs-2025.4.28}/src/yostlabs/communication/ble.py

@@ -39,14 +39,14 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         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)
         """
-        self.event_loop = asyncio.new_event_loop()
         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
-                device = self.event_loop.run_until_complete(BleakScanner.find_device_by_name(ble, timeout=discovery_timeout))
+                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)
@@ -63,7 +63,8 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         self.__timeout = self.DEFAULT_TIMEOUT
 
         self.buffer = bytearray()
-        self.data_read_event = asyncio.Event()
+        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
@@ -91,6 +92,8 @@ class ThreespaceBLEComClass(ThreespaceComClass):
             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
@@ -106,6 +109,9 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         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):
@@ -138,6 +144,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
 
     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):
@@ -153,10 +160,10 @@ class ThreespaceBLEComClass(ThreespaceComClass):
     
     async def __await_read(self, timeout_time: int):
         self.__assert_connected()
-        self.data_read_event.clear()
         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

{yostlabs-2025.3.6 → 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.3.6 → 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
 
@@ -160,8 +158,8 @@ def quaternion_swap_axes_fast(quat: list, old_parsed_order: list[list, list, boo
     
     if old_right_handed != new_right_handed:
         #Different handed systems rotate opposite directions. So to maintain the same rotation,
-        #negate the rotation of the quaternion when swapping systems
-        new_quat[-1] *= -1
+        #invert the quaternion
+        new_quat = quat_inverse(new_quat)
 
     return new_quat
 

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

@@ -471,16 +471,18 @@ class ThreespaceSensor:
     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()
@@ -496,11 +498,13 @@ class ThreespaceSensor:
             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
@@ -511,11 +515,13 @@ 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        
+        #even though it would have eventually succeeded   
+        self.log("Clearing com")     
         self.__clear_com(initial_clear_timeout)
 
 
@@ -529,19 +535,24 @@ class ThreespaceSensor:
         self.getStreamingBatchCommand: ThreespaceGetStreamingBatchCommand = None
         self.funcs = {}
 
+        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):
@@ -554,6 +565,7 @@ class ThreespaceSensor:
         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()
@@ -651,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)
 
@@ -766,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):
@@ -777,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"
 
@@ -996,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):
@@ -1364,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()
 
@@ -1375,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()
@@ -1439,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()
@@ -1456,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())
@@ -1472,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)