yostlabs 2025.10.29.2__py3-none-any.whl → 2026.2.4__py3-none-any.whl

yostlabs/math/quaternion.py

@@ -30,7 +30,7 @@ def quat_rotate_vec(quat: list[float], vec: list[float]):
     final = quat_mul(halfway, inv)
     return [final[0], final[1], final[2]]
 
-def angles_to_quaternion(angles: list[float], order: str, degrees=True):
+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]
@@ -42,9 +42,18 @@ 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]
-        quat = quat_mul(quat, angle_quat)
+        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]
@@ -134,6 +143,164 @@ def quaternion_to_3x3_rotation_matrix(quat):
         [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)

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/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/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.2.dist-info → yostlabs-2026.2.4.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: yostlabs
-Version: 2025.10.29.2
+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.2.dist-info → yostlabs-2026.2.4.dist-info}/RECORD

@@ -6,18 +6,18 @@ yostlabs/communication/bluetooth.py,sha256=GuqCRauGxOt0smh9qLA1NCfs7s5onZJjngXZs
 yostlabs/communication/serial.py,sha256=Ex41l3ePXWrZDF8ZGxWoQs1tR0yfwZaSdG7a597zBsA,6505
 yostlabs/communication/socket.py,sha256=mZw_9-5teIPUMxLXQh7kVLu6CJxCNFC7a9CnhSAI2Og,3822
 yostlabs/math/__init__.py,sha256=JFzsPQ4AbsX1AH1brBpn1c_Pa_ItF43__D3mlPvA2a4,34
-yostlabs/math/quaternion.py,sha256=CUIh4RHUcoSt2RUyPPh7Vw-xJfcUP2Z_mAkA0EHwyn4,6865
-yostlabs/math/vector.py,sha256=9vfVFSahHa0ZZRZ_SgAU5ucVplt7J-fHQ0s8ymOanj4,2725
+yostlabs/math/quaternion.py,sha256=E61BcyN6vn6UdCqwQEpqs-YHElOj5GXuLH981xZmNEc,13765
+yostlabs/math/vector.py,sha256=s-bNK9Qum8wgVKDfRUJ_Hz5rs7Pqbg9-wFb1XUAOe04,2788
 yostlabs/tss3/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
-yostlabs/tss3/api.py,sha256=WRMuf-YdQvn5_RUnYmI4n9dATZ79xkzGAW_HP7A6Ijk,86317
-yostlabs/tss3/consts.py,sha256=6bAe2E3GY61hK3Z0DqAQmDJLvXUtS0kHkP6QEOBZaDI,2503
+yostlabs/tss3/api.py,sha256=mSBWjCrT-Re5xw4QR9QJjHxSYNTzy6nUEd7bAwVBfw8,89257
+yostlabs/tss3/consts.py,sha256=HU72GAEb3aAi2be3Fny1tOwHPwhqomSAWgpVGXxMHIQ,2592
 yostlabs/tss3/eepts.py,sha256=7A7sCyOfDiJgw5Y9pGneg-5YgNvcfKtqeS9FoVWfJO8,9540
 yostlabs/tss3/utils/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
 yostlabs/tss3/utils/calibration.py,sha256=J7RnY23MUUo4lmfapHavUqrTArFo4PxGKw68sL4DfOM,14070
 yostlabs/tss3/utils/parser.py,sha256=ij_kQpB3EukOq3O8wQTYhkqBP-OneiHMUcsHLuL6m-8,11347
 yostlabs/tss3/utils/streaming.py,sha256=G2OjSIL9zub0EbkgDGDWaqSXoRY6MJzMD4mazWOCUOA,22419
 yostlabs/tss3/utils/version.py,sha256=NT2H9l-oIRCYhV_yjf5UjkadoJQ0IN4eLl8y__pyTPc,3001
-yostlabs-2025.10.29.2.dist-info/METADATA,sha256=PYSyY8PEl5MCKO8oifmd2GHPjyzpNtwn8OW5z9gyD8Y,2778
-yostlabs-2025.10.29.2.dist-info/WHEEL,sha256=qtCwoSJWgHk21S1Kb4ihdzI2rlJ1ZKaIurTj_ngOhyQ,87
-yostlabs-2025.10.29.2.dist-info/licenses/LICENSE,sha256=PtF8EXRlVhm1_ve52_3GHixSPwMn0tGajFxF3xKS-j0,1090
-yostlabs-2025.10.29.2.dist-info/RECORD,,
+yostlabs-2026.2.4.dist-info/METADATA,sha256=ReMVDqVO5CzGLXI-z1qzRRNk_Ez8lMHv7KTV3s-ZCac,2774
+yostlabs-2026.2.4.dist-info/WHEEL,sha256=WLgqFyCfm_KASv4WHyYy0P3pM_m7J5L9k2skdKLirC8,87
+yostlabs-2026.2.4.dist-info/licenses/LICENSE,sha256=PtF8EXRlVhm1_ve52_3GHixSPwMn0tGajFxF3xKS-j0,1090
+yostlabs-2026.2.4.dist-info/RECORD,,

{yostlabs-2025.10.29.2.dist-info → yostlabs-2026.2.4.dist-info}/WHEEL

@@ -1,4 +1,4 @@
 Wheel-Version: 1.0
-Generator: hatchling 1.27.0
+Generator: hatchling 1.28.0
 Root-Is-Purelib: true
 Tag: py3-none-any