yostlabs 2025.10.29.1__tar.gz → 2026.2.4__tar.gz

yostlabs-2026.2.4/Examples/example_commands.py

@@ -0,0 +1,33 @@
+from yostlabs.tss3.api import ThreespaceSensor
+from yostlabs.communication.serial import ThreespaceSerialComClass
+
+#Create a sensor by auto detecting a ThreespaceSerialComClass
+sensor = ThreespaceSensor(ThreespaceSerialComClass)
+
+result = sensor.getPrimaryCorrectedAccelVec()
+
+#Read out the result
+accel_vec = result.data
+
+#The result is a dataclass that allows easily accessing 
+# individual parts of the data and multiple ways of interpreting it
+print("Result:")
+#The base ThreespaceCmdResult dataclass
+print(result)
+#The actual byte response from the sensor that made this result (header + data)
+print(result.raw_binary)
+print()
+print("Header:")
+#The ThreespaceHeader data class
+print(result.header)
+#The individual components of the header as an array
+#similar to how the old API functioned
+print(result.header.raw)
+#The actual byte representation of the header from the sensor
+print(result.header.raw_binary)
+print()
+print("Data:")
+print(f"{result.data=}")
+print(f"{result.raw_data=}")
+
+sensor.cleanup()

{yostlabs-2025.10.29.1 → yostlabs-2026.2.4}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: yostlabs
-Version: 2025.10.29.1
+Version: 2026.2.4
 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.10.29.1 → yostlabs-2026.2.4}/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.10.29.1"
+version = "2026.2.4"
 authors = [
   { name="Yost Labs Inc.", email="techsupport@yostlabs.com" },
   { name="Andy Riedlinger", email="techsupport@yostlabs.com" },

{yostlabs-2025.10.29.1 → yostlabs-2026.2.4}/src/yostlabs/communication/bluetooth.py

@@ -2,6 +2,8 @@ from yostlabs.communication.socket import ThreespaceSocketComClass
 import bluetooth
 import time
 import threading
+import sys
+import math
 
 from dataclasses import dataclass
 from typing import Callable, Generator
@@ -94,17 +96,21 @@ class ScannerResult:
 
 class Scanner:
 
-    def __init__(self, interval=5):
+    def __init__(self, desired_scan_time=5):
         self.enabled = False
         self.done = True
 
-        self.nearby = None
-        self.thread = None
-        self.updated = False
-        self.duration = int(interval / 1.2)
+        self.continous = False      #If true, will constantly update the nearby devices
+        self.execute = False        #Set to true to trigger a read when not in continous mode
+        self.nearby = None          #The list of nearby devices
+        self.thread = None          #The thread running the asynchronous scanner
+        self.updated = False        #Set to true when self.nearby has been updated
+        self.duration = math.ceil(desired_scan_time / 1.28) * 1.28  #The time, in second, each scan lasts. Scanning is in 1.28 second intervals, so this may differ from the desired scan time
 
     def start(self):
-        if not self.done: return
+        if not self.done:
+            self.execute = True
+            return
         self.thread = threading.Thread(target=self.process, daemon=True)
         self.enabled = True
         self.done = False
@@ -114,6 +120,9 @@ class Scanner:
     def stop(self):
         self.enabled = False
 
+    def set_continous(self, continous: bool):
+        self.continous = continous
+
     def get_most_recent(self):
         if not self.updated: return None
         self.updated = False
@@ -125,20 +134,57 @@ class Scanner:
 
     def process(self):
         while self.enabled:
-            nearby = bluetooth.discover_devices(duration=self.duration, lookup_names=True, lookup_class=True)
+            if not self.continous and not self.execute: continue
+            nearby = bluetooth.discover_devices(duration=math.ceil(self.duration/1.28), lookup_names=True, lookup_class=True)
             self.nearby = [ScannerResult(addr, name, decode_class_of_device(cod)) for addr, name, cod in nearby]
+            self.execute = False
             self.updated = True
         self.done = True
 
+if sys.platform == "win32":
+    import ctypes
+    from ctypes import wintypes
+
+    # Define BLUETOOTH_ADDRESS structure
+    class BLUETOOTH_ADDRESS(ctypes.Structure):
+        _fields_ = [("rgBytes", ctypes.c_byte * 6)]
+
+    # Load Bluetooth API
+    win_bluetooth = ctypes.WinDLL("BluetoothAPIs.dll")
+
+    # Define function prototype
+    BluetoothRemoveDevice = win_bluetooth.BluetoothRemoveDevice
+    BluetoothRemoveDevice.argtypes = [ctypes.POINTER(BLUETOOTH_ADDRESS)]
+    BluetoothRemoveDevice.restype = wintypes.BOOL
+
+    def remove_device(mac_address: str):
+        # Convert MAC string "XX:XX:XX:XX:XX:XX" -> bytes reversed
+        addr_bytes = bytes.fromhex(mac_address.replace(":", ""))[::-1]
+        addr = BLUETOOTH_ADDRESS()
+        ctypes.memmove(addr.rgBytes, addr_bytes, 6)
+
+        err = BluetoothRemoveDevice(ctypes.byref(addr))
+        print("Remove device status:", err)
+        if not err:
+            print(f"Successfully removed device {mac_address}")
+        else:
+            if err == 1168:  # ERROR_NOT_FOUND
+                print(f"Device {mac_address} not found or already unpaired.")
+            else:
+                print(f"Failed to remove device {mac_address}. Error code: {err}")
+else:
+    def remove_device(mac_address: str):
+        raise NotImplementedError("Only implemented on windows")
+
 class ThreespaceBluetoothComClass(ThreespaceSocketComClass):
 
-    SCANNER = None
+    SCANNER: Scanner = None
 
     def __init__(self, addr: str, name: str = None, connection_timeout=None):
         super().__init__(bluetooth.BluetoothSocket(bluetooth.Protocols.RFCOMM), (addr, 1), connection_timeout=connection_timeout)
         self.address = addr
         self.__name = name or addr
-    
+
     @property
     def name(self) -> str:
         return self.__name
@@ -149,6 +195,11 @@ class ThreespaceBluetoothComClass(ThreespaceSocketComClass):
             cls.SCANNER = Scanner()
             cls.SCANNER.start()
 
+    @classmethod
+    def set_scanner_continous(cls, continous: bool):
+        cls.__lazy_init_scanner()
+        cls.SCANNER.set_continous(continous)
+
     @staticmethod
     def __default_filter(result: ScannerResult):
         return result.class_of_device.major_class == "Wearable" and result.class_of_device.minor_class == "Minor code 0"
@@ -162,6 +213,7 @@ class ThreespaceBluetoothComClass(ThreespaceSocketComClass):
         cls.__lazy_init_scanner()
         if filter is None: 
             filter = cls.__default_filter
+        cls.SCANNER.start()
         if wait_for_update:
             cls.SCANNER.updated = False
             while not cls.SCANNER.updated: time.sleep(0.1)
@@ -171,6 +223,18 @@ class ThreespaceBluetoothComClass(ThreespaceSocketComClass):
             name = device_info.name or None
             yield ThreespaceBluetoothComClass(device_info.address, name)
 
+    @staticmethod
+    def unpair(address: str):
+        """
+        It is recommended to call this after done with a ThreespaceBluetoothComClass object
+        so that windows will not report the device as still being available when powered off.
+        This occurs because windows Bluetooth detect functions report nearby devices & paired devices,
+        regardless of their power status. By removing the device from the list of paired devices, auto detect
+        will only report actual nearby and powered devices.
+        EX: ThreespaceBluetoothComClass.unpair(com.address)
+        """
+        remove_device(address)
+
 
 if __name__ == "__main__":
     from yostlabs.tss3.api import ThreespaceSensor

yostlabs-2026.2.4/src/yostlabs/math/quaternion.py

@@ -0,0 +1,354 @@
+import math
+import yostlabs.math.vector as _vec
+
+def quat_mul(a: list[float], b: list[float]):
+    out = [0, 0, 0, 0]
+    x = 0; y = 1; z = 2; w = 3
+    out[w] = a[w]*b[w] - a[x]*b[x] - a[y]*b[y] - a[z]*b[z]
+    out[x] = a[w]*b[x] + a[x]*b[w] + a[y]*b[z] - a[z]*b[y]
+    out[y] = a[w]*b[y] + a[y]*b[w] + a[z]*b[x] - a[x]*b[z]
+    out[z] = a[w]*b[z] + a[z]*b[w] + a[x]*b[y] - a[y]*b[x]
+    return out
+
+#Rotates quaternion b by quaternion a, it does not combine them
+def quat_rotate(a: list[float], b: list[float]):
+    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]):
+    inv = quat_inverse(quat)
+    tmp = [vec[0], vec[1], vec[2], 0]
+    halfway = quat_mul(quat, tmp)
+    final = quat_mul(halfway, inv)
+    return [final[0], final[1], final[2]]
+
+def angles_to_quaternion(angles: list[float], order: str, degrees=True, extrinsic=False):
+    quat = [0, 0, 0, 1]
+    for i in range(len(angles)):
+        axis = order[i]
+        angle = angles[i]
+        if degrees:
+            angle = math.radians(angle)
+        unit_vec = _vec.axis_to_unit_vector(axis)
+        w = math.cos(angle / 2)
+        imaginary = math.sin(angle / 2)
+        unit_vec = [v * imaginary for v in unit_vec]
+        angle_quat = [*unit_vec, w]
+        if extrinsic:
+            quat = quat_mul(angle_quat, quat)
+        else:
+            quat = quat_mul(quat, angle_quat)
+    return quat
+
+def quat_from_angles(angles: list[float], order: str, degrees=True, extrinsic=False):
+    return angles_to_quaternion(angles, order, degrees=degrees, extrinsic=extrinsic)
+
+def quat_from_euler(angles: list[float], order: list[int], degrees=False, extrinsic=False):
+    return angles_to_quaternion(angles, order, degrees=degrees, extrinsic=extrinsic)
+
+def quat_from_axis_angle(axis: list[float], angle: float):
+    imaginary = math.sin(angle / 2)
+    quat = [imaginary * v for v in axis]
+    quat.append(math.cos(angle / 2))
+    return quat
+
+#There are multiple valid quats that can be returned by this. The intention of this function
+#is to be able to rotate an arrow by the quat such that it points the correct direction. The rotation
+#of that arrow along its axis may differ though
+def quat_from_one_vector(vec: list[float]):
+    vec = _vec.vec_normalize(vec)
+    perpendicular = _vec.vec_normalize(_vec.vec_cross([0, 0, 1], vec))
+    angle = math.acos(_vec.vec_dot([0, 0, 1], vec))
+    return quat_from_axis_angle(perpendicular, angle)
+
+def quat_from_two_vectors(forward: list[float], down: list[float]):
+    """
+    This function requires two orthogonal vectors to work
+    """
+    forward_reference = [0, 0, 1]
+    down_reference = [0, -1, 0]
+
+    forward = _vec.vec_normalize(forward)
+    down = _vec.vec_normalize(down)
+
+    #Create the first rotation to align the forward axis
+    axis_of_rotation = _vec.vec_cross(forward_reference, forward)
+    axis_of_rotation = _vec.vec_normalize(axis_of_rotation)
+    if not any(abs(v) > 0 for v in axis_of_rotation):
+        axis_of_rotation = down_reference #This is just a direct 180 degree rotation around any orthogonal axis, so just use the down_ref
+    dot = min(1, max(-1, _vec.vec_dot(forward_reference, forward)))
+    angle1 = math.acos(dot)
+    imaginary = math.sin(angle1/2)
+    quat = [v * imaginary for v in axis_of_rotation] #XYZ
+    quat.append(math.cos(angle1/2)) #W
+
+    #Update the reference to figure out where it is after the turn
+    down_reference = quat_rotate_vec(quat, down_reference)
+
+    #find the rotation to make the remaining reference align with its given value
+    axis_of_rotation = _vec.vec_cross(down_reference, down)
+    axis_of_rotation = _vec.vec_normalize(axis_of_rotation)
+    if not any(abs(v) > 0 for v in axis_of_rotation):
+        axis_of_rotation = forward #Rotate along the final forward vector until up is aligned
+    dot = min(1, max(-1, _vec.vec_dot(down_reference, down)))
+    angle2 = math.acos(dot)
+    imaginary = math.sin(angle2/2)
+    rotation = [v * imaginary for v in axis_of_rotation] #XYZ
+    rotation.append(math.cos(angle2/2)) #W
+
+    quat = quat_mul(rotation, quat)
+    return quat
+
+def quaternion_to_3x3_rotation_matrix(quat):
+    """
+    Convert a quaternion in form x, y, z, w to a rotation matrix
+    """
+    x, y, z, w = quat
+
+    fTx = 2.0 * x
+    fTy = 2.0 * y
+    fTz = 2.0 * z
+    fTwx = fTx * w
+    fTwy = fTy * w
+    fTwz = fTz * w
+    fTxx = fTx * x
+    fTxy = fTy * x
+    fTxz = fTz * x
+    fTyy = fTy * y
+    fTyz = fTz * y
+    fTzz = fTz * z
+
+    out = [0] * 9
+    out[0] = 1.0-(fTyy+fTzz)
+    out[1] = fTxy-fTwz
+    out[2] = fTxz+fTwy
+    out[3] = fTxy+fTwz
+    out[4] = 1.0-(fTxx+fTzz)
+    out[5] = fTyz-fTwx
+    out[6] = fTxz-fTwy
+    out[7] = fTyz + fTwx
+    out[8] = 1.0 - (fTxx+fTyy)
+
+    return [
+        [out[0], out[1], out[2]],
+        [out[3], out[4], out[5]],
+        [out[6], out[7], out[8]],
+    ]
+
+#Quat is expected in XYZW order
+#https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/quat_2_euler_paper_ver2-1.pdf
+def q2ea(in_quat: list[float], order: list[int]) -> list[float]:
+    X, Y, Z, W = 0, 1, 2, 3 #Index order helpers
+    if len(order) != 3: raise Exception()
+    if len(in_quat) != 4: raise Exception()
+    out = [0, 0, 0]
+
+    i1, i2, i3 = order
+    i1n = (i1+ 1) % 3
+    i1nn = (i1n + 1) % 3
+
+    #Find the direction the final axis ends up pointing after the full rotation
+    #Note that this vector does not change when the third rotation is being applied.
+    v3_rot = [0, 0, 0]
+    v3_rot[i3] = 1 
+    v3_rot = quat_rotate_vec(in_quat, v3_rot)
+
+    #NOTE: Whenever using asin/acos, ensure the input is in range of -1 <= x <= 1
+    #All this math should result in that, but floating point sometimes causes values like 1.0000002 which can cause NANs
+    v3_rot = [max(-1, min(1, v)) for v in v3_rot]
+
+    #Can now discover the first 2 rotations
+    #This is because the first two rotations determine the direction of the final axis,
+    #and each rotation only affects 1 plane, therefore think of it like the first rotation
+    #positions the third axis underneath its final spot, and the second rotation swings it up to its final position.
+    #These can be calculated using trig and the known axis pattern.
+
+	#Non-Circular, Repeated Axes
+    #XZX, YXY, ZYZ
+    if (i1 == 0 and i2 == 2 and i3 == 0) or     \
+    (i1 == 1 and i2 == 0 and i3 == 1) or        \
+    (i1 == 2 and i2 == 1 and i3 == 2):      
+        out[0] = math.atan2(v3_rot[i1nn], v3_rot[i1n])
+        out[1] = math.acos(v3_rot[i1])
+	#Non-Circular, Non-Repeated Axes
+    #XZY, YXZ, ZYX
+    elif (i1 == 0 and i2 == 2 and i3 == 1) or   \
+    (i1 == 1 and i2 == 0 and i3 == 2) or        \
+    (i1 == 2 and i2 == 1 and i3 == 0):          
+        out[0] = math.atan2(v3_rot[i1nn], v3_rot[i1n])
+        out[1] = -math.asin(v3_rot[i1])
+	#Circular, Repeated Axes
+    #XYX, YZY, ZXZ
+    elif (i1 == 0 and i2 == 1 and i3 == 0) or   \
+    (i1 == 1 and i2 == 2 and i3 == 1) or        \
+    (i1 == 2 and i2 == 0 and i3 == 2):
+        out[0] = math.atan2(v3_rot[i1n], -v3_rot[i1nn])
+        out[1] = math.acos(v3_rot[i1])
+	#Circular, Non-Repeated Axes
+    #XYZ, YZX, ZXY
+    elif (i1 == 0 and i2 == 1 and i3 == 2) or   \
+    (i1 == 1 and i2 == 2 and i3 == 0) or        \
+    (i1 == 2 and i2 == 0 and i3 == 1):
+        out[0] = math.atan2(-v3_rot[i1n], v3_rot[i1nn])
+        out[1] = math.asin(v3_rot[i1]) #Note, the paper incorrectly says -asin
+    else:
+        raise ValueError("Invalid Order")
+
+    #Now compute the third angle
+
+    #Create a quaternion that applies the first two rotations
+    q1_rotation = [0, 0, 0, 0]
+    q1_rotation[W] = math.cos(out[0] / 2)
+    q1_rotation[i1] = math.sin(out[0] / 2)
+
+    q2_rotation = [0, 0, 0, 0]
+    q2_rotation[W] = math.cos(out[1] / 2)
+    q2_rotation[i2] = math.sin(out[1] / 2)
+
+    q12 = quat_mul(q1_rotation, q2_rotation)
+
+    #Now apply that quaternion and the original quaternion to the axis that will be rotated by the last axis
+    #(Rotating along Axis 3 does not change the position of Axis 3, this is done to obtain an axis that will be rotated
+    #so that the difference can be computed to decide how much rotation is needed for the last angle)
+    i3n = (i3 + 1) % 3
+    v3n = [0, 0, 0]
+    v3n[i3n] = 1
+
+    v3_q12 = v3n.copy()
+    v3_q = v3n.copy()
+    v3_q12 = quat_rotate_vec(q12, v3_q12)
+    v3_q = quat_rotate_vec(in_quat, v3_q)
+
+    #Now do trig to figure out how much rotation, and what direction, is needed to rotate v3_q12 to v3_q
+    d = _vec.vec_dot(v3_q12, v3_q) #Get angle between the two, ensure in range of acos
+    d = max(-1, min(1, d))
+    magnitude = math.acos(d)
+
+    #Determine the sign of the rotation
+    cross = _vec.vec_cross(v3_q12, v3_q)
+    sign = _vec.vec_dot(cross, v3_rot)
+    if sign < 0: sign = -1
+    else: sign = 1
+
+    out[2] = sign * abs(magnitude)
+
+    return out
+
+def string_order_to_indices(order: str):
+    order = order.lower()
+    int_order = []
+    if len(order) != 3: raise ValueError()
+    for c in order:
+        if c == 'x': int_order.append(0)
+        elif c == 'y': int_order.append(1)
+        elif c == 'z': int_order.append(2)
+        else: raise ValueError()
+    
+    return int_order
+
+def quat_to_euler_angles(in_quat: list[float], order: str|list[int], extrinsic=False):
+    if isinstance(order, str):
+        if 'e' in order.lower():
+            extrinsic = True
+        elif 'i' in order.lower():
+            extrinsic = False
+        #Only care about the first 3 chars, the 4th optional char is extrinsic vs intrinsic
+        order = string_order_to_indices(order[:3])
+    
+    #Extrinsic is performing the rotations around the axes of the original fixed system (Rotates around world axes)
+    #Intrinsic is performing the rotations around the axes of a coordinate system that rotates with each step (It rotates around the local axes of the rotating object).
+    #These two representations are very similar, and even related. An extrinsic rotation is the same as an intrinsic rotation by the same angles, but with the order inverted.
+    if extrinsic:
+        #Taking advantage of the above info that intrinsic = extrinsic but in reverse and vice versa.
+        order[0], order[2] = order[2], order[0]
+    
+    #Actual conversion, everything else is just setup
+    angles = q2ea(in_quat, order)
+
+    if extrinsic:
+        #Swap back to the original desired order
+        angles[0], angles[2] = angles[2], angles[0]
+    
+    return angles
+
+def quat_from_euler_angles(angles: list[float], order: list[int], degrees=False, extrinsic=False):
+    if isinstance(order, str):
+        order = _vec.parse_axis_string(order)[0]
+    quat = [0, 0, 0, 1]
+    for i in range(len(angles)):
+        axis = order[i]
+        angle = angles[i]
+        if degrees:
+            angle = math.radians(angle)
+
+        #Create unit vector for this
+        unit_vec = [0, 0, 0]
+        unit_vec[axis] = 1
+
+        #Create quaternion for this rotation and apply to overall rotation
+        angle_quat = quat_from_axis_angle(unit_vec, angle)
+        if extrinsic:
+            quat = quat_mul(angle_quat, quat)
+        else:
+            quat = quat_mul(quat, angle_quat)
+    return quat
+
+def quaternion_global_to_local(quat, vec):
+    inverse = quat_inverse(quat)
+    return quat_rotate_vec(inverse, 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)
+    if dot < 0: #To force it to be the shortest route
+        b = [-v for v in b]
+
+    theta = math.acos(dot)
+    sin_theta = math.sin(theta)
+    r1 = math.sin(1 - t) * theta / sin_theta
+    r2 = math.sin(t * theta) / sin_theta
+    a = [r1 * v for v in a]
+    b = [r2 * v for v in b]
+    return _vec.vec_normalize([v + w for v, w in zip(a, b)])

{yostlabs-2025.10.29.1 → yostlabs-2026.2.4}/src/yostlabs/math/vector.py

@@ -33,7 +33,8 @@ def vec_is_right_handed(order: str, negations: list[bool] = None):
     return right_handed
 
 def axis_to_unit_vector(axis: str):
-    axis = axis.lower()
+    if isinstance(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]
@@ -45,7 +46,7 @@ def parse_axis_string(axis: str):
     axis = axis.lower()
     order = [0, 1, 2]
     multipliers = [1, 1, 1]
-    if 'x' in axis: #Using XYZ notation
+    if any(c in axis for c in ['x', 'y', 'z']): #Using XYZ notation
         index = 0
         for c in axis:
             if c == '-':

{yostlabs-2025.10.29.1 → yostlabs-2026.2.4}/src/yostlabs/tss3/api.py

@@ -63,6 +63,9 @@ class ThreespaceCommand:
     BINARY_START_BYTE = 0xf7
     BINARY_START_BYTE_HEADER = 0xf9
 
+    BINARY_READ_SETTINGS_START_BYTE = 0xFA
+    BINARY_READ_SETTINGS_START_BYTE_HEADER = 0xFC
+
     def __init__(self, name: str, num: int, in_format: str, out_format: str, custom_func: Callable = None):
         self.info = ThreespaceCommandInfo(name, num, in_format, out_format)
         self.in_format = _3space_format_to_external(self.info.in_format)
@@ -82,7 +85,7 @@ class ThreespaceCommand:
 
     def send_command(self, com: ThreespaceOutputStream, *args, header_enabled = False):
         cmd = self.format_cmd(*args, header_enabled=header_enabled)
-        com.write(cmd)
+        com.write(cmd) 
 
     #Read the command result from an already read buffer. This will modify the given buffer to remove
     #that data as well
@@ -415,6 +418,7 @@ class StreamableCommands(Enum):
     GetBatteryPercent = 202
     GetBatteryStatus = 203
 
+    GetGpsActiveState = 214
     GetGpsCoord = 215
     GetGpsAltitude = 216
     GetGpsFixState = 217
@@ -917,6 +921,40 @@ class ThreespaceSensor:
             return list(response_dict.values())[0]
         return response_dict
 
+    """
+    WIP. Currently does not work with string values or have full validation.
+    """
+    def get_setting_binary(self, key: str, format: str, use_threespace_format=True):
+        if use_threespace_format:
+            format = _3space_format_to_external(format)
+        checksum = sum(ord(v) for v in key) % 256
+        #Format = StartByte + Key + Null Terminator of key + Checksum
+        cmd = struct.pack(f"<B{len(key)}sBB", ThreespaceCommand.BINARY_READ_SETTINGS_START_BYTE_HEADER, key.encode(), 0, checksum)
+        self.com.write(cmd)
+        try:
+            min_response_len = key.index(';')
+        except ValueError:
+            min_response_len = len(key)
+        if min_response_len < len(THREESPACE_GET_SETTINGS_ERROR_RESPONSE):
+            min_response_len = len(THREESPACE_GET_SETTINGS_ERROR_RESPONSE)
+        min_response_len += (1 + THREESPACE_BINARY_SETTINGS_ID_SIZE) #Null terminator and header size
+        if self.__await_get_settings_binary(min_response_len) != THREESPACE_AWAIT_COMMAND_FOUND:
+            raise RuntimeError(f"Failed to get binary setting: {key}")
+        self.com.read(THREESPACE_BINARY_SETTINGS_ID_SIZE) #Read pass the Header ID
+        
+        try:
+            echoed_key = self.com.read_until(b'\0')[:-1].decode()
+            if echoed_key != key.lower():
+                raise ValueError()
+        except:
+            raise ValueError()
+        result = struct.unpack(f"<{format}", self.com.read(struct.calcsize(format)))
+        self.com.read(2) #Remove the null terminator and the checksum
+        if len(result) == 1:
+            return result[0]
+        return result
+
+
     #-----------Base Settings Parsing----------------
 
     def __await_set_settings(self, timeout=2):
@@ -1004,6 +1042,31 @@ class ThreespaceSensor:
             self.misaligned = False
             return THREESPACE_AWAIT_COMMAND_FOUND
 
+    """
+    Incomplete, binary settings protocol is Work In Progress
+    """
+    def __await_get_settings_binary(self, min_resp_length: int, timeout=2, check_bootloader=False):
+        start_time = time.time()
+
+        while True:
+            remaining_time = timeout - (time.time() - start_time)
+            if remaining_time <= 0:
+                return THREESPACE_AWAIT_COMMAND_TIMEOUT
+            
+            if self.com.length < min_resp_length: continue
+            if check_bootloader and self.com.peek(2) == b'OK':
+                return THREESPACE_AWAIT_BOOTLOADER
+            
+            id = self.com.peek(THREESPACE_BINARY_SETTINGS_ID_SIZE)
+            if struct.unpack("<L", id)[0] != THREESPACE_BINARY_READ_SETTINGS_ID:
+                self.__internal_update(self.__try_peek_header())
+                continue
+            
+            #TODO: Check the rest of the message structure, not just the ID
+
+            self.misaligned = False
+            return THREESPACE_AWAIT_COMMAND_FOUND
+
 #---------------------------------BASE COMMAND PARSING--------------------------------------
     def __try_peek_header(self):
         """
@@ -1651,6 +1714,7 @@ class ThreespaceSensor:
     def getBatteryVoltage(self) -> ThreespaceCmdResult[float]: ...
     def getBatteryPercent(self) -> ThreespaceCmdResult[int]: ...
     def getBatteryStatus(self) -> ThreespaceCmdResult[int]: ...     
+    def getGpsActiveState(self) -> ThreespaceCmdResult[bool]: ...
     def getGpsCoord(self) -> ThreespaceCmdResult[list[float]]: ...
     def getGpsAltitude(self) -> ThreespaceCmdResult[float]: ...
     def getGpsFixState(self) -> ThreespaceCmdResult[int]: ...
@@ -1810,6 +1874,7 @@ _threespace_commands: list[ThreespaceCommand] = [
     ThreespaceCommand("getBatteryPercent", 202, "", "b"),
     ThreespaceCommand("getBatteryStatus", 203, "", "b"),
 
+    ThreespaceCommand("getGpsActiveState", 214, "", "b"),
     ThreespaceCommand("getGpsCoord", 215, "", "dd"),
     ThreespaceCommand("getGpsAltitude", 216, "", "f"),
     ThreespaceCommand("getGpsFixState", 217, "", "b"),

{yostlabs-2025.10.29.1 → yostlabs-2026.2.4}/src/yostlabs/tss3/consts.py

@@ -24,6 +24,8 @@ THREESPACE_OUTPUT_MODE_ASCII = 1
 THREESPACE_OUTPUT_MODE_BINARY = 2
 
 THREESPACE_GET_SETTINGS_ERROR_RESPONSE = "<KEY_ERROR>"
+THREESPACE_BINARY_SETTINGS_ID_SIZE = 4
+THREESPACE_BINARY_READ_SETTINGS_ID = 0xC695B5E1
 
 #This is not comprehensive, just enough to seperate keys from debug messages
 THREESPACE_SETTING_KEY_INVALID_CHARS = " :;"

yostlabs-2025.10.29.1/Examples/example_commands.py

@@ -1,26 +0,0 @@
-from yostlabs.tss3.api import ThreespaceSensor
-from yostlabs.communication.serial import ThreespaceSerialComClass
-
-#Create a sensor by auto detecting a ThreespaceSerialComClass
-sensor = ThreespaceSensor(ThreespaceSerialComClass)
-
-result = sensor.getPrimaryCorrectedAccelVec()
-
-#Read out the result
-accel_vec = result.data
-
-#The result is a dataclass that allows easily accessing individual parts of the data and multiple ways of interpreting it
-print("Result:")
-print(result) #The base ThreespaceCmdResult dataclass
-print(result.raw_binary) #The actual byte response from the sensor that made this result (header + data)
-print()
-print("Header:")
-print(result.header) #The ThreespaceHeader data class
-print(result.header.raw) #The individual components of the header as an array, similar to how the old API functioned
-print(result.header.raw_binary) #The actual byte representation of the header from the sensor
-print()
-print("Data:")
-print(f"{result.data=}")
-print(f"{result.raw_data=}")
-
-sensor.cleanup()

yostlabs-2025.10.29.1/src/yostlabs/math/quaternion.py

@@ -1,187 +0,0 @@
-import math
-import yostlabs.math.vector as _vec
-
-def quat_mul(a: list[float], b: list[float]):
-    out = [0, 0, 0, 0]
-    x = 0; y = 1; z = 2; w = 3
-    out[w] = a[w]*b[w] - a[x]*b[x] - a[y]*b[y] - a[z]*b[z]
-    out[x] = a[w]*b[x] + a[x]*b[w] + a[y]*b[z] - a[z]*b[y]
-    out[y] = a[w]*b[y] + a[y]*b[w] + a[z]*b[x] - a[x]*b[z]
-    out[z] = a[w]*b[z] + a[z]*b[w] + a[x]*b[y] - a[y]*b[x]
-    return out
-
-#Rotates quaternion b by quaternion a, it does not combine them
-def quat_rotate(a: list[float], b: list[float]):
-    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]):
-    inv = quat_inverse(quat)
-    tmp = [vec[0], vec[1], vec[2], 0]
-    halfway = quat_mul(quat, tmp)
-    final = quat_mul(halfway, inv)
-    return [final[0], final[1], final[2]]
-
-def angles_to_quaternion(angles: list[float], order: str, degrees=True):
-    quat = [0, 0, 0, 1]
-    for i in range(len(angles)):
-        axis = order[i]
-        angle = angles[i]
-        if degrees:
-            angle = math.radians(angle)
-        unit_vec = _vec.axis_to_unit_vector(axis)
-        w = math.cos(angle / 2)
-        imaginary = math.sin(angle / 2)
-        unit_vec = [v * imaginary for v in unit_vec]
-        angle_quat = [*unit_vec, w]
-        quat = quat_mul(quat, angle_quat)
-    return quat
-
-def quat_from_axis_angle(axis: list[float], angle: float):
-    imaginary = math.sin(angle / 2)
-    quat = [imaginary * v for v in axis]
-    quat.append(math.cos(angle / 2))
-    return quat
-
-#There are multiple valid quats that can be returned by this. The intention of this function
-#is to be able to rotate an arrow by the quat such that it points the correct direction. The rotation
-#of that arrow along its axis may differ though
-def quat_from_one_vector(vec: list[float]):
-    vec = _vec.vec_normalize(vec)
-    perpendicular = _vec.vec_normalize(_vec.vec_cross([0, 0, 1], vec))
-    angle = math.acos(_vec.vec_dot([0, 0, 1], vec))
-    return quat_from_axis_angle(perpendicular, angle)
-
-def quat_from_two_vectors(forward: list[float], down: list[float]):
-    """
-    This function requires two orthogonal vectors to work
-    """
-    forward_reference = [0, 0, 1]
-    down_reference = [0, -1, 0]
-
-    forward = _vec.vec_normalize(forward)
-    down = _vec.vec_normalize(down)
-
-    #Create the first rotation to align the forward axis
-    axis_of_rotation = _vec.vec_cross(forward_reference, forward)
-    axis_of_rotation = _vec.vec_normalize(axis_of_rotation)
-    if not any(abs(v) > 0 for v in axis_of_rotation):
-        axis_of_rotation = down_reference #This is just a direct 180 degree rotation around any orthogonal axis, so just use the down_ref
-    dot = min(1, max(-1, _vec.vec_dot(forward_reference, forward)))
-    angle1 = math.acos(dot)
-    imaginary = math.sin(angle1/2)
-    quat = [v * imaginary for v in axis_of_rotation] #XYZ
-    quat.append(math.cos(angle1/2)) #W
-
-    #Update the reference to figure out where it is after the turn
-    down_reference = quat_rotate_vec(quat, down_reference)
-
-    #find the rotation to make the remaining reference align with its given value
-    axis_of_rotation = _vec.vec_cross(down_reference, down)
-    axis_of_rotation = _vec.vec_normalize(axis_of_rotation)
-    if not any(abs(v) > 0 for v in axis_of_rotation):
-        axis_of_rotation = forward #Rotate along the final forward vector until up is aligned
-    dot = min(1, max(-1, _vec.vec_dot(down_reference, down)))
-    angle2 = math.acos(dot)
-    imaginary = math.sin(angle2/2)
-    rotation = [v * imaginary for v in axis_of_rotation] #XYZ
-    rotation.append(math.cos(angle2/2)) #W
-
-    quat = quat_mul(rotation, quat)
-    return quat
-
-def quaternion_to_3x3_rotation_matrix(quat):
-    """
-    Convert a quaternion in form x, y, z, w to a rotation matrix
-    """
-    x, y, z, w = quat
-
-    fTx = 2.0 * x
-    fTy = 2.0 * y
-    fTz = 2.0 * z
-    fTwx = fTx * w
-    fTwy = fTy * w
-    fTwz = fTz * w
-    fTxx = fTx * x
-    fTxy = fTy * x
-    fTxz = fTz * x
-    fTyy = fTy * y
-    fTyz = fTz * y
-    fTzz = fTz * z
-
-    out = [0] * 9
-    out[0] = 1.0-(fTyy+fTzz)
-    out[1] = fTxy-fTwz
-    out[2] = fTxz+fTwy
-    out[3] = fTxy+fTwz
-    out[4] = 1.0-(fTxx+fTzz)
-    out[5] = fTyz-fTwx
-    out[6] = fTxz-fTwy
-    out[7] = fTyz + fTwx
-    out[8] = 1.0 - (fTxx+fTyy)
-
-    return [
-        [out[0], out[1], out[2]],
-        [out[3], out[4], out[5]],
-        [out[6], out[7], out[8]],
-    ]
-
-def quaternion_global_to_local(quat, vec):
-    inverse = quat_inverse(quat)
-    return quat_rotate_vec(inverse, 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)
-    if dot < 0: #To force it to be the shortest route
-        b = [-v for v in b]
-
-    theta = math.acos(dot)
-    sin_theta = math.sin(theta)
-    r1 = math.sin(1 - t) * theta / sin_theta
-    r2 = math.sin(t * theta) / sin_theta
-    a = [r1 * v for v in a]
-    b = [r2 * v for v in b]
-    return _vec.vec_normalize([v + w for v, w in zip(a, b)])