yostlabs 2025.1.16__py3-none-any.whl

yostlabs/tss3/consts.py

@@ -0,0 +1,24 @@
+THREESPACE_HEADER_STATUS_BIT_POS = 0
+THREESPACE_HEADER_TIMESTAMP_BIT_POS = 1
+THREESPACE_HEADER_ECHO_BIT_POS = 2
+THREESPACE_HEADER_CHECKSUM_BIT_POS = 3
+THREESPACE_HEADER_SERIAL_BIT_POS = 4
+THREESPACE_HEADER_LENGTH_BIT_POS = 5
+THREESPACE_HEADER_NUM_BITS = 6
+
+THREESPACE_HEADER_STATUS_BIT = (1 << THREESPACE_HEADER_STATUS_BIT_POS)
+THREESPACE_HEADER_TIMESTAMP_BIT = (1 << THREESPACE_HEADER_TIMESTAMP_BIT_POS)
+THREESPACE_HEADER_ECHO_BIT = (1 << THREESPACE_HEADER_ECHO_BIT_POS)
+THREESPACE_HEADER_CHECKSUM_BIT = (1 << THREESPACE_HEADER_CHECKSUM_BIT_POS)
+THREESPACE_HEADER_SERIAL_BIT = (1 << THREESPACE_HEADER_SERIAL_BIT_POS)
+THREESPACE_HEADER_LENGTH_BIT = (1 << THREESPACE_HEADER_LENGTH_BIT_POS)
+
+FIRMWARE_VALID_BIT = 0x01
+
+PASSIVE_CALIBRATE_GYRO = (1 << 0)
+PASSIVE_CALIBRATE_MAG_REF = (1 << 1)
+
+STREAMING_MAX_HZ = 2000
+
+THREESPACE_OUTPUT_MODE_ASCII = 1
+THREESPACE_OUTPUT_MODE_BINARY = 2

yostlabs/tss3/eepts.py

@@ -0,0 +1,228 @@
+from dataclasses import dataclass, field, fields
+from typing import ClassVar
+
+YL_SENSOR_BIT_GYRO: int = 1
+YL_SENSOR_BIT_ACCEL: int = 2
+YL_SENSOR_BIT_MAG: int = 4
+YL_SENSOR_BIT_BARO: int = 8
+YL_SENSOR_BIT_TEMP: int = 16
+YL_SENSOR_BIT_GPS: int = 32
+
+@dataclass
+class YL_EEPTS_INPUT_DATA:
+    gyro_data: list[float] = None  # XYZ in radians/sec. only needs to be float32
+    accel_data: list[float] = None # XYZ in G force units(9.8 m/s^2). only needs to be float32
+    mag_data: list[float] = None  # XYZ in Gauss. only needs to be float32
+    orient_data: list[float] = None # XYZW Optional, may not be here if YL_BUILTIN_ORIENT is false
+    barometer_data: float = 0  # in hPa. only needs to be float32
+    temperature: float = 0  # in degrees C. only needs to be float32
+    gps_longitude: float = 0  # in decimal degrees (positive east, negative west). needs to be float64
+    gps_latitude: float = 0  # in decimal degrees (positive north, negative south). needs to be float64
+    gps_altitude: float = 0  # in meters. only needs to be float32
+    timestamp: int = 0 # in microseconds. uint32_t
+
+#Locomotion Modes
+YL_LOCOMOTION_IDLE: int = 0
+YL_LOCOMOTION_WALKING: int = 1
+YL_LOCOMOTION_JOGGING: int = 2
+YL_LOCOMOTION_RUNNING: int = 3
+YL_LOCOMOTION_CRAWLING: int = 4
+YL_LOCOMOTION_OTHER: int = 6
+YL_LOCOMOTION_UNKNOWN: int = 5
+
+#Sensor Locations
+YL_SENSOR_RSHOULDER: int = 5
+YL_SENSOR_BACK: int = 3
+YL_SENSOR_RHAND: int = 4
+YL_SENSOR_WAIST: int = 2
+YL_SENSOR_CHEST: int = 1
+YL_SENSOR_UNKNOWN: int = 0
+
+LocomotionModes = {
+    YL_LOCOMOTION_IDLE: "Idle",
+    YL_LOCOMOTION_WALKING: "Walking",
+    YL_LOCOMOTION_JOGGING: "Jogging",
+    YL_LOCOMOTION_RUNNING: "Running",
+    YL_LOCOMOTION_CRAWLING: "Crawling",
+    YL_LOCOMOTION_OTHER: "Other",
+    YL_LOCOMOTION_UNKNOWN: "Unknown"
+}
+
+SensorLocations = {
+    YL_SENSOR_RSHOULDER: "RShoulder",
+    YL_SENSOR_BACK: "Back",
+    YL_SENSOR_RHAND: "RHand",
+    YL_SENSOR_WAIST: "Waist",
+    YL_SENSOR_CHEST: "Chest",
+    YL_SENSOR_UNKNOWN: "Unknown"
+}
+
+PresetMotionWR = 0
+PresetMotionWRC = 1
+
+PresetHandDisabled = 0
+PresetHandEnabled = 1
+PresetHandOnly = 2
+
+PresetHeadingDynamic = 0
+PresetHeadingStatic = 1
+
+@dataclass
+class YL_EEPTS_OUTPUT_DATA:
+
+    segment_count: int = 0                  #number of segments (a segment refers to a single step action)
+    timestamp: int = 0                      #in microseconds, set to end of last estimated segment
+    estimated_gps_longitude: float = 0      #DDMM.MMMM (positive east, negative west)
+    estimated_gps_latitude: float = 0       #DDMM.MMMM (positive north, negative south)
+    estimated_gps_altitude: float = 0       #in meters
+    estimated_heading_angle: float = 0      #in degrees (0 north, 90 east, 180 south, 270 west)
+    estimated_distance_travelled: float = 0 #in meters, since last reset (A reset is a start command)
+    estimated_distance_x: float = 0         #in meters since last update, along positive east/negative west (An update is a read)
+    estimated_distance_y: float = 0         #in meters, since last update, along positive north/negative south
+    estimated_distance_z: float = 0         #in meters, since last update, along positive up/negative down
+    estimated_locomotion_mode: int = 0      #0=idle, 1=walking, etc...
+    estimated_receiver_location: int = 0    #0=unknown, 1=chest, etc...
+    last_event_confidence: float = 1
+    overall_confidence: float = 1
+
+    LOCMOTION_DICT: ClassVar[dict[int, str]] = { YL_LOCOMOTION_IDLE : "Idle", YL_LOCOMOTION_WALKING : "Walking", YL_LOCOMOTION_JOGGING : "Jogging", YL_LOCOMOTION_RUNNING : "Running", YL_LOCOMOTION_CRAWLING: "Crawling", YL_LOCOMOTION_OTHER: "Other", YL_LOCOMOTION_UNKNOWN: "Unknown" } 
+    LOCATION_DICT: ClassVar[dict[int, str]] = { YL_SENSOR_UNKNOWN : "Unknown", YL_SENSOR_CHEST : "Chest", YL_SENSOR_WAIST : "Waist", YL_SENSOR_BACK : "Back", YL_SENSOR_RHAND : "RHand", YL_SENSOR_RSHOULDER : "RShoulder" }
+
+    def clone(self, other: "YL_EEPTS_OUTPUT_DATA"):
+        self.segment_count = other.segment_count
+        self.timestamp = other.timestamp
+        self.estimated_gps_longitude = other.estimated_gps_longitude
+        self.estimated_gps_latitude = other.estimated_gps_latitude
+        self.estimated_gps_altitude = other.estimated_gps_altitude
+        self.estimated_heading_angle = other.estimated_heading_angle
+        self.estimated_distance_travelled = other.estimated_distance_travelled
+        self.estimated_distance_x = other.estimated_distance_x
+        self.estimated_distance_y = other.estimated_distance_y
+        self.estimated_distance_z = other.estimated_distance_z
+        self.estimated_locomotion_mode = other.estimated_locomotion_mode
+        self.estimated_receiver_location = other.estimated_receiver_location
+        self.last_event_confidence = other.last_event_confidence
+        self.overall_confidence = other.overall_confidence
+
+    def get_locomotion_string(self):
+        return YL_EEPTS_OUTPUT_DATA.LOCMOTION_DICT[self.estimated_locomotion_mode]
+    
+    def get_location_string(self):
+        return YL_EEPTS_OUTPUT_DATA.LOCATION_DICT[self.estimated_receiver_location]
+
+    def __str__(self):
+        return f"{self.segment_count},{self.timestamp}," \
+               f"{self.estimated_gps_longitude},{self.estimated_gps_latitude}," \
+               f"{self.estimated_gps_altitude},{self.estimated_heading_angle}," \
+               f"{self.estimated_distance_travelled},{self.estimated_distance_x}," \
+               f"{self.estimated_distance_y},{self.estimated_distance_z}," \
+               f"{self.estimated_locomotion_mode},{self.estimated_receiver_location}"
+    
+    def print_fancy(self):
+        print("Segment Count:", self.segment_count)
+        print("Timestamp:", self.timestamp)
+        print("Longitude:", self.estimated_gps_longitude)
+        print("Latitude:", self.estimated_gps_latitude)
+        print("Altitude:", self.estimated_gps_altitude)
+        print("Heading:", self.estimated_heading_angle)
+        print("Distance Travelled:", self.estimated_distance_travelled)
+        print("Delta_X:", self.estimated_distance_x)
+        print("Delta_Y:", self.estimated_distance_y)
+        print("Delta_Z:", self.estimated_distance_z)
+        print("Locomotion:", self.get_locomotion_string())
+        print("Sensor Location:", self.get_location_string())
+        print("Confidence:", self.last_event_confidence)
+        print("Overall Confidence:", self.overall_confidence)
+
+@dataclass
+class SensorData:
+    accel_x : float
+    accel_y : float
+    accel_z : float
+
+    gyro_x : float
+    gyro_y : float
+    gyro_z : float
+
+    mag_x : float
+    mag_y : float
+    mag_z : float
+
+    accel : list[float] = field(init=False, default_factory=list)
+    gyro : list[float] = field(init=False, default_factory=list)
+    mag : list[float] = field(init=False, default_factory=list)
+
+    def __post_init__(self):
+        self.accel = [self.accel_x, self.accel_y, self.accel_z]
+        self.gyro = [self.gyro_x, self.gyro_y, self.gyro_z]
+        self.mag = [self.mag_x, self.mag_y, self.mag_z]
+    
+    def __str__(self):
+        return f"{self.accel_x},{self.accel_y},{self.accel_z}," \
+               f"{self.gyro_x},{self.gyro_y}, {self.gyro_z}," \
+               f"{self.mag_x},{self.mag_y},{self.mag_z}"
+
+    def print_fancy(self):
+        print("Accel:", self.accel)
+        print("Gyro:", self.gyro)
+        print("Mag:", self.mag)
+
+@dataclass
+class DebugMessage:
+    level: int = 0
+    module: int = 0
+    msg: str = ""
+
+    def get_display_str(self):
+        return f"Level: {self.level} Module: {self.module}    {self.msg}"
+
+@dataclass
+class Segment:
+    #Output Structure
+    segment_count: int = 0
+    timestamp: int = 0
+    estimated_gps_longitude: float = 0
+    estimated_gps_latitude: float = 0
+    estimated_gps_altitude: float = 0
+    estimated_heading_angle: float = 0
+    estimated_distance_travelled: float = 0
+    estimated_distance_x: float = 0
+    estimated_distance_y: float = 0
+    estimated_distance_z: float = 0
+    estimated_locomotion_mode: int = 0
+    estimated_receiver_location: int = 0
+    last_event_confidence: float = 0
+    overall_confidence: float = 0
+
+    #Segment Info Structure
+    start_global_index: int = 0
+    end_global_index: int = 0
+    f_start_index_offset: float = 0
+    f_end_index_offset: float = 0
+    len: int = 0
+    f_len: float = 0
+    dir: int = 0
+
+    #Debug Messages:
+    debug_msgs: list[DebugMessage] = field(default_factory=lambda: [])
+    
+    @classmethod
+    def from_only_output_obj(cls, output: YL_EEPTS_OUTPUT_DATA):
+        new_obj = cls()
+        output_fields = [f for f in dir(output) if not f.startswith('__') and not callable(getattr(output, f))]
+        for field in output_fields: #Can NOT just assign. That will cause unintentional problems if ever saving segment info
+            setattr(new_obj, field, getattr(output, field))
+
+        return new_obj
+    
+    def __str__(self):
+        s = ""
+        for field in fields(self):
+            attr = getattr(self, field.name)
+            if field.name == "estimated_locomotion_mode":
+                s += f"{field.name}: {LocomotionModes[attr]}\n"
+            elif field.name == "estimated_receiver_location":
+                s += f"{field.name}: {SensorLocations[attr]}\n"
+            else:
+                s += f"{field.name}: {attr}\n"
+        return s

yostlabs/tss3/utils/calibration.py

@@ -0,0 +1,153 @@
+import yostlabs.math.quaternion as quat
+import yostlabs.math.vector as vec
+
+import numpy as np
+from dataclasses import dataclass
+import copy
+
+class ThreespaceGradientDescentCalibration:
+
+    @dataclass
+    class StageInfo:
+        start_vector: int
+        end_vector: int
+        stage: int
+        scale: float
+
+        count: int = 0
+
+    MAX_SCALE = 1000000000
+    MIN_SCALE = 1
+    STAGES = [
+        StageInfo(0, 6, 0, MAX_SCALE),
+        StageInfo(0, 12, 1, MAX_SCALE),
+        StageInfo(0, 24, 2, MAX_SCALE)
+    ]
+
+    #Note that each entry has a positive and negative vector included in this list
+    CHANGE_VECTORS = [
+        np.array([0,0,0,0,0,0,0,0,0,.0001,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,0,-.0001,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,0,0,.0001,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,0,0,-.0001,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,0,0,0,.0001], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,0,0,0,-.0001], dtype=np.float64), #First 6 only try to change the bias
+        np.array([.001,0,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([-.001,0,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,.001,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,-.001,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,.001,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,-.001,0,0,0], dtype=np.float64), #Next 6 only try to change the scale
+        np.array([0,.0001,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,-.0001,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,.0001,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,-.0001,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,.0001,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,-.0001,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,.0001,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,-.0001,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,.0001,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,-.0001,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,.0001,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,-.0001,0,0,0,0], dtype=np.float64), #Next 12 only try to change the shear
+    ]
+
+    def __init__(self, relative_sensor_orients: list[np.ndarray[float]], no_inverse=False):
+        """
+        Params
+        ------
+        relative_sensor_orients : The orientation of the sensor during which each sample is taken if it was tared as if pointing into the screen. 
+        The inverse of these will be used to calculate where the axes should be located relative to the sensor
+        no_inverse : The relative_sensor_orients will be treated as the sample_rotations
+        """
+        if no_inverse:
+            self.rotation_quats = relative_sensor_orients
+        else:
+            self.rotation_quats = [np.array(quat.quat_inverse(orient)) for orient in relative_sensor_orients]
+
+    def apply_parameters(self, sample: np.ndarray[float], params: np.ndarray[float]):
+        bias = params[9:]
+        scale = params[:9]
+        scale = scale.reshape((3, 3))
+        return scale @ (sample + bias)
+
+    def rate_parameters(self, params: np.ndarray[float], samples: list[np.ndarray[float]], targets: list[np.ndarray[float]]):
+        total_error = 0
+        for i in range(len(samples)):
+            sample = samples[i]
+            target = targets[i]
+
+            sample = self.apply_parameters(sample, params)
+            
+            error = target - sample
+            total_error += vec.vec_len(error)
+        return total_error
+
+    def generate_target_list(self, origin: np.ndarray):
+        targets = []
+        for orient in self.rotation_quats:
+            new_vec = np.array(quat.quat_rotate_vec(orient, origin), dtype=np.float64)
+            targets.append(new_vec)
+        return targets
+
+    def __get_stage(self, stage_number: int):
+        if stage_number >= len(self.STAGES):
+            return None
+        #Always get a shallow copy of the stage so can modify without removing the initial values
+        return copy.copy(self.STAGES[stage_number])
+
+    def calculate(self, samples: list[np.ndarray[float]], origin: np.ndarray[float], verbose=False, max_cycles_per_stage=1000):
+        targets = self.generate_target_list(origin)
+        initial_params = np.array([1,0,0,0,1,0,0,0,1,0,0,0], dtype=np.float64)
+        stage = self.__get_stage(0)
+
+        best_params = initial_params
+        best_rating = self.rate_parameters(best_params, samples, targets)
+        count = 0
+        while True:
+            last_best_rating = best_rating
+            params = best_params
+
+            #Apply all the changes to see if any improve the result
+            for change_index in range(stage.start_vector, stage.end_vector):
+                change_vector = self.CHANGE_VECTORS[change_index]
+                new_params = params + (change_vector * stage.scale)
+                rating = self.rate_parameters(new_params, samples, targets)
+
+                #A better rating, store it
+                if rating < best_rating:
+                    best_params = new_params
+                    best_rating = rating
+            
+            if verbose and count % 100 == 0:
+                print(f"Round {count}: {best_rating=} {stage=}")
+            
+            #Decide if need to go to the next stage or not
+            count += 1
+            stage.count += 1
+            if stage.count >= max_cycles_per_stage:
+                stage = self.__get_stage(stage.stage + 1)
+                if stage is None:
+                    if verbose: print("Done from reaching count limit")
+                    break
+                if verbose: print("Going to next stage from count limit")
+                
+            if best_rating == last_best_rating: #The rating did not improve
+                if stage.scale == self.MIN_SCALE: #Go to the next stage since can't get any better in this stage!
+                    stage = self.__get_stage(stage.stage + 1)
+                    if stage is None:
+                        if verbose: print("Done from exhaustion")
+                        break
+                    if verbose: print("Going to next stage from exhaustion")
+                else:   #Reduce the size of the changes to hopefully get more accurate tuning
+                    stage.scale *= 0.1  
+                    if stage.scale < self.MIN_SCALE:
+                        stage.scale = self.MIN_SCALE
+            else: #Rating got better! To help avoid falling in a local minimum, increase the size of the change to see if that could make it better
+                stage.scale *= 1.1
+        
+        if verbose:
+            print(f"Final Rating: {best_rating}")
+            print(f"Final Params: {best_params}")
+
+        return best_params

yostlabs/tss3/utils/parser.py

@@ -0,0 +1,256 @@
+from yostlabs.tss3.api import *
+import math
+
+class ThreespaceBufferInputStream(ThreespaceInputStream):
+    """
+    Default Input Stream for the binary parser.
+    Ignores timeout since this is only used synchronously for now.
+    """
+
+    def __init__(self):
+        self.buffer = bytearray()
+
+    """Reads specified number of bytes."""
+    def read(self, num_bytes) -> bytes:
+        num_bytes = min(len(self.buffer), num_bytes)
+        result = self.buffer[:num_bytes]
+        del self.buffer[:num_bytes]
+        return result
+    
+    def read_all(self):
+        return self.read(self.length)
+
+    def read_until(self, expected: bytes) -> bytes:
+        if expected not in self.buffer:
+            return self.read_all()
+        
+        length = self.buffer.index(expected) + len(expected)
+        result = self.buffer[:length]
+        del self.buffer[:length]
+        raise result
+
+    """Allows reading without removing the data from the buffer"""
+    def peek(self, num_bytes) -> bytes:
+        num_bytes = min(len(self.buffer), num_bytes)
+        return self.buffer[:num_bytes]
+    
+    def peek_until(self, expected: bytes, max_length=None) -> bytes:
+        if expected in self.buffer: #Read until the expected
+            length = self.buffer.index(expected) + len(expected)
+            if max_length is not None and length > max_length:
+                length = max_length
+            return self.buffer[:length]
+        
+        #There is no expected, so read as far as possible
+        length = len(self.buffer)
+        if max_length is not None:
+            length = min(length, max_length)
+        return self.buffer[:length]
+    
+    def readline(self) -> bytes:
+        return self.read_until(b"\n")
+    
+    def peekline(self, max_length=None) -> bytes:
+        return self.peek_until(b"\n", max_length=max_length)    
+    
+    def insert(self, data: bytes):
+        self.buffer.extend(data)
+
+    @property
+    def length(self) -> int:
+        return len(self.buffer)
+    
+    @property
+    def timeout(self) -> float:
+        raise NotImplementedError()
+
+    @timeout.setter
+    def timeout(self, timeout: float):
+        raise NotImplementedError()    
+
+class ThreespaceBinaryParser:
+    """
+    A class that can be used to parse a stream of binary data
+    that could contain multiple different command responses and validates
+    the responses to handle misalignment/data corruption.
+
+    Requires all expected responses to have the same header enabled.
+
+    The header should contain the cmd_echo, checksum, and data_length fields
+    for full functionality. The lack of any of those fields could limit functionality
+    of the parser.
+
+    If cmd_echo is missing, only one command can be registered with the binary parser as it has no way
+    of knowing what of verifying what the current incoming response is.
+
+    If checksum is missing, data integrity can not be checked.
+
+    If data_length is missing, commands that do not return a static length may cause blocking operations
+    or errors while parsing. (These are planned to be fixed in a future version)
+
+    NOTE: For speed, a custom implementation will be better then this parser class. This parser handles allowing
+    multiple commands as well as data validation and misalignment correction. It also formats the data into the ThreespaceCmdResult
+    response type. The overhead added because of all these additional checks/calculations can add a significant amount of time
+    to processing binary data compared to just reading a known amount of data and instantly unpacking it to a tuple in the desired format.
+    """
+
+    COMMAND_EXCEPTIONS = [84, 177] #getStreamingBatch and fileReadBytes need additional info and so need registered via the 
+
+    def __init__(self, data_stream: ThreespaceInputStream = None, verbose=False):
+        """
+        Parameters
+        ----------
+        data_stream - (optional) The data stream to use with the Binary Parser. If not supplied, will default to a new ThreespaceBufferInputStream
+        """
+        self.data_stream = data_stream
+        self.registered_commands: dict[int,ThreespaceCommand] = {}
+
+        if self.data_stream is None:
+            self.data_stream = ThreespaceBufferInputStream()
+        self.header_info = None
+
+        self.__parsing_header: ThreespaceHeader = None  #Used to optimize preventing reading to much by cacheing the header seperately from the cmd data
+        self.__parsing_command: ThreespaceCommand = None
+        self.__parsing_msg_length: int = None           #Used seperately from the __parsing_header so can handle msg lengths that are static without modifying the header
+
+        self.misaligned = False
+        self.verbose = verbose
+
+    def register_command(self, cmd: int|ThreespaceCommand, **kwargs):
+        """
+        Registers the given cmd number/cmd with the binary parser.
+
+        Some commands may require additional information:
+        stream_slots - list[int] Required when registering command 84 (getStreamingBatch) a list of command numbers that are being streamed.
+        read_size - 'auto' or int Required when registering a command that requires a given length such as fileReadBytes. If 'auto' will use the header length to determine length.
+        """
+        if isinstance(cmd, int):
+            cmd = threespaceCommandGet(cmd)
+            if cmd is None:
+                raise ValueError(f"Invalid Cmd {cmd}")
+            
+        if cmd.info.num in self.registered_commands:
+            return False
+        
+        #These command types are special and need additional info
+        if cmd.info.num == THREESPACE_FILE_READ_BYTES_COMMAND_NUM:
+            if "read_size" not in kwargs:
+                raise ValueError("Missing arguement 'read_size' when registering the fileReadBytes command with the binary parser")
+            raise NotImplementedError("The fileReadBytes command has yet to be implemented for the ThreespaceBinaryParser")
+        elif cmd.info.num == THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM:
+            if "stream_slots" not in kwargs:
+                raise ValueError("Missing arguement 'stream_slots' when registering the getStreamingBatch command with the binary parser")
+            cmd = ThreespaceGetStreamingBatchCommand(kwargs['stream_slots'])
+
+        self.registered_commands[cmd.info.num] = cmd
+        return True
+
+    def unregister_command(self, cmd: int|ThreespaceCommand):
+        if cmd.info.num not in self.registered_commands:
+            return False
+        del self.registered_commands[cmd.info.num]
+        return True
+
+    def set_header(self, header_info: ThreespaceHeaderInfo):
+        self.header_info = header_info
+
+    def insert_data(self, data: bytes):
+        """
+        Add the given data to the default ThreespaceBufferInputStream.
+        This method will raise an exception if used on a different type of InputStream
+        """
+        if not isinstance(self.data_stream, ThreespaceBufferInputStream):
+            raise Exception("Insert data with Binary Parser only valid when using the default data_stream")
+        self.data_stream.insert(data)
+    
+    def parse_message(self) -> ThreespaceCmdResult:
+        if self.__parsing_header is None:
+            self.__parse_header()
+        
+        if self.__parsing_command is None:
+            return None
+        
+        return self.__parse_command()
+
+    def __parse_header(self):
+        if self.data_stream.length < self.header_info.size:
+            return
+        
+        header = self.data_stream.peek(self.header_info.size)
+        header = ThreespaceHeader.from_bytes(header, self.header_info)
+
+        cmd_found = False
+        if self.header_info.echo_enabled: #Search for the command to parse
+            for command in self.registered_commands.values():
+                if header.echo != command.info.num: continue
+
+                #Command matches! Attempt to parse
+                self.__parsing_command = command
+                self.__parsing_header = header
+
+                cmd_found = True
+        else: #Can only parse one command
+            if len(self.registered_commands) > 1:
+                raise Exception("Only one command type can be parsed when the 'cmd echo' is not enabled in the header")
+            self.__parsing_command = list(self.registered_commands.values())[0]
+            self.__parsing_header = header
+            cmd_found = True
+
+        if cmd_found:
+            if self.header_info.length_enabled:
+                self.__parsing_msg_length = self.__parsing_header.length
+            else:
+                self.__parsing_msg_length = self.__parsing_command.info.out_size
+            return
+
+        #This header is not related to any command, so it needs skipped
+        if self.verbose and not self.misaligned:
+            print("Unexpected header:", header)
+        self.misaligned = True
+        self.data_stream.read(1)
+
+    def __peek_checksum(self):
+        header_len = len(self.__parsing_header.raw_binary)
+        data = self.data_stream.peek(header_len + self.__parsing_msg_length)[header_len:]
+        checksum = sum(data) % 256
+        return checksum == self.__parsing_header.checksum
+
+    def __parse_command(self):
+        #Not enough data to parse yet
+        if self.data_stream.length < self.header_info.size + self.__parsing_msg_length:
+            return None
+        
+        if self.header_info.checksum_enabled and not math.isnan(self.__parsing_msg_length): #Can validate checksum before parsing
+            print("Pre validating checksum")
+            if not self.__peek_checksum():
+                #Data corruption/Misalignment error
+                if self.verbose and not self.misaligned:
+                    print("Checksum mismatch for command", self.__parsing_command.info.num)
+                self.misaligned = True
+                self.data_stream.read(1)
+                self.__parsing_command = None
+                self.__parsing_header = None
+                return None
+        
+        #Header and pre validation checksum checks out! Now just parse the actual command result and return it
+        header = self.__parsing_header
+        self.data_stream.read(len(header.raw_binary)) #Skip these bytes since they are already parsed
+        result, raw = self.__parsing_command.read_command(self.data_stream)
+
+        #Validate checksum if couldn't pre-validate due to unknown message length
+        if math.isnan(self.__parsing_msg_length) and self.header_info.checksum_enabled:
+            checksum = sum(raw) % 256
+            if checksum != header.checksum:
+                if self.verbose and not self.misaligned:
+                    print("Checksum mismatch for command", self.__parsing_command.info.num)
+                self.misaligned = True
+                self.__parsing_command = None
+                self.__parsing_header = None
+                return None
+
+        #Reset and return
+        self.__parsing_header = None
+        self.__parsing_command = None
+        self.misaligned = False
+        return ThreespaceCmdResult(result, header, data_raw_binary=raw)
+