Merge pull request #13 from NCSU-High-Powered-Rocketry-Club/fix-logging

Fix quaternion logging
NCSU-High-Powered-Rocketry-Club · Sep 18, 2024 · cc3574a · cc3574a
2 parents 821d91f + b5df4a8
commit cc3574a
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 11 deletions.
diff --git 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
@@ -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.