yostlabs 2025.1.10__py3-none-any.whl → 2025.2.10__py3-none-any.whl

yostlabs/tss3/api.py

@@ -5,10 +5,12 @@ from yostlabs.communication.serial import ThreespaceSerialComClass
 from enum import Enum
 from dataclasses import dataclass, field
 from typing import TypeVar, Generic
+from collections.abc import Callable
 import struct
 import types
 import inspect
 import time
+import math
 
 
 #For converting from internal format specifiers to struct module specifiers
@@ -61,10 +63,11 @@ class ThreespaceCommand:
     BINARY_START_BYTE = 0xf7
     BINARY_START_BYTE_HEADER = 0xf9
 
-    def __init__(self, name: str, num: int, in_format: str, out_format: str):
+    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)
         self.out_format = _3space_format_to_external(self.info.out_format)
+        self.custom_func = custom_func
 
     def format_cmd(self, *args, header_enabled=False):
         cmd_data = struct.pack("<B", self.info.num)
@@ -86,23 +89,29 @@ class ThreespaceCommand:
     def parse_response(self, response: bytes):
         if self.info.num_out_params == 0: return None
         output = []
-        for c in self.out_format:
-            if c != 's':
-                format_str = f"<{c}"
-                size = struct.calcsize(format_str)
-                output.append(struct.unpack(format_str, response[:size])[0])
-                response = response[size:]
-            else: #Strings are special, find the null terminator
-                str_len = response.index(0)
-                output.append(struct.unpack(f"<{str_len}s", response[str_len])[0])
-                response = response[str_len + 1:] #+1 to skip past the null terminator character too
+        
+        if math.isnan(self.info.out_size): #Has strings in it, must slow parse
+            for c in self.out_format:
+                if c != 's':
+                    format_str = f"<{c}"
+                    size = struct.calcsize(format_str)
+                    output.append(struct.unpack(format_str, response[:size])[0])
+                    #TODO: Switch to using numpy views instead of slicing
+                    response = response[size:]
+                else: #Strings are special, find the null terminator
+                    str_len = response.index(0)
+                    output.append(struct.unpack(f"<{str_len}s", response[str_len])[0])
+                    response = response[str_len + 1:] #+1 to skip past the null terminator character too
+        else: #Fast parse because no strings
+            output.extend(struct.unpack(f"<{self.out_format}", response[:self.info.out_size]))
+
         
         if self.info.num_out_params == 1:
             return output[0]
         return output
 
     #Read the command dynamically from an input stream
-    def read_command(self, com: ThreespaceInputStream):
+    def read_command(self, com: ThreespaceInputStream, verbose=False):
         raw = bytearray([])
         if self.info.num_out_params == 0: return None, raw
         output = []
@@ -113,14 +122,16 @@ class ThreespaceCommand:
                 response = com.read(size)
                 raw += response
                 if len(response) != size:
-                    print(f"Failed to read {c} type. Aborting...")
+                    if verbose:
+                        print(f"Failed to read {c} type. Aborting...")
                     return None
                 output.append(struct.unpack(format_str, response)[0])
             else: #Strings are special, find the null terminator
                 response = com.read(1)
                 raw += response
                 if len(response) != 1:
-                    print(f"Failed to read string. Aborting...")
+                    if verbose:
+                        print(f"Failed to read string. Aborting...")
                     return None
                 byte = chr(response[0])
                 string = ""
@@ -130,7 +141,8 @@ class ThreespaceCommand:
                     response = com.read(1)
                     raw += response
                     if len(response) != 1:
-                        print(f"Failed to read string. Aborting...")
+                        if verbose:
+                            print(f"Failed to read string. Aborting...")
                         return None
                     byte = chr(response[0])
                 output.append(string)
@@ -150,6 +162,7 @@ class ThreespaceGetStreamingBatchCommand(ThreespaceCommand):
     def set_stream_slots(self, streaming_slots: list[ThreespaceCommand]):
         self.commands = streaming_slots
         self.out_format = ''.join(slot.out_format for slot in streaming_slots if slot is not None)
+        self.info.out_size = struct.calcsize(f"<{self.out_format}")
 
     def parse_response(self, response: bytes):
         data = []
@@ -161,7 +174,7 @@ class ThreespaceGetStreamingBatchCommand(ThreespaceCommand):
         
         return data
     
-    def read_command(self, com: ThreespaceInputStream):
+    def read_command(self, com: ThreespaceInputStream, verbose=False):
         #Get the response to all the streaming commands
         response = []
         raw_response = bytearray([])
@@ -404,6 +417,7 @@ class StreamableCommands(Enum):
 
 THREESPACE_AWAIT_COMMAND_FOUND = 0
 THREESPACE_AWAIT_COMMAND_TIMEOUT = 1
+THREESPACE_AWAIT_BOOTLOADER = 2
 
 T = TypeVar('T')
 
@@ -450,7 +464,7 @@ class ThreespaceBootloaderInfo:
 THREESPACE_REQUIRED_HEADER = THREESPACE_HEADER_ECHO_BIT | THREESPACE_HEADER_CHECKSUM_BIT | THREESPACE_HEADER_LENGTH_BIT
 class ThreespaceSensor:
     
-    def __init__(self, com = None, timeout=2):
+    def __init__(self, com = None, timeout=2, verbose=False):
         if com is None: #Default to attempting to use the serial com class if none is provided
             com = ThreespaceSerialComClass
         
@@ -473,22 +487,15 @@ class ThreespaceSensor:
             except:
                 raise ValueError("Failed to create default ThreespaceSerialComClass from parameter:", type(com), com)
 
-        self.com.read_all() #Clear anything that may be there
-
-        self.commands: list[ThreespaceCommand] = [None] * 256
-        self.getStreamingBatchCommand: ThreespaceGetStreamingBatchCommand = None
-        self.funcs = {}
-        for command in _threespace_commands:
-            #Some commands are special and need added specially
-            if command.info.num == THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM:
-                self.getStreamingBatchCommand = ThreespaceGetStreamingBatchCommand([])
-                command = self.getStreamingBatchCommand
-            
-            self.__add_command(command)
-
-        self.immediate_debug = False
+        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
+        self.header_info = ThreespaceHeaderInfo()
         self.header_enabled = True 
 
         #All the different streaming options
@@ -499,12 +506,27 @@ class ThreespaceSensor:
 
         #Used to ensure connecting to the correct sensor when reconnecting
         self.serial_number = None
+        self.short_serial_number = None
+        self.sensor_family = None
+        self.firmware_version = None
+
+        self.commands: list[ThreespaceCommand] = [None] * 256
+        self.getStreamingBatchCommand: ThreespaceGetStreamingBatchCommand = None
+        self.funcs = {}
 
         self.__cached_in_bootloader = self.__check_bootloader_status()
         if not self.in_bootloader:
             self.__firmware_init()
         else:
-            self.serial_number = self.bootloader_get_sn()
+            self.__cache_serial_number(self.bootloader_get_sn())
+            self.__empty_debug_cache()
+
+    #Just a helper for outputting information
+    def log(self, *args):
+        if self.verbose:
+            print(*args)
+
+#-----------------------INITIALIZIATION & REINITIALIZATION-----------------------------------
 
     def __firmware_init(self):
         """
@@ -512,9 +534,13 @@ class ThreespaceSensor:
         Called for powerup events when booting into firmware
         """
         self.dirty_cache = False #No longer dirty cause initializing
-
-        self.com.read_all() #Clear anything that may be there
         
+        #Only reinitialize settings if detected firmware version changed (Or on startup)
+        version = self.get_settings("version_firmware")
+        if version != self.firmware_version:
+            self.firmware_version = version
+            self.__initialize_commands()
+    
         self.__reinit_firmware()
         
         self.valid_mags = self.__get_valid_components("valid_mags")
@@ -522,16 +548,10 @@ class ThreespaceSensor:
         self.valid_gyros = self.__get_valid_components("valid_gyros")
         self.valid_baros = self.__get_valid_components("valid_baros")
 
-    def __get_valid_components(self, key: str):
-        valid = self.get_settings(key)
-        if len(valid) == 0: return []
-        return [int(v) for v in valid.split(',')]
-
     def __reinit_firmware(self):
         """
         Called when settings may have changed but a full reboot did not occur
         """
-        self.com.read_all() #Clear anything that may be there
         self.dirty_cache = False #No longer dirty cause initializing
         
         self.header_info = ThreespaceHeaderInfo()
@@ -543,26 +563,68 @@ class ThreespaceSensor:
         self.file_stream_length = 0
 
         self.streaming_packet_size = 0
-        self.header_enabled = True
         self._force_stop_streaming()
 
         #Now reinitialize the cached settings
         self.__cache_header_settings()
-        self.cache_streaming_settings()
+        self.__cache_streaming_settings()
 
-        self.serial_number = int(self.get_settings("serial_number"), 16)
+        self.__cache_serial_number(int(self.get_settings("serial_number"), 16))
+        self.__empty_debug_cache()
         self.immediate_debug = int(self.get_settings("debug_mode")) == 1 #Needed for some startup processes when restarting
 
+    def __initialize_commands(self):
+        self.commands: list[ThreespaceCommand] = [None] * 256
+        self.getStreamingBatchCommand: ThreespaceGetStreamingBatchCommand = None
+        self.funcs = {}
+
+        valid_commands = self.get_settings("valid_commands")
+        if valid_commands == THREESPACE_GET_SETTINGS_ERROR_RESPONSE:
+            #Treat all commands as valid because firmware is too old to have this setting
+            valid_commands = list(range(256))
+            self.log("Please update firmware to a version that contains ?valid_commands")
+        else:
+            valid_commands = list(int(v) for v in valid_commands.split(','))
+        
+        for command in _threespace_commands:
+            #Skip commands that are not valid for this sensor
+            if command.info.num not in valid_commands:
+                #Register as invalid.
+                setattr(self, command.info.name, self.__invalid_command)
+                continue
+
+            #Some commands are special and need added specially
+            if command.info.num == THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM:
+                self.getStreamingBatchCommand = ThreespaceGetStreamingBatchCommand([])
+                command = self.getStreamingBatchCommand
+            
+            self.__add_command(command)
+
+#------------------------------INITIALIZATION HELPERS--------------------------------------------
+
+    def __get_valid_components(self, key: str):
+        valid = self.get_settings(key)
+        if len(valid) == 0: return []
+        return [int(v) for v in valid.split(',')]
+
     def __add_command(self, command: ThreespaceCommand):
         if self.commands[command.info.num] != None:
-            print(f"Registering duplicate command: {command.info.num} {self.commands[command.info.num].info.name} {command.info.name}")
+            self.log(f"Registering duplicate command: {command.info.num} {self.commands[command.info.num].info.name} {command.info.name}")
         self.commands[command.info.num] = command
 
-        #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)"
-        exec(code, globals(), self.funcs)
-        setattr(self, command.info.name, types.MethodType(self.funcs[command.info.name], self))
+        #This command type has special logic that requires its own function.
+        #Make that function be called instead of using the generic execute that gets built
+        method = None
+        if command.custom_func is not None:
+            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)"
+            exec(code, globals(), self.funcs)
+            method = types.MethodType(self.funcs[command.info.name], self)
+
+        setattr(self, command.info.name, method)
 
     def __get_command(self, command_name: str):
         for command in self.commands:
@@ -570,10 +632,107 @@ class ThreespaceSensor:
             if command.info.name == command_name:
                 return command
         return None
+    
+    def __attempt_rediscover_self(self):
+        """
+        Trys to change the com class currently being used to be a detected
+        com class with the same serial number. Useful for re-enumeration, such as when
+        entering bootloader and using USB.
+        """
+        for potential_com in self.com.auto_detect():
+            potential_com.open()
+            sensor = ThreespaceSensor(potential_com)
+            if sensor.serial_number == self.serial_number:
+                self.com = potential_com
+                return True
+            sensor.cleanup() #Handles closing the potential_com
+        return False
 
-    @property
-    def is_streaming(self):
-        return self.is_data_streaming or self.is_log_streaming or self.is_file_streaming
+    def __cache_header_settings(self):
+        """
+        Should be called any time changes are made to the header. Will normally be called via the check_dirty/reinit
+        """
+        result = self.get_settings("header")
+        header = int(result)
+        #API requires these bits to be enabled, so don't let them be disabled
+        required_header = header | THREESPACE_REQUIRED_HEADER
+        if header == self.header_info.bitfield and header == required_header: return #Nothing to update
+        
+        #Don't allow the header to change while streaming
+        #This is to prevent a situation where the header for streaming and commands are different
+        #since streaming caches the header. This would cause an issue where the echo byte could be in seperate
+        #positions, causing a situation where parsing a command and streaming at the same time breaks since it thinks both are valid cmd echoes.
+        if self.is_streaming:
+            self.log("Preventing header change due to currently streaming")
+            self.set_settings(header=self.header_info.bitfield)
+            return
+        
+        if required_header != header:
+            self.log(f"Forcing header checksum, echo, and length enabled")
+            self.set_settings(header=required_header)
+            return
+        
+        #Current/New header is valid, so can cache it
+        self.header_info.bitfield = header
+        self.cmd_echo_byte_index = self.header_info.get_start_byte(THREESPACE_HEADER_ECHO_BIT) #Needed for cmd validation while streaming
+
+    def __cache_serial_number(self, serial_number: int):
+        """
+        Doesn't actually retrieve the serial number, rather sets various properties based on the serial number
+        """
+        self.serial_number = serial_number
+
+        #Short SN is the 32 bit version of the u64 serial number
+        #It is defined as the FamilyVersion (byte) << 24 | Incrementor (24 bits) 
+        family = (self.serial_number & THREESPACE_SN_FAMILY_MSK) >> THREESPACE_SN_FAMILY_POS
+        incrementor = (self.serial_number & THREESPACE_SN_INCREMENTOR_MSK) >> THREESPACE_SN_INCREMENTOR_POS
+        self.short_serial_number = family << 24 | incrementor
+        self.sensor_family = THREESPACE_SN_FAMILY_TO_NAME.get(family)
+        if self.sensor_family is None:
+            self.log(f"Unknown Sensor Family detected, {family}")
+            
+
+#--------------------------------REINIT/DIRTY Helpers-----------------------------------------------
+    def set_cached_settings_dirty(self):
+        """
+        Could be streaming settings, header settings...
+        Basically the sensor needs reinitialized
+        """
+        self.dirty_cache = True
+
+    def check_dirty(self):
+        if not self.dirty_cache: return
+        if self.com.reenumerates and not self.com.check_open(): #Must check this, as could have transitioned from bootloader to firmware or vice versa and just needs re-opened/detected
+            success = self.__attempt_rediscover_self()
+            if not success:
+                raise RuntimeError("Sensor connection lost")
+        
+        self._force_stop_streaming() #Can't be streaming when checking the dirty cache. If you want to stream, don't do things that cause the object to go dirty.
+        was_in_bootloader = self.__cached_in_bootloader
+        self.__cached_in_bootloader = self.__check_bootloader_status()
+        
+        if was_in_bootloader and not self.__cached_in_bootloader: #Just Exited bootloader, need to fully reinit
+            self.__firmware_init()
+        elif not self.__cached_in_bootloader:   #Was already in firmware, so only need to partially reinit
+            self.__reinit_firmware()    #Partially init when just naturally dirty
+        self.dirty_cache = False
+
+#-----------------------------------DEBUG COMMANDS---------------------------------------------------
+    def __default_debug_callback(self, msg: str, sensor: "ThreespaceSensor"):
+        if self.serial_number is None:
+            self.__debug_cache.append(msg.strip())
+        else:
+            print(f"DEBUG {hex(self.serial_number)}:", msg.strip())
+
+    def __empty_debug_cache(self):
+        for msg in self.__debug_cache:
+            print(f"DEBUG {hex(self.serial_number)}:", msg)
+        self.__debug_cache.clear()
+
+    def set_debug_callback(self, callback: Callable[[str, "ThreespaceSensor"], None]):
+        self.debug_callback = callback
+
+#-----------------------------------------------BASE SETTINGS PROTOCOL------------------------------------------------
 
     #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"]
@@ -593,10 +752,18 @@ class ThreespaceSensor:
             params.append(f"{key}={value}")
         cmd = f"!{';'.join(params)}\n"
 
+        if len(cmd) > 2048:
+            self.log("Too many settings in one set_settings call. Max str length is 2048 but got", len(cmd))
+            return 0xFF, 0xFF
+
         #For dirty check
         keys = cmd[1:-1].split(';')
         keys = [v.split('=')[0] for v in keys]
 
+        #Must enable this before sending the set so can properly handle reading the response
+        if "debug_mode=1" in cmd:
+            self.immediate_debug = True
+
         #Send cmd
         self.com.write(cmd.encode())
 
@@ -604,50 +771,18 @@ class ThreespaceSensor:
         err = 3
         num_successes = 0
 
-        #Read response
-        if self.is_streaming: #Streaming have to read via peek and also validate it more
-            max_response_length = len("255,255\r\n")
-            found_response = False
-            start_time = time.time()
-            while not found_response: #Infinite loop to wait for the data to be available
-                if time.time() - start_time > self.com.timeout:
-                    print("Timed out waiting for set_settings response")
-                    return err, num_successes
-                line = ""
-                while True: #A loop used to allow breaking out of to be less wet.
-                    line = self.com.peekline(max_length=max_response_length)
-                    if b'\n' not in line:
-                        break
-                    
-                    try:
-                        values = line.decode().strip()
-                        values = values.split(',')
-                        if len(values) != 2: break
-                        err = int(values[0])
-                        num_successes = int(values[1])
-                    except: break
-                    if err > 255 or num_successes > 255:
-                        break
-
-                    #Successfully got pass all the checks!
-                    #Consume the buffer and continue
-                    found_response = True
-                    self.com.readline()
-                    break
-                if found_response: break
-                while not self.updateStreaming(max_checks=1): pass #Wait for streaming to parse something!
-        else:
-            #When not streaming, way more straight forward
-            try:
-                response = self.com.readline()
-                response = response.decode().strip()
-                err, num_successes = response.split(',')
-                err = int(err)
-                num_successes = int(num_successes)    
-            except:
-                print("Failed to parse set response:", response)
-                return err, num_successes
-        
+        response = self.__await_set_settings(self.com.timeout)
+        if response == THREESPACE_AWAIT_COMMAND_TIMEOUT:
+            self.log("Failed to get set_settings response")
+            return err, num_successes
+
+        #Decode response
+        response = self.com.readline()
+        response = response.decode().strip()
+        err, num_successes = response.split(',')
+        err = int(err)
+        num_successes = int(num_successes)    
+
         #Handle updating state variables based on settings
         #If the user modified the header, need to cache the settings so the API knows how to interpret responses
         if "header" in cmd.lower(): #First do a quick check
@@ -655,13 +790,14 @@ class ThreespaceSensor:
                 self.__cache_header_settings()
         
         if "stream_slots" in cmd.lower():
-            self.cache_streaming_settings()
+            self.__cache_streaming_settings()
         
-        if any(v in keys for v in ("default", "reboot")): #All the settings changed, just need to mark dirty
+        #All the settings changed, just need to mark dirty
+        if any(v in keys for v in ("default", "reboot")):
             self.set_cached_settings_dirty()
 
         if err:
-            print(f"Err setting {cmd}: {err=} {num_successes=}")
+            self.log(f"Err setting {cmd}: {err=} {num_successes=}")
         return err, num_successes
 
     def get_settings(self, *args: str) -> dict[str, str] | str:
@@ -672,91 +808,165 @@ class ThreespaceSensor:
         self.com.write(cmd.encode())
 
         keys = cmd[1:-1].split(';')
-        error_response = "<KEY_ERROR>"
+        error_response_len = len(THREESPACE_GET_SETTINGS_ERROR_RESPONSE)
 
-        #Wait for the response to be available if streaming
-        #NOTE: THIS WILL NOT WORK WITH SETTINGS SUCH AS ?all ?settings or QUERY STRINGS
-        #THIS can be worked around by first getting a setting that does echo normally, as that will allow
-        #the sensor to determine where the ascii data actually starts.
-        #Ex: get_settings("header", "all") would work
-        if self.is_streaming:
-            first_key = bytes(keys[0] + "=", 'ascii') #Add on the equals sign to try and make this less likely to conflict with binary data
-            possible_outputs = [(len(error_response), bytes(error_response, 'ascii')), (len(first_key), first_key)]
-            possible_outputs.sort() #Must try the smallest one first because if streaming is slow, may take a while for the data to fill pass the largest possible value
-            start_time = time.time()
-            while True:
-                if time.time() - start_time > self.com.timeout:
-                    print("Timeout parsing get response")
-                    return {}
-                found_response = False
-                for length, key in possible_outputs:
-                    possible_response = self.com.peek(length)
-                    if possible_response == key: #This the response, so break and parse
-                        found_response = True
-                        break
-                if found_response: break
-                while not self.updateStreaming(max_checks=1): pass #Wait for streaming to process something. May just advance due to invalid
+        min_resp_length = 0
+        for key in keys:
+            min_resp_length += min(len(key) + 1, error_response_len)
+        
         
-        #Read the response
-        try:
-            response = self.com.readline()
-            if ord('\n') not in response:
-                print("Failed to get whole line")
-            response = response.decode().strip().split(';')
-        except:
-            print("Failed to parse get:", response)
+        response = self.__await_get_settings(min_resp_length, timeout=self.com.timeout)
+        if response == THREESPACE_AWAIT_COMMAND_TIMEOUT:
+            self.log("Requested:", cmd)
+            self.log("Potential response:", self.com.peekline())
+            raise RuntimeError("Failed to receive get_settings response")      
+
+        response = self.com.readline()
+        response = response.decode().strip().split(';')
         
         #Build the response dict
         response_dict = {}
         for i, v in enumerate(response):
-            if v == error_response:
-                response_dict[keys[i]] = error_response
+            if v == THREESPACE_GET_SETTINGS_ERROR_RESPONSE:
+                response_dict[keys[i]] = THREESPACE_GET_SETTINGS_ERROR_RESPONSE
                 continue
             try:
                 key, value = v.split('=')
                 response_dict[key] = value
             except:
-                print("Failed to parse get:", response)
+                self.log("Failed to parse get value:", i, v, len(v))
         
         #Format response
         if len(response_dict) == 1:
             return list(response_dict.values())[0]
         return response_dict
 
-    def execute_command(self, cmd: ThreespaceCommand, *args):
-        self.check_dirty()
+    #-----------Base Settings Parsing----------------
 
-        retries = 0
-        MAX_RETRIES = 3
+    def __await_set_settings(self, timeout=2):
+        start_time = time.time()
+        MINIMUM_LENGTH = len("0,0\r\n")
+        MAXIMUM_LENGTH = len("255,255\r\n")
 
-        while retries < MAX_RETRIES:
-            cmd.send_command(self.com, *args, header_enabled=self.header_enabled)
-            result = self.__await_command(cmd)
-            if result == THREESPACE_AWAIT_COMMAND_FOUND:
-                break
-            retries += 1
-        
-        if retries == MAX_RETRIES:
-            raise RuntimeError(f"Failed to get response to command {cmd.info.name}")
+        while True:
+            remaining_time = timeout - (time.time() - start_time)
+            if remaining_time <= 0:
+                return THREESPACE_AWAIT_COMMAND_TIMEOUT
+            if self.com.length < MINIMUM_LENGTH: continue
+            
+            possible_response = self.com.peekline()
+            if b'\r\n' not in possible_response: continue
 
-        return self.read_and_parse_command(cmd)
-    
-    def read_and_parse_command(self, cmd: ThreespaceCommand):
-        if self.header_enabled:
-            header = ThreespaceHeader.from_bytes(self.com.read(self.header_info.size), self.header_info)
-        else:
-            header = ThreespaceHeader()
-        result, raw = cmd.read_command(self.com)
-        return ThreespaceCmdResult(result, header, data_raw_binary=raw)
+            if len(possible_response) < MINIMUM_LENGTH:
+                self.__internal_update(self.__try_peek_header())
+                continue
+
+            #Attempt to parse the line
+            values = possible_response.split(b',')
+            if len(values) != 2:
+                self.__internal_update(self.__try_peek_header())
+                continue
+
+            v1 = 0
+            v2 = 0
+            try:
+                v1 = int(values[0].decode())
+                v2 = int(values[0].decode())
+            except:
+                self.__internal_update(self.__try_peek_header())
+                continue
+
+            if v1 < 0 or v1 > 255 or v2 < 0 or v2 > 255:
+                self.__internal_update(self.__try_peek_header())
+                continue
+            
+            self.misaligned = False
+            return THREESPACE_AWAIT_COMMAND_FOUND
+            
+    def __await_get_settings(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
+            
+            possible_response = self.com.peekline()
+            if b'\r\n' not in possible_response: #failed to get newline
+                continue
+
+            if len(possible_response) < min_resp_length:
+                self.__internal_update(self.__try_peek_header())
+                continue
+
+            #Make sure the line is all ascii data
+            if not possible_response.isascii():
+                self.__internal_update(self.__try_peek_header())
+                continue
+
+            #Check to make sure each potential key conforms to the standard
+            key_value_pairs = possible_response.decode().split(';')
+            err = False
+            for kvp in key_value_pairs:
+                if kvp.strip() == THREESPACE_GET_SETTINGS_ERROR_RESPONSE: continue
+                split = kvp.split('=')
+                if len(split) != 2:
+                    err = True
+                    break
+                k, v = split
+                if any(c in k for c in THREESPACE_SETTING_KEY_INVALID_CHARS):
+                    err = True
+                    break
+            if err:
+                self.__internal_update(self.__try_peek_header())
+                continue
+            
+            self.misaligned = False
+            return THREESPACE_AWAIT_COMMAND_FOUND
+
+#---------------------------------BASE COMMAND PARSING--------------------------------------
+    def __try_peek_header(self):
+        """
+        Attempts to retrieve a header from the com class immediately.
+
+        Returns
+        -------
+        The header retrieved, or None
+        """
+        if not self.header_enabled: return None
+        if self.com.length < self.header_info.size: return None
+        header = self.com.peek(self.header_info.size)
+        if len(header) != self.header_info.size: return None
+        header = ThreespaceHeader.from_bytes(header, self.header_info)
+        return header
+
+    def __peek_checksum(self, header: ThreespaceHeader, max_data_length=4096):
+        """
+        Using a header that contains the checksum and data length, calculate the checksum of the expected
+        data and verify if with the checksum in the header.
 
-    def __peek_checksum(self, header: ThreespaceHeader):
+        Params
+        ------
+        header : The header to verify
+        max_data_length : The maximum size to allow from header_length. This should be set to avoid a corrupted header with an extremely large length causing a lockup/timeout
+        """
         header_len = len(header.raw_binary)
+        if header.length > max_data_length:
+            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
         checksum = sum(data) % 256
         return checksum == header.checksum
 
     def __await_command(self, cmd: ThreespaceCommand, timeout=2):
+        #Header isn't enabled, nothing can do. Just pretend we found it
+        if not self.header_enabled: return THREESPACE_AWAIT_COMMAND_FOUND
+
         start_time = time.time()
 
         #Update the streaming until the result for this command is next in the buffer
@@ -765,179 +975,137 @@ class ThreespaceSensor:
                 return THREESPACE_AWAIT_COMMAND_TIMEOUT
             
             #Get potential header
-            header = self.com.peek(self.header_info.size)
-            if len(header) != self.header_info.size: #Wait for more data
+            header = self.__try_peek_header()
+            if header is None:
                 continue
 
-            #Check to see what this packet is a response to
-            header = ThreespaceHeader.from_bytes(header, self.header_info)
             echo = header.echo
 
             if echo == cmd.info.num: #Cmd matches
-                if self.__peek_checksum(header):
+                if self.__peek_checksum(header, max_data_length=cmd.info.out_size):
+                    self.misaligned = False
                     return THREESPACE_AWAIT_COMMAND_FOUND
                 
                 #Error in packet, go start realigning
                 if not self.misaligned:
-                    print(f"Checksum mismatch for command {cmd.info.num}")
+                    self.log(f"Checksum mismatch for command {cmd.info.num}")
                     self.misaligned = True
                 self.com.read(1)
             else:
                 #It wasn't a response to the command, so may be a response to some internal system
-                self.__internal_update(header)
+                self.__internal_update(header)        
+
+#------------------------------BASE INPUT PARSING--------------------------------------------
 
-    def __internal_update(self, header: ThreespaceHeader):
+    def __internal_update(self, header: ThreespaceHeader = None):
         """
-        This should be called after a header is obtained via a command and it is determined that it can't
-        be in response to a synchronous command that got sent. This manages updating the streaming and realigning
-        the data buffer
+        Manages checking the datastream for asynchronous responses (Streaming, Immediate Debug Messages).
+        If no data is found to match these responses, the data buffer will be considered corrupted/misaligned
+        and start advancing 1 byte at a time until a message is retrieved.
+        For this reason, if waiting for a synchronous command response, this should be only checked after confirming the data
+        is not in response to any synchronously queued commands to avoid removing actual data bytes from the com class.
+
+        Parameters
+        ----------
+        header : ThreespaceHeader
+            The header to use for checking if streaming results exist. Can optionally leave None if don't want to check streaming responses.
+
+        Returns
+        --------
+        False : Misalignment
+        True : Internal Data Found/Parsed
         """
         checksum_match = False #Just for debugging
 
-        #NOTE: FOR THIS TO WORK IT IS REQUIRED THAT THE HEADER DOES NOT CHANGE WHILE STREAMING ANY FORM OF DATA.
-        #IT IS UP TO THE API TO ENFORCE NOT ALLOWING HEADER CHANGES WHILE ANY OF THOSE THINGS ARE HAPPENING
-        if self.is_data_streaming and header.echo == THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM: #Its a streaming packet, so update streaming
-            if checksum_match := self.__peek_checksum(header):
-                self.__update_base_streaming()
-                return True
-        elif self.is_log_streaming and header.echo == THREESPACE_FILE_READ_BYTES_COMMAND_NUM:
-            if checksum_match := self.__peek_checksum(header):
-                self.__update_log_streaming()
-                return True
-        elif self.is_file_streaming and header.echo == THREESPACE_FILE_READ_BYTES_COMMAND_NUM:
-            if checksum_match := self.__peek_checksum(header):
-                self.__update_file_streaming()
-                return True
+        if header is not None:
+            #NOTE: FOR THIS TO WORK IT IS REQUIRED THAT THE HEADER DOES NOT CHANGE WHILE STREAMING ANY FORM OF DATA.
+            #IT IS UP TO THE API TO ENFORCE NOT ALLOWING HEADER CHANGES WHILE ANY OF THOSE THINGS ARE HAPPENING
+            if self.is_data_streaming and header.echo == THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM: #Its a streaming packet, so update streaming
+                if checksum_match := self.__peek_checksum(header, max_data_length=self.getStreamingBatchCommand.info.out_size):
+                    self.__update_base_streaming()
+                    self.misaligned = False
+                    return True
+            elif self.is_log_streaming and header.echo == THREESPACE_FILE_READ_BYTES_COMMAND_NUM:
+                if checksum_match := self.__peek_checksum(header, max_data_length=2560): #TODO: Confirm this number can be 2048 and then pound define it. Currently set to 2560 because I know that is big enough, but not optimal
+                    self.__update_log_streaming()
+                    self.misaligned = False
+                    return True
+            elif self.is_file_streaming and header.echo == THREESPACE_FILE_READ_BYTES_COMMAND_NUM:
+                if checksum_match := self.__peek_checksum(header, max_data_length=THREESPACE_FILE_STREAMING_MAX_PACKET_SIZE):
+                    self.__update_file_streaming()
+                    self.misaligned = False
+                    return True
+        
+        #Debug messages are possible and there is enough data to potentially be a debug message
+        #NOTE: Firmware should avoid putting more then one \r\n in a debug message as they will be treated as unprocessed/misaligned characters
+        if self.immediate_debug and self.com.length >= 7:
+            #This peek can't be blocking so peekline can't be used
+            potential_message = self.com.peek(min(self.com.length, 27)) #27 is 20 digit timestamp + " Level:"
+            if b"Level:" in potential_message: #There is a debug message somewhere in the data, must validate it is the next item
+                level_index = potential_message.index(b" Level:")
+                partial = potential_message[:level_index]
+                #There should not be a newline until the end of the message, so it shouldn't be in partial
+                if partial.isascii() and partial.decode('ascii').isnumeric() and b'\r\n' not in partial:
+                    message = self.com.readline() #Read out the whole message!
+                    self.debug_callback(message.decode('ascii'), self)
+                    self.misaligned = False
+                    return True
 
         #The response didn't match any of the expected asynchronous streaming API responses, so assume a misalignment
-        #and start reading through the buffer
-        if not self.misaligned:
-            print(f"Possible Misalignment or corruption/debug message, header {header} raw {[hex(v) for v in header.raw_binary]}, Checksum match? {checksum_match}")
-            self.misaligned = True
-        self.com.read(1) #Because of expected misalignment, go through buffer 1 by 1 until realigned
-
-    def updateStreaming(self, max_checks=float('inf')):
-        """
-        Returns true if any amount of data was processed whether valid or not
-        """
-        if not self.is_streaming: return False
-
-        #I may need to make this have a max num bytes it will process before exiting to prevent locking up on slower machines
-        #due to streaming faster then the program runs
-        num_checks = 0
-        data_processed = False
-        while num_checks < max_checks:
-            if self.com.length < self.header_info.size:
-                return data_processed
-            
-            #Get header
-            header = self.com.peek(self.header_info.size)
+        if header is not None:
+            msg = f"Possible Misalignment or corruption/debug message, header {header} raw {header.raw_binary} {[hex(v) for v in header.raw_binary]}" \
+            f" Checksum match? {checksum_match}"
+            #f"{self.com.peek(min(self.com.length, 10))}"
+        else:
+            msg = "Possible Misalignment or corruption/debug message"
+        #self.log("Misaligned:", self.com.peek(1))
+        self.__handle_misalignment(msg)
+        return False
 
-            #Get the header and send it to the internal update
-            header = ThreespaceHeader.from_bytes(header, self.header_info)
-            self.__internal_update(header)
-            data_processed = True #Internal update always processes data. Either reads a streaming message, or advances buffer due to misalignment
-            num_checks += 1
-        
-        return data_processed
+    def __handle_misalignment(self, message: str = None):
+        if not self.misaligned and message is not None:
+            self.log(message)
+        self.misaligned = True
+        self.com.read(1) #Because of expected misalignment, go through buffer 1 by 1 until realigned
 
+#-----------------------------BASE COMMAND EXECUTION-------------------------------------
 
-    def startStreaming(self):
-        if self.is_data_streaming: return
+    def execute_command(self, cmd: ThreespaceCommand, *args):
         self.check_dirty()
-        self.streaming_packets.clear()
 
-        self.header_enabled = True
+        retries = 0
+        MAX_RETRIES = 3
 
-        self.cache_streaming_settings()
+        while retries < MAX_RETRIES:
+            cmd.send_command(self.com, *args, header_enabled=self.header_enabled)
+            result = self.__await_command(cmd)
+            if result == THREESPACE_AWAIT_COMMAND_FOUND:
+                break
+            retries += 1
+        
+        if retries == MAX_RETRIES:
+            raise RuntimeError(f"Failed to get response to command {cmd.info.name}")
 
-        cmd = self.commands[85]
-        cmd.send_command(self.com, header_enabled=self.header_enabled)
-        if self.header_enabled:
-            self.__await_command(cmd)
-            header = ThreespaceHeader.from_bytes(self.com.read(self.header_info.size), self.header_info)
-        else:
-            header = ThreespaceHeader()
-        self.is_data_streaming = True
-        return ThreespaceCmdResult(None, header)
-
-    def _force_stop_streaming(self):
-        """
-        This function is used to stop streaming without validating it was streaming and ignoring any output of the
-        communication line. This is a destructive call that will lose data, but will gurantee stopping streaming
-        and leave the communication line in a clean state
-        """
-        cached_header_enabled = self.header_enabled
-        cahched_dirty = self.dirty_cache
-
-        #Must set these to gurantee it doesn't try and parse a response from anything
-        self.dirty_cache = False
-        self.header_enabled = False #Keep off for the attempt at stop streaming since if in an invalid state, won't be able to get response
-        self.stopStreaming() #Just in case was streaming
-        self.fileStopStream()
-
-        #TODO: Change this to pause the data logging instead, then check the state and update
-        self.stopDataLogging()
-        
-        #Restore
-        self.header_enabled = cached_header_enabled
-        self.dirty_cache = cahched_dirty
+        return self.read_and_parse_command(cmd)
+    
+    def __invalid_command(self, *args):
+        raise NotImplementedError("This method is not available.")
 
-    def stopStreaming(self):
-        self.check_dirty()
-        cmd = self.commands[86]
-        cmd.send_command(self.com, header_enabled=self.header_enabled)
-        if self.header_enabled: #Header will be enabled while streaming, but this is useful for startup
-            self.__await_command(cmd)
+    def read_and_parse_command(self, cmd: ThreespaceCommand):
+        if self.header_enabled:
             header = ThreespaceHeader.from_bytes(self.com.read(self.header_info.size), self.header_info)
         else:
             header = ThreespaceHeader()
-        time.sleep(0.05)
-        while self.com.length:
-            self.com.read_all()
-        self.is_data_streaming = False
-        return ThreespaceCmdResult(None, header)
-
-    def set_cached_settings_dirty(self):
-        """
-        Could be streaming settings, header settings...
-        Basically the sensor needs reinitialized
-        """
-        self.dirty_cache = True
+        result, raw = cmd.read_command(self.com, verbose=self.verbose)
+        return ThreespaceCmdResult(result, header, data_raw_binary=raw)
 
-    def __attempt_rediscover_self(self):
-        """
-        Trys to change the com class currently being used to be a detected
-        com class with the same serial number. Useful for re-enumeration, such as when
-        entering bootloader and using USB
-        """
-        for potential_com in self.com.auto_detect():
-            potential_com.open()
-            sensor = ThreespaceSensor(potential_com)
-            if sensor.serial_number == self.serial_number:
-                self.com = potential_com
-                return True
-            sensor.cleanup() #Handles closing the potential_com
-        return False
+#-----------------------------------BASE STREAMING COMMMANDS----------------------------------------------
 
-    def check_dirty(self):
-        if not self.dirty_cache: return
-        if self.com.reenumerates and not self.com.check_open(): #Must check this, as could have transitioned from bootloader to firmware or vice versa and just needs re-opened/detected
-            success = self.__attempt_rediscover_self()
-            if not success:
-                raise RuntimeError("Sensor connection lost")
-        
-        self._force_stop_streaming() #Can't be streaming when checking the dirty cache. If you want to stream, don't do things that cause the object to go dirty.
-        was_in_bootloader = self.__cached_in_bootloader
-        self.__cached_in_bootloader = self.__check_bootloader_status()
-        
-        if was_in_bootloader and not self.__cached_in_bootloader: #Just Exited bootloader, need to fully reinit
-            self.__firmware_init()
-        elif not self.__cached_in_bootloader:   #Was already in firmware, so only need to partially reinit
-            self.__reinit_firmware()    #Partially init when just naturally dirty
-        self.dirty_cache = False
+    @property
+    def is_streaming(self):
+        return self.is_data_streaming or self.is_log_streaming or self.is_file_streaming
 
-    def cache_streaming_settings(self):
+    def __cache_streaming_settings(self):
         cached_slots: list[ThreespaceCommand] = []
         slots: str = self.get_settings("stream_slots")
         slots = slots.split(',')
@@ -954,32 +1122,21 @@ class ThreespaceSensor:
             if command == None: continue
             self.streaming_packet_size += command.info.out_size
 
-    def __cache_header_settings(self):
-        """
-        Should be called any time changes are made to the header. Will normally be called via the check_dirty/reinit
-        """
-        header = int(self.get_settings("header"))
-        #API requires these bits to be enabled, so don't let them be disabled
-        required_header = header | THREESPACE_REQUIRED_HEADER
-        if header == self.header_info.bitfield and header == required_header: return #Nothing to update
-        
-        #Don't allow the header to change while streaming
-        #This is to prevent a situation where the header for streaming and commands are different
-        #since streaming caches the header. This would cause an issue where the echo byte could be in seperate
-        #positions, causing a situation where parsing a command and streaming at the same time breaks since it thinks both are valid cmd echoes.
-        if self.is_streaming:
-            print("PREVENTING HEADER CHANGE DUE TO CURRENTLY STREAMING")
-            self.set_settings(header=self.header_info.bitfield)
-            return
-        
-        if required_header != header:
-            print(f"Forcing header checksum, echo, and length enabled")
-            self.set_settings(header=required_header)
-            return
-        
-        #Current/New header is valid, so can cache it
-        self.header_info.bitfield = header
-        self.cmd_echo_byte_index = self.header_info.get_start_byte(THREESPACE_HEADER_ECHO_BIT) #Needed for cmd validation while streaming
+    def startStreaming(self) -> ThreespaceCmdResult[None]: ...
+    def __startStreaming(self) -> ThreespaceCmdResult[None]:
+        if self.is_data_streaming: return
+        self.streaming_packets.clear()
+        self.__cache_streaming_settings()
+
+        result = self.execute_command(self.commands[THREESPACE_START_STREAMING_COMMAND_NUM])
+        self.is_data_streaming = True
+        return result
+
+    def stopStreaming(self) -> ThreespaceCmdResult[None]: ...
+    def __stopStreaming(self) -> ThreespaceCmdResult[None]:
+        result = self.execute_command(self.commands[THREESPACE_STOP_STREAMING_COMMAND_NUM])
+        self.is_data_streaming = False
+        return result
 
     def __update_base_streaming(self):
         """
@@ -1000,44 +1157,85 @@ class ThreespaceSensor:
     def clearStreamingPackets(self):
         self.streaming_packets.clear()
     
-    def fileStartStream(self) -> ThreespaceCmdResult[int]:
-        self.check_dirty()
-        self.header_enabled = True
+    #This is called for all streaming types
+    def updateStreaming(self, max_checks=float('inf')):
+        """
+        Returns true if any amount of data was processed whether valid or not. This is called for all streaming types.
+        """
+        if not self.is_streaming: return False
 
-        cmd = self.__get_command("__fileStartStream")
-        cmd.send_command(self.com, header_enabled=self.header_enabled)
-        self.__await_command(cmd)
+        #I may need to make this have a max num bytes it will process before exiting to prevent locking up on slower machines
+        #due to streaming faster then the program runs
+        num_checks = 0
+        data_processed = False
+        while num_checks < max_checks:
+            if self.com.length < self.header_info.size:
+                return data_processed
+            
+            #Get header
+            header = self.com.peek(self.header_info.size)
+
+            #Get the header and send it to the internal update
+            header = ThreespaceHeader.from_bytes(header, self.header_info)
+            self.__internal_update(header)
+            data_processed = True #Internal update always processes data. Either reads a streaming message, or advances buffer due to misalignment
+            num_checks += 1
         
-        if self.header_enabled:
-            header = ThreespaceHeader.from_bytes(self.com.read(self.header_info.size), self.header_info)
-        else:
-            header = ThreespaceHeader()
+        return data_processed
 
-        result, raw = cmd.read_command(self.com)
-        self.file_stream_length = result
-        self.is_file_streaming = True  
+    #This is more so used for initialization. Its a way of stopping streaming without having to worry about parsing.
+    #That way it can clean up the data stream that won't match the expected state if not already configured.
+    def _force_stop_streaming(self):
+        """
+        This function attempts to stop all possible streaming without knowing anything about the state of the sensor.
+        This includes trying to stop before any commands are even registered as valid. This is to ensure the sensor can properly
+        start and recover from error conditions.
 
-        return ThreespaceCmdResult(result, header, data_raw_binary=raw)
-    
-    def fileStopStream(self) -> ThreespaceCmdResult[None]:
-        self.check_dirty()
+        This will stop streaming without validating it was streaming and ignoring any output of the
+        communication line. This is a destructive call that will lose data, but will gurantee stopping streaming
+        and leave the communication line in a clean state.
+        """
+        cached_header_enabled = self.header_enabled
+        cahched_dirty = self.dirty_cache
 
-        cmd = self.__get_command("__fileStopStream")
-        cmd.send_command(self.com, header_enabled=self.header_enabled)
+        #Must set these to gurantee it doesn't try and parse a response from anything since don't know the state of header
+        self.dirty_cache = False
+        self.header_enabled = False #Keep off for the attempt at stop streaming since if in an invalid state, won't be able to get response
 
-        if self.header_enabled: #Header will be enabled while streaming, but this is useful for startup
-            self.__await_command(cmd)
-            header = ThreespaceHeader.from_bytes(self.com.read(self.header_info.size), self.header_info)
-        else:
-            header = ThreespaceHeader()
+        #NOTE that commands are accessed directly from the global table instead of commands registered to this sensor object
+        #since this sensor object may have yet to register these commands when calling force_stop_streaming
+
+        #Stop base Streaming
+        self.execute_command(threespaceCommandGetByName("stopStreaming"))
+        self.is_data_streaming = False
+
+        #Stop file streaming
+        self.execute_command(threespaceCommandGetByName("fileStopStream"))
+        self.is_file_streaming = False  
+
+        #Stop logging streaming
+        # #TODO: Change this to pause the data logging instead, then check the state and update
+        self.execute_command(threespaceCommandGetByName("stopDataLogging"))
+        self.is_log_streaming = False              
         
-        #TODO: Remove me now that realignment exists and multiple things can be streaming at once
-        time.sleep(0.05)
-        while self.com.length:
-            self.com.read_all()
+        #Restore
+        self.header_enabled = cached_header_enabled
+        self.dirty_cache = cahched_dirty
+
+#-------------------------------------FILE STREAMING----------------------------------------------
 
+    def fileStartStream(self) -> ThreespaceCmdResult[int]: ...
+    def __fileStartStream(self) -> ThreespaceCmdResult[int]:
+        result = self.execute_command(self.__get_command("fileStartStream"))
+        self.file_stream_length = result.data
+        self.is_file_streaming = True
+        return result
+    
+    def fileStopStream(self) -> ThreespaceCmdResult[None]: ...
+    def __fileStopStream(self) -> ThreespaceCmdResult[None]:
+        result = self.execute_command(self.__get_command("fileStopStream"))
         self.is_file_streaming = False
-        return ThreespaceCmdResult(None, header)
+        return result
 
     def getFileStreamData(self):
         to_return = self.file_stream_data.copy()
@@ -1055,52 +1253,32 @@ class ThreespaceSensor:
         data = self.com.read(header.length)
         self.file_stream_data += data
         self.file_stream_length -= header.length
-        if header.length < 512 or self.file_stream_length == 0: #File streaming sends in chunks of 512. If not 512, it must be the last packet
+        if header.length < THREESPACE_FILE_STREAMING_MAX_PACKET_SIZE or self.file_stream_length == 0: #File streaming sends in chunks of 512. If not 512, it must be the last packet
             self.is_file_streaming = False
             if self.file_stream_length != 0:
-                print(f"File streaming stopped due to last packet. However still expected {self.file_stream_length} more bytes.")
+                self.log(f"File streaming stopped due to last packet. However still expected {self.file_stream_length} more bytes.")
 
-    def startDataLogging(self) -> ThreespaceCmdResult[None]:
-        self.check_dirty()
+#----------------------------DATA LOGGING--------------------------------------
 
-        self.header_enabled = True
-        self.cache_streaming_settings()
+    def startDataLogging(self) -> ThreespaceCmdResult[None]: ...
+    def __startDataLogging(self) -> ThreespaceCmdResult[None]:
+        self.__cache_streaming_settings()
 
         #Must check whether streaming is being done alongside logging or not. Also configure required settings if it is
         streaming = bool(int(self.get_settings("log_immediate_output")))
         if streaming:
             self.set_settings(log_immediate_output_header_enabled=1,
                                 log_immediate_output_header_mode=THREESPACE_OUTPUT_MODE_BINARY) #Must have header enabled in the log messages for this to work and must use binary for the header
-        cmd = self.__get_command("__startDataLogging")
-        cmd.send_command(self.com, header_enabled=self.header_enabled)
-        if self.header_enabled:
-            self.__await_command(cmd)
-            header = ThreespaceHeader.from_bytes(self.com.read(self.header_info.size), self.header_info)
-        else:
-            header = ThreespaceHeader()
-
+        
+        result = self.execute_command(self.__get_command("startDataLogging"))
         self.is_log_streaming = streaming 
-        return ThreespaceCmdResult(None, header)
-
-    def stopDataLogging(self) -> ThreespaceCmdResult[None]:
-        self.check_dirty()
-
-        cmd = self.__get_command("__stopDataLogging")
-        cmd.send_command(self.com, header_enabled=self.header_enabled)
-
-        if self.header_enabled: #Header will be enabled while streaming, but this is useful for startup
-            self.__await_command(cmd)
-            header = ThreespaceHeader.from_bytes(self.com.read(self.header_info.size), self.header_info)
-        else:
-            header = ThreespaceHeader()
-        #TODO: Remove me now that realignment exists and multiple things can be streaming at once
-        if self.is_log_streaming:
-            time.sleep(0.05)
-            while self.com.length:
-                self.com.read_all()
+        return result
 
+    def stopDataLogging(self) -> ThreespaceCmdResult[None]: ...
+    def __stopDataLogging(self) -> ThreespaceCmdResult[None]:
+        result = self.execute_command(self.__get_command("stopDataLogging"))
         self.is_log_streaming = False
-        return ThreespaceCmdResult(None, header)
+        return result
 
     def __update_log_streaming(self):
         """
@@ -1112,23 +1290,26 @@ class ThreespaceSensor:
         data = self.com.read(header.length)
         self.file_stream_data += data
 
-    def softwareReset(self):
+#---------------------------------POWER STATE CHANGING COMMANDS & BOOTLOADER------------------------------------
+
+    def softwareReset(self): ...
+    def __softwareReset(self):
         self.check_dirty()
-        cmd = self.commands[226]
+        cmd = self.commands[THREESPACE_SOFTWARE_RESET_COMMAND_NUM]
         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
         self.com.open()
-        if self.immediate_debug:
-            time.sleep(2) #An additional 2 seconds to ensure can clear all debug messages
-            self.com.read_all()
         self.__firmware_init()
 
-    def enterBootloader(self):
+    def enterBootloader(self): ...
+    def __enterBootloader(self):
         if self.in_bootloader: return
 
-        cmd = self.commands[229]
+        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
         if self.com.reenumerates:
             self.com.close()
@@ -1166,12 +1347,20 @@ class ThreespaceSensor:
         #By then adding a ?UUU, that will trigger a <KEY_ERROR> if in firmware. So, can tell if in bootloader or firmware by checking for OK or <KEY_ERROR>
         bootloader = False
         self.com.write("UUU?UUU\n".encode())
-        response = self.com.read(2)
-        if len(response) == 0: 
-            raise RuntimeError("Failed to discover bootloader or firmware. Is the sensor a 3.0?")
-        if response == b'OK':
+        response = self.__await_get_settings(2, check_bootloader=True)
+        if response == THREESPACE_AWAIT_COMMAND_TIMEOUT: 
+            self.log("Requested Bootloader, Got:")
+            self.log(self.com.peek(self.com.length))
+            raise RuntimeError("Failed to discover bootloader or firmware.")
+        if response == THREESPACE_AWAIT_BOOTLOADER:
             bootloader = True
-        self.com.read_all() #Remove the rest of the OK responses or the rest of the <KEY_ERROR> response
+            time.sleep(0.1) #Give time for all the OK responses to come in
+            self.com.read_all() #Remove the rest of the OK responses or the rest of the <KEY_ERROR> response
+        elif response == THREESPACE_AWAIT_COMMAND_FOUND:
+            bootloader = False
+            self.com.readline() #Clear the setting, no need to parse
+        else:
+            raise Exception("Failed to detect if in bootloader or firmware")
         return bootloader
     
     def bootloader_get_sn(self):
@@ -1191,11 +1380,6 @@ class ThreespaceSensor:
             success = self.__attempt_rediscover_self()
             if not success:
                 raise RuntimeError("Failed to reconnect to sensor in firmware")
-        self.com.read_all() #If debug_mode=1, might be debug messages waiting
-        if self.immediate_debug:
-            print("Waiting longer before booting into firmware because immediate debug was enabled.")
-            time.sleep(2)
-            self.com.read_all()
         in_bootloader = self.__check_bootloader_status()
         if in_bootloader:
             raise RuntimeError("Failed to exit bootloader")
@@ -1249,299 +1433,134 @@ class ThreespaceSensor:
                 self.fileStopStream()
             if self.is_log_streaming:
                 self.stopDataLogging()
-            #self.closeFile() #May not be opened, but also not cacheing that so just attempt to close. Currently commented out because breaks embedded
+
+            #The sensor may or may not have this command registered. So just try it
+            try:
+                #May not be opened, but also not cacheing that so just attempt to close.
+                self.closeFile()
+            except: pass
         self.com.close()
 
 #-------------------------START ALL PROTOTYPES------------------------------------
 
-    def eeptsStart(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")
-
-    def eeptsStop(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")    
-    
-    def eeptsGetOldestStep(self) -> ThreespaceCmdResult[list]:
-        raise NotImplementedError("This method is not available.")  
-
-    def eeptsGetNewestStep(self) -> ThreespaceCmdResult[list]:
-        raise NotImplementedError("This method is not available.")      
-
-    def eeptsGetNumStepsAvailable(self) -> ThreespaceCmdResult[int]:
-        raise NotImplementedError("This method is not available.")       
-
-    def eeptsInsertGPS(self, latitude: float, longitude: float) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")      
-       
-    def eeptsAutoOffset(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")    
-    
-    def getRawGyroRate(self, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def getRawAccelVec(self, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def getRawMagVec(self, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")   
-
-    def getTaredOrientation(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.") 
-
-    def getTaredOrientationAsEulerAngles(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")  
-                                    
-    def getTaredOrientationAsRotationMatrix(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def getTaredOrientationAsAxisAngles(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def getTaredOrientationAsTwoVector(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")  
-    
-    def getDifferenceQuaternion(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")   
-
-    def getUntaredOrientation(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")  
-
-    def getUntaredOrientationAsEulerAngles(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")  
-
-    def getUntaredOrientationAsRotationMatrix(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")  
-
-    def getUntaredOrientationAsAxisAngles(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")  
-
-    def getUntaredOrientationAsTwoVector(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")  
-    
-    def commitSettings(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")    
-
-    def getMotionlessConfidenceFactor(self) -> ThreespaceCmdResult[float]:
-        raise NotImplementedError("This method is not available.")
-    
-    def enableMSC(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")
-
-    def disableMSC(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")    
-    
-    def getNextDirectoryItem(self) -> ThreespaceCmdResult[list[int,str,int]]:    
-        raise NotImplementedError("This method is not available.")
-    
-    def changeDirectory(self, path: str) -> ThreespaceCmdResult[None]:    
-        raise NotImplementedError("This method is not available.")    
-
-    def openFile(self, path: str) -> ThreespaceCmdResult[None]:    
-        raise NotImplementedError("This method is not available.") 
-    
-    def closeFile(self) -> ThreespaceCmdResult[None]:    
-        raise NotImplementedError("This method is not available.")    
-
-    def fileGetRemainingSize(self) -> ThreespaceCmdResult[int]:    
-        raise NotImplementedError("This method is not available.")    
-
-    def fileReadLine(self) -> ThreespaceCmdResult[str]:    
-        raise NotImplementedError("This method is not available.")     
-
-    def fileReadBytes(self, num_bytes: int) -> ThreespaceCmdResult[bytes]:    
+#To actually see how commands work, look at __initialize_commands and __add_command
+#But basically, these are all just prototypes. Information about the commands is in the table
+#beneath here, and the API simply calls its execute_command function on the Command information objects defined.
+
+    def eeptsStart(self) -> ThreespaceCmdResult[None]: ...
+    def eeptsStop(self) -> ThreespaceCmdResult[None]: ...
+    def eeptsGetOldestStep(self) -> ThreespaceCmdResult[list]: ...
+    def eeptsGetNewestStep(self) -> ThreespaceCmdResult[list]: ...   
+    def eeptsGetNumStepsAvailable(self) -> ThreespaceCmdResult[int]: ...    
+    def eeptsInsertGPS(self, latitude: float, longitude: float) -> ThreespaceCmdResult[None]: ...
+    def eeptsAutoOffset(self) -> ThreespaceCmdResult[None]: ... 
+    def getRawGyroRate(self, id: int) -> ThreespaceCmdResult[list[float]]: ...
+    def getRawAccelVec(self, id: int) -> ThreespaceCmdResult[list[float]]: ...
+    def getRawMagVec(self, id: int) -> ThreespaceCmdResult[list[float]]: ...
+    def getTaredOrientation(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getTaredOrientationAsEulerAngles(self) -> ThreespaceCmdResult[list[float]]: ...                        
+    def getTaredOrientationAsRotationMatrix(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getTaredOrientationAsAxisAngles(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getTaredOrientationAsTwoVector(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getDifferenceQuaternion(self) -> ThreespaceCmdResult[list[float]]: ... 
+    def getUntaredOrientation(self) -> ThreespaceCmdResult[list[float]]: ...  
+    def getUntaredOrientationAsEulerAngles(self) -> ThreespaceCmdResult[list[float]]: ... 
+    def getUntaredOrientationAsRotationMatrix(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getUntaredOrientationAsAxisAngles(self) -> ThreespaceCmdResult[list[float]]: ... 
+    def getUntaredOrientationAsTwoVector(self) -> ThreespaceCmdResult[list[float]]: ...
+    def commitSettings(self) -> ThreespaceCmdResult[None]: ...    
+    def getMotionlessConfidenceFactor(self) -> ThreespaceCmdResult[float]: ...
+    def enableMSC(self) -> ThreespaceCmdResult[None]: ...
+    def disableMSC(self) -> ThreespaceCmdResult[None]: ...  
+    def getNextDirectoryItem(self) -> ThreespaceCmdResult[list[int,str,int]]: ...
+    def changeDirectory(self, path: str) -> ThreespaceCmdResult[None]: ...   
+    def openFile(self, path: str) -> ThreespaceCmdResult[None]: ... 
+    def closeFile(self) -> ThreespaceCmdResult[None]: ...  
+    def fileGetRemainingSize(self) -> ThreespaceCmdResult[int]: ...
+    def fileReadLine(self) -> ThreespaceCmdResult[str]: ... 
+    def fileReadBytes(self, num_bytes: int) -> ThreespaceCmdResult[bytes]: ...
+    def __fileReadBytes(self, num_bytes: int) -> ThreespaceCmdResult[bytes]:    
         self.check_dirty()
         cmd = self.commands[THREESPACE_FILE_READ_BYTES_COMMAND_NUM]
         cmd.send_command(self.com, num_bytes, header_enabled=self.header_enabled)
         self.__await_command(cmd)
         if self.header_enabled:
             header = ThreespaceHeader.from_bytes(self.com.read(self.header_info.size), self.header_info)
+            num_bytes = min(num_bytes, header.length) #Its possible for less bytes to be returned when an error occurs (EX: Reading from unopened file)
         else:
             header = ThreespaceHeader()
 
         response = self.com.read(num_bytes)
         return ThreespaceCmdResult(response, header, data_raw_binary=response)
 
-    def deleteFile(self, path: str) -> ThreespaceCmdResult[None]:    
-        raise NotImplementedError("This method is not available.") 
-
-    def getStreamingBatch(self):
-        raise NotImplementedError("This method is not available.")
-
-    def setOffsetWithCurrentOrientation(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")
-    
-    def resetBaseOffset(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")
-
-    def setBaseOffsetWithCurrentOrientation(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")    
-
-    def getTaredTwoVectorInSensorFrame(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")    
-
-    def getUntaredTwoVectorInSensorFrame(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")        
-
-    def getPrimaryBarometerPressure(self) -> ThreespaceCmdResult[float]:
-        raise NotImplementedError("This method is not available.")
-    
-    def getPrimaryBarometerAltitude(self) -> ThreespaceCmdResult[float]:
-        raise NotImplementedError("This method is not available.")    
-
-    def getBarometerAltitude(self, id: int) -> ThreespaceCmdResult[float]:
-        raise NotImplementedError("This method is not available.")   
-    
-    def getBarometerPressure(self, id: int) -> ThreespaceCmdResult[float]:
-        raise NotImplementedError("This method is not available.")      
-
-    def getAllPrimaryNormalizedData(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-    
-    def getPrimaryNormalizedGyroRate(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def getPrimaryNormalizedAccelVec(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def getPrimaryNormalizedMagVec(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-    
-    def getAllPrimaryCorrectedData(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-    
-    def getPrimaryCorrectedGyroRate(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def getPrimaryCorrectedAccelVec(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def getPrimaryCorrectedMagVec(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")    
-    
-    def getPrimaryGlobalLinearAccel(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.") 
-
-    def getPrimaryLocalLinearAccel(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")         
-
-    def getTemperatureCelsius(self) -> ThreespaceCmdResult[float]:
-        raise NotImplementedError("This method is not available.") 
-    
-    def getTemperatureFahrenheit(self) -> ThreespaceCmdResult[float]:
-        raise NotImplementedError("This method is not available.")     
-
-    def getNormalizedGyroRate(self, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-    
-    def getNormalizedAccelVec(self, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def getNormalizedMagVec(self, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")        
-
-    def getCorrectedGyroRate(self, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-    
-    def getCorrectedAccelVec(self, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def getCorrectedMagVec(self, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")        
-
-    def enableMSC(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")  
-    
-    def disableMSC(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")    
-
-    def getTimestamp(self) -> ThreespaceCmdResult[int]:
-        raise NotImplementedError("This method is not available.")  
-
-    def getBatteryVoltage(self) -> ThreespaceCmdResult[float]:
-        raise NotImplementedError("This method is not available.") 
-    
-    def getBatteryPercent(self) -> ThreespaceCmdResult[int]:
-        raise NotImplementedError("This method is not available.") 
-
-    def getBatteryStatus(self) -> ThreespaceCmdResult[int]:
-        raise NotImplementedError("This method is not available.")         
-
-    def getGpsCoord(self) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-    
-    def getGpsAltitude(self) -> ThreespaceCmdResult[float]:
-        raise NotImplementedError("This method is not available.") 
-
-    def getGpsFixState(self) -> ThreespaceCmdResult[int]:
-        raise NotImplementedError("This method is not available.") 
-
-    def getGpsHdop(self) -> ThreespaceCmdResult[float]:
-        raise NotImplementedError("This method is not available.") 
-
-    def getGpsSatellites(self) -> ThreespaceCmdResult[int]:
-        raise NotImplementedError("This method is not available.")                 
-
-    def getButtonState(self) -> ThreespaceCmdResult[int]:
-        raise NotImplementedError("This method is not available.")
-
-    def correctRawGyroData(self, x: float, y: float, z: float, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-    
-    def correctRawAccelData(self, x: float, y: float, z: float, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def correctRawMagData(self, x: float, y: float, z: float, id: int) -> ThreespaceCmdResult[list[float]]:
-        raise NotImplementedError("This method is not available.")
-
-    def formatSd(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")    
-
-    def setDateTime(self, year: int, month: int, day: int, hour: int, minute: int, second: int) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")
-
-    def getDateTime(self) -> ThreespaceCmdResult[list[int]]:
-        raise NotImplementedError("This method is not available.")  
-
-    def tareWithCurrentOrientation(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.") 
-    
-    def setBaseTareWithCurrentOrientation(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")     
-
-    def resetFilter(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")
-    
-    def getNumDebugMessages(self) -> ThreespaceCmdResult[int]:
-        raise NotImplementedError("This method is not available.")
-    
-    def getOldestDebugMessage(self) -> ThreespaceCmdResult[str]:
-        raise NotImplementedError("This method is not available.")
-    
-    def beginPassiveAutoCalibration(self, enabled_bitfield: int) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")
-
-    def getActivePassiveAutoCalibration(self) -> ThreespaceCmdResult[int]:
-        raise NotImplementedError("This method is not available.")
-
-    def beginActiveAutoCalibration(self) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")
-
-    def isActiveAutoCalibrationActive(self) -> ThreespaceCmdResult[int]:
-        raise NotImplementedError("This method is not available.")                
-    
-    def getStreamingLabel(self, cmd_num: int) -> ThreespaceCmdResult[str]:
-        raise NotImplementedError("This method is not available.")   
-
-    def setCursor(self, cursor_index: int) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.") 
-
-    def getLastLogCursorInfo(self) -> ThreespaceCmdResult[tuple[int,str]]:
-        raise NotImplementedError("This method is not available.")   
-
-    def pauseLogStreaming(self, pause: bool) -> ThreespaceCmdResult[None]:
-        raise NotImplementedError("This method is not available.")               
+    def deleteFile(self, path: str) -> ThreespaceCmdResult[None]: ...
+    def getStreamingBatch(self) -> ThreespaceCmdResult[list]: ...
+    def setOffsetWithCurrentOrientation(self) -> ThreespaceCmdResult[None]: ...
+    def resetBaseOffset(self) -> ThreespaceCmdResult[None]: ...
+    def setBaseOffsetWithCurrentOrientation(self) -> ThreespaceCmdResult[None]: ...    
+    def getTaredTwoVectorInSensorFrame(self) -> ThreespaceCmdResult[list[float]]: ...    
+    def getUntaredTwoVectorInSensorFrame(self) -> ThreespaceCmdResult[list[float]]: ...        
+    def getPrimaryBarometerPressure(self) -> ThreespaceCmdResult[float]: ...
+    def getPrimaryBarometerAltitude(self) -> ThreespaceCmdResult[float]: ...    
+    def getBarometerAltitude(self, id: int) -> ThreespaceCmdResult[float]: ...   
+    def getBarometerPressure(self, id: int) -> ThreespaceCmdResult[float]: ...      
+    def getAllPrimaryNormalizedData(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getPrimaryNormalizedGyroRate(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getPrimaryNormalizedAccelVec(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getPrimaryNormalizedMagVec(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getAllPrimaryCorrectedData(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getPrimaryCorrectedGyroRate(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getPrimaryCorrectedAccelVec(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getPrimaryCorrectedMagVec(self) -> ThreespaceCmdResult[list[float]]: ...    
+    def getPrimaryGlobalLinearAccel(self) -> ThreespaceCmdResult[list[float]]: ... 
+    def getPrimaryLocalLinearAccel(self) -> ThreespaceCmdResult[list[float]]: ...         
+    def getTemperatureCelsius(self) -> ThreespaceCmdResult[float]: ... 
+    def getTemperatureFahrenheit(self) -> ThreespaceCmdResult[float]: ...     
+    def getNormalizedGyroRate(self, id: int) -> ThreespaceCmdResult[list[float]]: ...
+    def getNormalizedAccelVec(self, id: int) -> ThreespaceCmdResult[list[float]]: ...
+    def getNormalizedMagVec(self, id: int) -> ThreespaceCmdResult[list[float]]: ...     
+    def getCorrectedGyroRate(self, id: int) -> ThreespaceCmdResult[list[float]]: ...
+    def getCorrectedAccelVec(self, id: int) -> ThreespaceCmdResult[list[float]]: ...
+    def getCorrectedMagVec(self, id: int) -> ThreespaceCmdResult[list[float]]: ...    
+    def enableMSC(self) -> ThreespaceCmdResult[None]: ...
+    def disableMSC(self) -> ThreespaceCmdResult[None]: ...
+    def getTimestamp(self) -> ThreespaceCmdResult[int]: ...
+    def getBatteryVoltage(self) -> ThreespaceCmdResult[float]: ...
+    def getBatteryPercent(self) -> ThreespaceCmdResult[int]: ...
+    def getBatteryStatus(self) -> ThreespaceCmdResult[int]: ...     
+    def getGpsCoord(self) -> ThreespaceCmdResult[list[float]]: ...
+    def getGpsAltitude(self) -> ThreespaceCmdResult[float]: ...
+    def getGpsFixState(self) -> ThreespaceCmdResult[int]: ...
+    def getGpsHdop(self) -> ThreespaceCmdResult[float]: ...
+    def getGpsSatellites(self) -> ThreespaceCmdResult[int]: ...             
+    def getButtonState(self) -> ThreespaceCmdResult[int]: ...
+    def correctRawGyroData(self, x: float, y: float, z: float, id: int) -> ThreespaceCmdResult[list[float]]: ...
+    def correctRawAccelData(self, x: float, y: float, z: float, id: int) -> ThreespaceCmdResult[list[float]]: ...
+    def correctRawMagData(self, x: float, y: float, z: float, id: int) -> ThreespaceCmdResult[list[float]]: ...
+    def formatSd(self) -> ThreespaceCmdResult[None]: ...
+    def setDateTime(self, year: int, month: int, day: int, hour: int, minute: int, second: int) -> ThreespaceCmdResult[None]: ...
+    def getDateTime(self) -> ThreespaceCmdResult[list[int]]: ...
+    def tareWithCurrentOrientation(self) -> ThreespaceCmdResult[None]: ...
+    def setBaseTareWithCurrentOrientation(self) -> ThreespaceCmdResult[None]: ... 
+    def resetFilter(self) -> ThreespaceCmdResult[None]: ...
+    def getNumDebugMessages(self) -> ThreespaceCmdResult[int]: ...
+    def getOldestDebugMessage(self) -> ThreespaceCmdResult[str]: ...
+    def selfTest(self) -> ThreespaceCmdResult[int]: ...
+    def beginPassiveAutoCalibration(self, enabled_bitfield: int) -> ThreespaceCmdResult[None]: ...
+    def getActivePassiveAutoCalibration(self) -> ThreespaceCmdResult[int]: ...
+    def beginActiveAutoCalibration(self) -> ThreespaceCmdResult[None]: ...
+    def isActiveAutoCalibrationActive(self) -> ThreespaceCmdResult[int]: ...            
+    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]: ...           
 
 THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM = 84
+THREESPACE_START_STREAMING_COMMAND_NUM = 85
+THREESPACE_STOP_STREAMING_COMMAND_NUM = 86
 THREESPACE_FILE_READ_BYTES_COMMAND_NUM = 177
+THREESPACE_SOFTWARE_RESET_COMMAND_NUM = 226
+THREESPACE_ENTER_BOOTLOADER_COMMAND_NUM = 229
 
 #Acutal command definitions
 _threespace_commands: list[ThreespaceCommand] = [
@@ -1609,8 +1628,8 @@ _threespace_commands: list[ThreespaceCommand] = [
     ThreespaceCommand("disableMSC", 58, "", ""),
 
     ThreespaceCommand("formatSd", 59, "", ""),
-    ThreespaceCommand("__startDataLogging", 60, "", ""),
-    ThreespaceCommand("__stopDataLogging", 61, "", ""),
+    ThreespaceCommand("startDataLogging", 60, "", "", custom_func=ThreespaceSensor._ThreespaceSensor__startDataLogging),
+    ThreespaceCommand("stopDataLogging", 61, "", "", custom_func=ThreespaceSensor._ThreespaceSensor__stopDataLogging),
 
     ThreespaceCommand("setDateTime", 62, "Bbbbbb", ""),
     ThreespaceCommand("getDateTime", 63, "", "Bbbbbb"),
@@ -1628,9 +1647,9 @@ _threespace_commands: list[ThreespaceCommand] = [
     ThreespaceCommand("eeptsAutoOffset", 74, "", ""),
 
     ThreespaceCommand("getStreamingLabel", 83, "b", "S"),
-    ThreespaceCommand("__getStreamingBatch", THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM, "", "S"),
-    ThreespaceCommand("__startStreaming", 85, "", ""),
-    ThreespaceCommand("__stopStreaming", 86, "", ""),
+    ThreespaceCommand("getStreamingBatch", THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM, "", "S"),
+    ThreespaceCommand("startStreaming", THREESPACE_START_STREAMING_COMMAND_NUM, "", "", custom_func=ThreespaceSensor._ThreespaceSensor__startStreaming),
+    ThreespaceCommand("stopStreaming", THREESPACE_STOP_STREAMING_COMMAND_NUM, "", "", custom_func=ThreespaceSensor._ThreespaceSensor__stopStreaming),
     ThreespaceCommand("pauseLogStreaming", 87, "b", ""),
     
     ThreespaceCommand("getTimestamp", 94, "", "U"),
@@ -1641,6 +1660,7 @@ _threespace_commands: list[ThreespaceCommand] = [
     ThreespaceCommand("resetFilter", 120, "", ""),
     ThreespaceCommand("getNumDebugMessages", 126, "", "B"),
     ThreespaceCommand("getOldestDebugMessage", 127, "", "S"),
+    ThreespaceCommand("selfTest", 128, "", "u"),
 
     ThreespaceCommand("beginPassiveAutoCalibration", 165, "b", ""),
     ThreespaceCommand("getActivePassiveAutoCalibration", 166, "", "b"),
@@ -1654,11 +1674,11 @@ _threespace_commands: list[ThreespaceCommand] = [
     ThreespaceCommand("closeFile", 174, "", ""),
     ThreespaceCommand("fileGetRemainingSize", 175, "", "U"),
     ThreespaceCommand("fileReadLine", 176, "", "S"),
-    ThreespaceCommand("__fileReadBytes", THREESPACE_FILE_READ_BYTES_COMMAND_NUM, "B", "S"), #This has to be handled specially as the output is variable length BYTES not STRING
+    ThreespaceCommand("fileReadBytes", THREESPACE_FILE_READ_BYTES_COMMAND_NUM, "B", "S", custom_func=ThreespaceSensor._ThreespaceSensor__fileReadBytes), #This has to be handled specially as the output is variable length BYTES not STRING
     ThreespaceCommand("deleteFile", 178, "S", ""),
     ThreespaceCommand("setCursor", 179, "U", ""),
-    ThreespaceCommand("__fileStartStream", 180, "", "U"),
-    ThreespaceCommand("__fileStopStream", 181, "", ""),
+    ThreespaceCommand("fileStartStream", 180, "", "U", custom_func=ThreespaceSensor._ThreespaceSensor__fileStartStream),
+    ThreespaceCommand("fileStopStream", 181, "", "", custom_func=ThreespaceSensor._ThreespaceSensor__fileStopStream),
 
     ThreespaceCommand("getBatteryVoltage", 201, "", "f"),
     ThreespaceCommand("getBatteryPercent", 202, "", "b"),
@@ -1671,8 +1691,8 @@ _threespace_commands: list[ThreespaceCommand] = [
     ThreespaceCommand("getGpsSatellites", 219, "", "b"),
 
     ThreespaceCommand("commitSettings", 225, "", ""),
-    ThreespaceCommand("__softwareReset", 226, "", ""),
-    ThreespaceCommand("__enterBootloader", 229, "", ""),
+    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("getButtonState", 250, "", "b"),
 ]
@@ -1683,6 +1703,12 @@ def threespaceCommandGet(cmd_num: int):
             return command
     return None
 
+def threespaceCommandGetByName(name: str):
+    for command in _threespace_commands:
+        if command.info.name == name:
+            return command
+    return None
+
 def threespaceCommandGetInfo(cmd_num: int):
     command = threespaceCommandGet(cmd_num)
     if command is None: return None