diff --git a/airbrakes/constants.py b/airbrakes/constants.py index 14b8e25b..3f4c4028 100644 --- a/airbrakes/constants.py +++ b/airbrakes/constants.py @@ -29,6 +29,9 @@ ESTIMATED_DESCRIPTOR_SET = 130 RAW_DESCRIPTOR_SET = 128 +# The maximum size of the data queue for the packets, so we don't run into memory issues +MAX_QUEUE_SIZE = 100000 + # ------------------------------------------------------- # Orientation Configuration # ------------------------------------------------------- diff --git a/airbrakes/imu/imu.py b/airbrakes/imu/imu.py index 9d2364f8..105ecee6 100644 --- a/airbrakes/imu/imu.py +++ b/airbrakes/imu/imu.py @@ -13,7 +13,7 @@ stacklevel=2, ) -from airbrakes.constants import ESTIMATED_DESCRIPTOR_SET, RAW_DESCRIPTOR_SET +from airbrakes.constants import ESTIMATED_DESCRIPTOR_SET, MAX_QUEUE_SIZE, RAW_DESCRIPTOR_SET from airbrakes.imu.imu_data_packet import EstimatedDataPacket, IMUDataPacket, RawDataPacket @@ -35,8 +35,10 @@ class IMU: ) def __init__(self, port: str, frequency: int, upside_down: bool): - # Shared Queue which contains the latest data from the IMU - self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue() + # Shared Queue which contains the latest data from the IMU. The MAX_QUEUE_SIZE is there + # to prevent memory issues. Realistically, the queue size never exceeds 50 packets when + # it's being logged. + self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue(MAX_QUEUE_SIZE) self._running = multiprocessing.Value("b", False) # Makes a boolean value that is shared between processes # Starts the process that fetches data from the IMU @@ -100,20 +102,19 @@ def _fetch_data_loop(self, port: str, frequency: int, _: bool): channel = data_point.channelName() # This cpp file was the only place I was able to find all the channel names # https://github.com/LORD-MicroStrain/MSCL/blob/master/MSCL/source/mscl/MicroStrain/MIP/MipTypes.cpp - # Check if the imu_data_packet has an attribute with the name of the channel - if hasattr(imu_data_packet, channel): + # Check if the channel name is one we want to save + if hasattr(imu_data_packet, channel) or "Quaternion" in channel: # First checks if the data point needs special handling, if not, just set the attribute match channel: # These specific data points are matrix's rather than doubles case "estAttitudeUncertQuaternion" | "estOrientQuaternion": + # This makes a 4x1 matrix from the data point with the data as [[x], [y], [z], [w]] matrix = data_point.as_Matrix() - # Converts the [4x1] matrix to the X, Y, Z, and W of the quaternion - quaternion_tuple = tuple(matrix[i, 0] for i in range(matrix.rows())) # Sets the X, Y, Z, and W of the quaternion to the data packet object - setattr(imu_data_packet, f"{channel}X", quaternion_tuple[0]) - setattr(imu_data_packet, f"{channel}Y", quaternion_tuple[1]) - setattr(imu_data_packet, f"{channel}Z", quaternion_tuple[2]) - setattr(imu_data_packet, f"{channel}W", quaternion_tuple[3]) + setattr(imu_data_packet, f"{channel}X", matrix.as_floatAt(0, 0)) + setattr(imu_data_packet, f"{channel}Y", matrix.as_floatAt(0, 1)) + setattr(imu_data_packet, f"{channel}Z", matrix.as_floatAt(0, 2)) + setattr(imu_data_packet, f"{channel}W", matrix.as_floatAt(0, 3)) case _: # Because the attribute names in our data packet classes are the same as the channel # names, we can just set the attribute to the value of the data point.