yostlabs 2025.9.18__py3-none-any.whl → 2025.10.24__py3-none-any.whl

yostlabs/communication/ble.py

@@ -7,6 +7,7 @@ from bleak.backends.device import BLEDevice
 from bleak.backends.scanner import AdvertisementData
 from bleak.backends.characteristic import BleakGATTCharacteristic
 from bleak.exc import BleakDeviceNotFoundError
+from dataclasses import dataclass
 
 #Services
 NORDIC_UART_SERVICE_UUID = "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
@@ -23,6 +24,12 @@ 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"
 
+@dataclass
+class ThreespaceBLENordicUartProfile:
+    SERVICE_UUID: str
+    RX_UUID: str
+    TX_UUID: str
+
 class TssBLENoConnectionError(Exception): ...
 
 def ylBleEventLoopThread(loop: asyncio.AbstractEventLoop):
@@ -36,6 +43,9 @@ class ThreespaceBLEComClass(ThreespaceComClass):
     EVENT_LOOP = None
     EVENT_LOOP_THREAD = None
 
+    DEFAULT_PROFILE = ThreespaceBLENordicUartProfile(NORDIC_UART_SERVICE_UUID, NORDIC_UART_RX_UUID, NORDIC_UART_TX_UUID)
+    REGISTERED_PROFILES: list[ThreespaceBLENordicUartProfile] = [DEFAULT_PROFILE]
+
     @classmethod
     def __lazy_event_loop_init(cls):
         if cls.EVENT_LOOP is None:
@@ -43,7 +53,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
             cls.EVENT_LOOP_THREAD = threading.Thread(target=ylBleEventLoopThread, args=(cls.EVENT_LOOP,), daemon=True)
             cls.EVENT_LOOP_THREAD.start()
 
-    def __init__(self, ble: BleakClient | BLEDevice | str, discover_name: bool = True, discovery_timeout=5, error_on_disconnect=True):
+    def __init__(self, ble: BleakClient | BLEDevice | str, discover_name: bool = True, discovery_timeout=5, error_on_disconnect=True, adv: AdvertisementData = None):
         """
         Parameters
         ----------
@@ -54,6 +64,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         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.__lazy_event_loop_init()
+        self.adv = adv
         bleak_options = { "timeout": discovery_timeout, "disconnected_callback": self.__on_disconnect }
         if isinstance(ble, BleakClient):    #Actual client
             self.client = ble
@@ -76,6 +87,19 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         else:
             raise TypeError("Invalid type for creating a ThreespaceBLEComClass:", type(ble), ble)
 
+        #Select the profile
+        self.profile = None
+        if self.adv is not None and len(self.adv.service_uuids) > 0:
+            for service_uuid in self.adv.service_uuids:
+                self.profile = self.get_profile(service_uuid)
+                if self.profile is not None:
+                    break
+            if self.profile is None:
+                self.profile = ThreespaceBLEComClass.DEFAULT_PROFILE
+                raise Exception(f"Unknown Service UUIDS: {self.adv.service_uuids}")
+        else:
+            self.profile = ThreespaceBLEComClass.DEFAULT_PROFILE
+
         self.__timeout = self.DEFAULT_TIMEOUT
 
         self.buffer = bytearray()
@@ -101,7 +125,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
     async def __async_open(self):
         self.data_read_event = asyncio.Event()
         await self.client.connect()
-        await self.client.start_notify(NORDIC_UART_TX_UUID, self.__on_data_received)
+        await self.client.start_notify(self.profile.TX_UUID, self.__on_data_received)
 
     def open(self):
         #If trying to open while already open, this infinitely loops
@@ -151,7 +175,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         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
             asyncio.run_coroutine_threadsafe(
-                self.client.write_gatt_char(NORDIC_UART_RX_UUID, bytes[start_index:end_index], response=False),
+                self.client.write_gatt_char(self.profile.RX_UUID, bytes[start_index:end_index], response=False),
                 self.EVENT_LOOP).result()
             start_index = end_index
     
@@ -244,8 +268,16 @@ class ThreespaceBLEComClass(ThreespaceComClass):
     def address(self) -> str:
         return self.client.address    
 
-    SCANNER = None
+    @classmethod
+    def get_profile(cls, service_uuid: str):
+        for profile in cls.REGISTERED_PROFILES:
+            if profile.SERVICE_UUID == service_uuid:
+                return profile
+        return None
+
+    SCANNER: BleakScanner = None
     SCANNER_LOCK = None
+    SCANNER_RUNNING = False
 
     SCANNER_CONTINOUS = False   #Controls if scanning will continously run
     SCANNER_TIMEOUT = 5         #Controls the scanners timeout
@@ -255,23 +287,94 @@ class ThreespaceBLEComClass(ThreespaceComClass):
     #Format: Address - dict = { device: ..., adv: ..., last_found: ... }
     discovered_devices: dict[str,dict] = {}
 
+    @classmethod
+    def set_profiles(cls, profiles: list[ThreespaceBLENordicUartProfile]):
+        cls.REGISTERED_PROFILES = profiles
+        if cls.SCANNER is not None:
+            asyncio.run_coroutine_threadsafe(cls.create_scanner(), cls.EVENT_LOOP).result()
+            cls.__remove_unused_profiles()
+
+    @classmethod
+    def register_profile(cls, profile: ThreespaceBLENordicUartProfile):        
+        if any(v.SERVICE_UUID == profile.SERVICE_UUID for v in cls.REGISTERED_PROFILES): return
+        cls.REGISTERED_PROFILES.append(profile)
+        if cls.SCANNER is not None:
+            asyncio.run_coroutine_threadsafe(cls.create_scanner(), cls.EVENT_LOOP).result()
+    
+    @classmethod
+    def unregister_profile(cls, service_uuid: str|ThreespaceBLENordicUartProfile):
+        if isinstance(service_uuid, ThreespaceBLENordicUartProfile):
+            service_uuid = service_uuid.SERVICE_UUID
+        index = None
+        for i in range(len(cls.REGISTERED_PROFILES)):
+            if cls.REGISTERED_PROFILES[i].SERVICE_UUID == service_uuid:
+                index = i
+                break
+        del cls.REGISTERED_PROFILES[index]
+        if cls.SCANNER is not None:
+            asyncio.run_coroutine_threadsafe(cls.create_scanner(), cls.EVENT_LOOP).result()
+            cls.__remove_unused_profiles()
+
+    @classmethod
+    def __remove_unused_profiles(cls):
+        if cls.SCANNER is None: return
+        to_remove = []
+        valid_service_uuids = [v.SERVICE_UUID for v in cls.REGISTERED_PROFILES]
+        with cls.SCANNER_LOCK:
+            for address in cls.discovered_devices:
+                adv: AdvertisementData = cls.discovered_devices[address]["adv"]
+                if not any(uuid in valid_service_uuids for uuid in adv.service_uuids):
+                    to_remove.append(address)
+            for address in to_remove:
+                del cls.discovered_devices[address]
+
+    #Scanner should be created inside of the Async Context that will use it
+    @classmethod
+    async def create_scanner(cls):
+        uuids = [v.SERVICE_UUID for v in cls.REGISTERED_PROFILES]
+        restart_scanner = cls.SCANNER_RUNNING
+        with cls.SCANNER_LOCK:
+            if restart_scanner:
+                await cls.__async_stop_scanner()
+            cls.SCANNER = BleakScanner(detection_callback=cls.__detection_callback, service_uuids=uuids)
+            if restart_scanner:
+                await cls.__async_start_scanner()
+
     @classmethod
     def __lazy_init_scanner(cls):
         cls.__lazy_event_loop_init()
         if cls.SCANNER is None:
             cls.SCANNER_LOCK = threading.Lock()
-            #Scanner should be created inside of the Async Context that will use it
-            async def create_scanner():
-                cls.SCANNER = BleakScanner(detection_callback=cls.__detection_callback, service_uuids=[NORDIC_UART_SERVICE_UUID])
-            asyncio.run_coroutine_threadsafe(create_scanner(), cls.EVENT_LOOP).result()
-
-            
+            asyncio.run_coroutine_threadsafe(cls.create_scanner(), cls.EVENT_LOOP).result()
 
     @classmethod
     def __detection_callback(cls, device: BLEDevice, adv: AdvertisementData):
         with cls.SCANNER_LOCK:
             cls.discovered_devices[device.address] = {"device": device, "adv": adv, "last_found": time.time()}
     
+    @classmethod
+    async def __async_start_scanner(cls):
+        if cls.SCANNER_RUNNING: return
+        await cls.SCANNER.start()
+        cls.SCANNER_RUNNING = True
+
+    @classmethod
+    async def __async_stop_scanner(cls):
+        if not cls.SCANNER_RUNNING: return
+        await cls.SCANNER.stop()
+        cls.SCANNER_RUNNING = False        
+
+    @classmethod
+    def __start_scanner(cls):
+        if cls.SCANNER_RUNNING: return
+        asyncio.run_coroutine_threadsafe(cls.__async_start_scanner(), cls.EVENT_LOOP).result()
+    
+    @classmethod
+    def __stop_scanner(cls):
+        if not cls.SCANNER_RUNNING: return
+        asyncio.run_coroutine_threadsafe(cls.__async_stop_scanner(), cls.EVENT_LOOP).result()
+        cls.__stop_scanner()
+
     @classmethod
     def set_scanner_continous(cls, continous: bool):
         """
@@ -285,9 +388,9 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         cls.__lazy_init_scanner()
         cls.SCANNER_CONTINOUS = continous
         if continous: 
-            asyncio.run_coroutine_threadsafe(cls.SCANNER.start(), cls.EVENT_LOOP).result()
+            cls.__start_scanner()
         else: 
-            asyncio.run_coroutine_threadsafe(cls.SCANNER.stop(), cls.EVENT_LOOP).result()
+            cls.__stop_scanner()
 
     @classmethod
     def update_nearby_devices(cls):
@@ -338,4 +441,4 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         cls.update_nearby_devices()
         with cls.SCANNER_LOCK:
             for device_info in cls.discovered_devices.values():
-                yield(ThreespaceBLEComClass(device_info["device"]))
+                yield(ThreespaceBLEComClass(device_info["device"], adv=device_info["adv"]))

yostlabs/communication/serial.py

@@ -1,16 +1,25 @@
 from yostlabs.communication.base import *
 import serial
 import serial.tools.list_ports
+from serial.tools.list_ports_common import ListPortInfo
 import time
 
-
 class ThreespaceSerialComClass(ThreespaceComClass):
-
     PID_V3_MASK = 0x3000
-    PID_BOOTLOADER = 0x1000
 
     VID = 0x2476
 
+    PID_BOOTLOADER = 0x1000
+    PID_EMBED = 0x3040
+    PID_DL = 0x3050
+
+    PID_TO_STR_DICT = {
+        PID_EMBED: "EM",
+        PID_DL: "DL",
+        PID_BOOTLOADER: "BOOT"
+    }
+
+
     DEFAULT_BAUDRATE = 115200
     DEFAULT_TIMEOUT = 2
 
@@ -18,12 +27,11 @@ class ThreespaceSerialComClass(ThreespaceComClass):
         if isinstance(ser, serial.Serial):
             self.ser = ser
         elif isinstance(ser, str):
-            self.ser = serial.Serial(ser, baudrate=ThreespaceSerialComClass.DEFAULT_BAUDRATE, timeout=ThreespaceSerialComClass.DEFAULT_TIMEOUT)
+            self.ser = serial.Serial(None, baudrate=ThreespaceSerialComClass.DEFAULT_BAUDRATE, timeout=ThreespaceSerialComClass.DEFAULT_TIMEOUT)
+            self.ser.port = ser
         else:
             raise TypeError("Invalid type for creating a ThreespaceSerialComClass:", type(ser), ser)
 
-        self.ser = ser
-
         self.peek_buffer = bytearray()
         self.peek_length = 0
 
@@ -120,6 +128,22 @@ class ThreespaceSerialComClass(ThreespaceComClass):
     @property
     def name(self) -> str:
         return self.ser.port
+    
+    @property
+    def suffix(self) -> str:
+        return self.pid_to_str(self.get_port_info().pid) 
+    
+    def get_port_info(self):
+        ports = serial.tools.list_ports.comports()
+        for port in ports:
+            if port.device == self.ser.port:
+                return port
+        return None
+
+    @staticmethod
+    def is_threespace_port(port: ListPortInfo):
+        cls = ThreespaceSerialComClass
+        return port.vid == cls.VID and (port.pid & cls.PID_V3_MASK == cls.PID_V3_MASK or port.pid == cls.PID_BOOTLOADER)
 
     #This is not part of the ThreespaceComClass interface, but is useful as a utility for those directly using the ThreespaceSerialComClass
     @staticmethod 
@@ -127,7 +151,7 @@ class ThreespaceSerialComClass(ThreespaceComClass):
         cls = ThreespaceSerialComClass
         ports = serial.tools.list_ports.comports()
         for port in ports:
-            if port.vid == cls.VID and (port.pid & cls.PID_V3_MASK == cls.PID_V3_MASK or port.pid == cls.PID_BOOTLOADER): 
+            if cls.is_threespace_port(port):
                 yield port
 
     @staticmethod
@@ -139,7 +163,12 @@ class ThreespaceSerialComClass(ThreespaceComClass):
         cls = ThreespaceSerialComClass
         ports = serial.tools.list_ports.comports()
         for port in ports:
-            if port.vid == cls.VID and (port.pid & cls.PID_V3_MASK == cls.PID_V3_MASK or port.pid == cls.PID_BOOTLOADER): 
+            if cls.is_threespace_port(port):
                 ser = serial.Serial(None, baudrate=default_baudrate, timeout=default_timeout) #By setting port as None, can create an object without immediately opening the port
                 ser.port = port.device #Now assign the port, allowing the serial object to exist without being opened yet
-                yield ThreespaceSerialComClass(ser)
+                yield ThreespaceSerialComClass(ser)
+
+    @classmethod
+    def pid_to_str(cls, pid):
+        if pid not in cls.PID_TO_STR_DICT: return "Unknown"
+        return cls.PID_TO_STR_DICT[pid]

yostlabs/math/quaternion.py

@@ -51,6 +51,15 @@ def quat_from_axis_angle(axis: list[float], angle: float):
     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

yostlabs/tss3/api.py

@@ -421,6 +421,8 @@ class StreamableCommands(Enum):
     GetGpsHdop = 218
     GetGpsSattelites = 219
 
+    GetLedColor = 238
+
     GetButtonState = 250
 
 THREESPACE_AWAIT_COMMAND_FOUND = 0
@@ -1674,7 +1676,8 @@ class ThreespaceSensor:
     def getStreamingLabel(self, cmd_num: int) -> ThreespaceCmdResult[str]: ...
     def setCursor(self, cursor_index: int) -> ThreespaceCmdResult[None]: ...
     def getLastLogCursorInfo(self) -> ThreespaceCmdResult[tuple[int,str]]: ...
-    def pauseLogStreaming(self, pause: bool) -> ThreespaceCmdResult[None]: ...           
+    def pauseLogStreaming(self, pause: bool) -> ThreespaceCmdResult[None]: ...  
+    def getLedColor(self) -> ThreespaceCmdResult[list[float]]: ...           
 
 THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM = 84
 THREESPACE_START_STREAMING_COMMAND_NUM = 85
@@ -1817,6 +1820,8 @@ _threespace_commands: list[ThreespaceCommand] = [
     ThreespaceCommand("softwareReset", THREESPACE_SOFTWARE_RESET_COMMAND_NUM, "", "", custom_func=ThreespaceSensor._ThreespaceSensor__softwareReset),
     ThreespaceCommand("enterBootloader", THREESPACE_ENTER_BOOTLOADER_COMMAND_NUM, "", "", custom_func=ThreespaceSensor._ThreespaceSensor__enterBootloader),
 
+    ThreespaceCommand("getLedColor", 238, "", "fff"),
+
     ThreespaceCommand("getButtonState", 250, "", "b"),
 ]
 

yostlabs/tss3/consts.py

@@ -61,4 +61,6 @@ THREESPACE_SN_FAMILY_TO_NAME = {
     0x15 : THREESPACE_FAMILY_BLUETOOTH,
     0x16 : THREESPACE_FAMILY_DATA_LOGGER,
     0x17 : THREESPACE_FAMILY_MICRO_USB
-}
+}
+
+THREESPACE_ERR_COMMIT_FS_LOCKED=8

yostlabs/tss3/utils/calibration.py

@@ -150,4 +150,214 @@ class ThreespaceGradientDescentCalibration:
             print(f"Final Rating: {best_rating}")
             print(f"Final Params: {best_params}")
 
-        return best_params
+        return best_params
+    
+def fibonacci_sphere(samples=1000):
+    points = []
+    phi = np.pi * (3. - np.sqrt(5.))  # golden angle
+
+    for i in range(samples):
+        y = 1 - (i / float(samples - 1)) * 2  # y goes from 1 to -1
+        radius = np.sqrt(1 - y * y)
+        theta = phi * i
+        x = np.cos(theta) * radius
+        z = np.sin(theta) * radius
+        points.append((x, y, z))
+
+    return np.array(points)
+
+class ThreespaceSphereCalibration:
+
+    def __init__(self, max_comparison_points=500):
+        self.buffer = 0.1
+        self.test_points = fibonacci_sphere(samples=max_comparison_points)
+        self.clear()
+
+    def process_point(self, raw_mag: list[float]):
+        if len(self.samples) == 0:
+            self.samples = np.array([raw_mag])
+            return True
+        
+        raw_mag = np.array(raw_mag, dtype=np.float64)
+        new_len = np.linalg.norm(raw_mag)
+
+        avg_len = np.linalg.norm(self.samples, axis=1)
+        avg_len = (avg_len + new_len) / 2 * self.buffer
+
+        dist = np.linalg.norm(self.samples - raw_mag, axis=1)
+        if np.any(dist < avg_len):
+            return False
+        
+        self.samples = np.concatenate((self.samples, [raw_mag]))
+        self.__update_density(raw_mag / new_len)
+        return True
+    
+    def __update_density(self, normalized_point: np.ndarray):
+        #First check to see if the new point is the closest point for any of the previous points
+        dots = np.sum(self.test_points * normalized_point, axis=1)
+        self.closest_dot = np.maximum(self.closest_dot, dots)
+        self.largest_delta_index = np.argmin(self.closest_dot)
+        self.largest_delta = np.rad2deg(np.acos(self.closest_dot[self.largest_delta_index]))
+
+    def clear(self):
+        self.samples = np.array([])
+        self.closest_dot = np.array([-1] * len(self.test_points))
+        self.largest_delta = 180
+        self.largest_delta_index = 0
+
+    @property
+    def sparsest_vector(self):
+        return self.test_points[self.largest_delta_index]
+
+    def calculate(self):
+        """
+        Returns matrix and bias
+        """
+        comp_A, comp_b, comp_d = ThreespaceSphereCalibration.alternating_least_squares(np.array(self.samples))
+        comp_A, comp_b = ThreespaceSphereCalibration.make_calibration_params(comp_A, comp_b, comp_d, 1)
+
+        comp_scale: float = comp_A[0][0] + comp_A[1][1] + comp_A[2][2]
+        comp_scale /= 3
+
+        comp_A /= comp_scale
+
+        return comp_A.flatten().tolist(), comp_b.tolist()
+
+    @staticmethod
+    def alternating_least_squares(data: np.ndarray[np.ndarray]) -> tuple[np.ndarray, np.ndarray, float]:
+        n = 3
+        m = len(data)
+
+        #Step 1
+        noise_variance = 0.1
+        sigma2 = noise_variance ** 2
+        rotini = data.T
+        rotini2 = rotini ** 2
+
+        T = np.zeros((5,n,m))
+        T[0,:,:] = 1
+        T[1,:,:] = rotini
+        T[2,:,:] = rotini2 - sigma2
+        T[3,:,:] = rotini2 * rotini - 3 * rotini * sigma2
+        T[4,:,:] = rotini2 * rotini2 - 6 * rotini2 * sigma2 + 3 * sigma2 * sigma2
+
+        #Step 2
+        one = np.ones((n+1,))
+        i = np.arange(1, n+1)
+        i = np.append(i, 0)
+
+        m1 = np.outer(i.T, one)
+        m2 = np.outer(one.T, i)
+
+        M = np.array([ThreespaceSphereCalibration.vec_s(m1), ThreespaceSphereCalibration.vec_s(m2)])
+
+        #Step 3
+        nb = int((n + 1) * n / 2 + n + 1)
+        R = np.zeros((nb,nb,n), dtype=np.int32)
+        
+        for p in range(nb):
+            for q in range(p, nb):
+                for i in range(1, n+1):
+                    R[p,q,i-1] = int(M[0,p] == i) + int(M[1,p] == i) + int(M[0,q] == i) + int(M[1,q] == i)
+
+        #Step 4
+        nals = np.zeros((nb, nb))
+        for p in range(nb):
+            for q in range(p, nb):
+                sum = 0
+                for l in range(m):
+                    prod = 1
+                    for i in range(n):
+                        prod *= T[R[p,q,i],i,l]
+                    sum += prod
+                nals[p,q] = sum
+
+        #Step 5
+        D2 = [i * (i + 1) / 2 for i in range(1, n+1)]
+        D = [d for d in range(1, (n+1) * n // 2 + 1) if not d in D2]
+
+        #Step 6
+        menorah_als = np.zeros((nb, nb))
+        
+        for p in range(nb):
+            for q in range(p, nb):
+                coeff = 2
+                if p + 1 in D and q + 1 in D:
+                    coeff = 4
+                elif not p + 1 in D and not q + 1 in D:
+                    coeff = 1
+                menorah_als[p,q] = coeff * nals[p,q]
+        
+        # Fill the lower triangle with the upper triangle values
+        i_lower, j_lower = np.tril_indices(nb, k=-1) 
+        menorah_als[i_lower, j_lower] = menorah_als[j_lower, i_lower]
+
+        #Step 7
+        eigenmat = menorah_als
+
+        #It is unclear if this is correct, there are differences in sign and positions
+        eigenvalues, eigenvectors = np.linalg.eig(eigenmat)
+        eigenvectors = eigenvectors.T
+
+        #Looks like this section gets the eigen vector with the largest eigen value?
+        combinedmatr = [[abs(eigenvalues[i]), eigenvectors[i]] for i in range(len(eigenvalues))]
+        combinedmatr.sort(key=lambda a: a[0], reverse=True)
+
+        bals = combinedmatr[-1][1]
+
+        #Step 8 : ensure normalized
+        bals = bals / np.linalg.norm(bals)
+
+        #Step 9:
+        triangle = n*(n+1)//2
+        A = ThreespaceSphereCalibration.inv_vec_s(bals[:triangle])
+        b = bals[triangle:nb-1]
+        d = bals[-1]
+
+        return A, b, d
+
+    @staticmethod
+    def make_calibration_params(Q: np.ndarray, u: np.ndarray, k: float, H_m: float) -> tuple[np.ndarray,np.ndarray]:
+        pa = np.linalg.inv(Q)
+        pb = u.T
+        b = np.dot(pa, pb) * 0.5
+
+        eigenvalues, V = np.linalg.eig(Q)
+        D = np.diag(eigenvalues)
+
+        vu_prod = np.dot(V.T, u.T)
+        p1a = np.dot(vu_prod.T, np.linalg.inv(D))
+        p1b = np.dot(p1a, vu_prod)
+        p1 = p1b - (4 * k)
+
+        alpha = 4 * (H_m ** 2) / p1
+
+        aD = np.diag(abs(alpha * eigenvalues) ** 0.5)
+
+        A = np.dot(np.dot(V, aD), V.T)
+        
+        return A, b
+
+    @staticmethod
+    def vec_s(matrix: np.ndarray):
+        rows, cols = np.tril_indices(matrix.shape[0])
+        return matrix[rows, cols]
+
+    @staticmethod
+    def inv_vec_s(vec: np.ndarray):
+        #Its unclear if this function works as intended. But this is how the suite does it.
+        size = int((-1 + (1 + 8 * len(vec)) ** 0.5) / 2)
+        matr = np.zeros((size,size))
+        base = 0
+        for i in range(size):
+            for j in range(i):
+                matr[i,j] = vec[base + j]
+                matr[j,i] = vec[base + j]
+            matr[i,i] = vec[base + i]
+            base += i + 1
+        
+        return matr
+
+    @property
+    def num_points(self):
+        return len(self.samples)

{yostlabs-2025.9.18.dist-info → yostlabs-2025.10.24.dist-info}/METADATA

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

@@ -1,21 +1,21 @@
 yostlabs/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
 yostlabs/communication/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
 yostlabs/communication/base.py,sha256=ahAIQndfo9ifX6Lf2NeEaHpIhRJ_uBv6jv9P7N3Rbhg,2884
-yostlabs/communication/ble.py,sha256=UwbUDEp0lU6CQv-BWmqB3mzEUJrwwRH78eeJUmFO9c4,15498
-yostlabs/communication/serial.py,sha256=j7SksPhd2mCvcMIGVvPcAAhYOE29K6uGLwZCwD-b21E,5685
+yostlabs/communication/ble.py,sha256=4udePfI1phCGvMdLsgq6Qh9A1GpGYHT0G5uCFtyyreM,19671
+yostlabs/communication/serial.py,sha256=gW-kP7iAE9A4CPa11uW5tcH9exdGAPlz8x3bC8v4_vM,6461
 yostlabs/math/__init__.py,sha256=JFzsPQ4AbsX1AH1brBpn1c_Pa_ItF43__D3mlPvA2a4,34
-yostlabs/math/quaternion.py,sha256=vQQmT5T0FXAOrZ27cj007Gb_sfEhs7h0Sz6nYxNc5hQ,6357
+yostlabs/math/quaternion.py,sha256=CUIh4RHUcoSt2RUyPPh7Vw-xJfcUP2Z_mAkA0EHwyn4,6865
 yostlabs/math/vector.py,sha256=9vfVFSahHa0ZZRZ_SgAU5ucVplt7J-fHQ0s8ymOanj4,2725
 yostlabs/tss3/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
-yostlabs/tss3/api.py,sha256=h8K1VsWZutTE-Vu5k2xJbz-OBwKxR-IKnFOJ5qBW0xs,86165
-yostlabs/tss3/consts.py,sha256=RwhqmKIXRGpRdusss3q17ukCuRS96ZsvBl6y0mjF0b4,2404
+yostlabs/tss3/api.py,sha256=WRMuf-YdQvn5_RUnYmI4n9dATZ79xkzGAW_HP7A6Ijk,86317
+yostlabs/tss3/consts.py,sha256=hjbPWoPFsyncyePhmkY0I68XIzsSEo8Fq6Vt0jZ1h8s,2441
 yostlabs/tss3/eepts.py,sha256=7A7sCyOfDiJgw5Y9pGneg-5YgNvcfKtqeS9FoVWfJO8,9540
 yostlabs/tss3/utils/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
-yostlabs/tss3/utils/calibration.py,sha256=42jCEzfTXoHuPZ4e-30N1ijOhkz9ld4PQnhX6AhTrZE,7069
+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.9.18.dist-info/METADATA,sha256=x7xN8xCOTP4k2aicgQdt3xLUAlQolI3vqOLaozFk5Rw,2751
-yostlabs-2025.9.18.dist-info/WHEEL,sha256=qtCwoSJWgHk21S1Kb4ihdzI2rlJ1ZKaIurTj_ngOhyQ,87
-yostlabs-2025.9.18.dist-info/licenses/LICENSE,sha256=PtF8EXRlVhm1_ve52_3GHixSPwMn0tGajFxF3xKS-j0,1090
-yostlabs-2025.9.18.dist-info/RECORD,,
+yostlabs-2025.10.24.dist-info/METADATA,sha256=Tof9Bd5Jo79kErGMUZnljvu0r3QFhbF_MXHXnImBcGU,2752
+yostlabs-2025.10.24.dist-info/WHEEL,sha256=qtCwoSJWgHk21S1Kb4ihdzI2rlJ1ZKaIurTj_ngOhyQ,87
+yostlabs-2025.10.24.dist-info/licenses/LICENSE,sha256=PtF8EXRlVhm1_ve52_3GHixSPwMn0tGajFxF3xKS-j0,1090
+yostlabs-2025.10.24.dist-info/RECORD,,