From 44d1518eac55935a33d3697a0118b4a3abfb19e7 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 26 Oct 2024 20:25:11 -0400 Subject: [PATCH 01/14] Did some more clean up for data_processor --- airbrakes/data_handling/data_processor.py | 145 ++++++++++++---------- 1 file changed, 78 insertions(+), 67 deletions(-) diff --git a/airbrakes/data_handling/data_processor.py b/airbrakes/data_handling/data_processor.py index 92cdb360..ef0b949d 100644 --- a/airbrakes/data_handling/data_processor.py +++ b/airbrakes/data_handling/data_processor.py @@ -21,13 +21,13 @@ class IMUDataProcessor: __slots__ = ( "_current_altitudes", "_current_orientation_quaternions", - "_data_points", - "_first_data_point", + "_data_packets", + "_first_data_packet", "_gravity_direction", "_gravity_orientation", "_gravity_upwards_index", "_initial_altitude", - "_last_data_point", + "_last_data_packet", "_max_altitude", "_max_vertical_velocity", "_previous_vertical_velocity", @@ -51,11 +51,11 @@ def __init__(self): self._previous_vertical_velocity: np.float64 = np.float64(0.0) self._initial_altitude: np.float64 | None = None self._current_altitudes: npt.NDArray[np.float64] = np.array([0.0], dtype=np.float64) - self._last_data_point: EstimatedDataPacket | None = None - self._first_data_point: EstimatedDataPacket | None = None + self._last_data_packet: EstimatedDataPacket | None = None + self._first_data_packet: EstimatedDataPacket | None = None self._current_orientation_quaternions: npt.NDArray[np.float64] | None = None self._rotated_accelerations: list[npt.NDArray[np.float64]] = [np.array([0.0]), np.array([0.0]), np.array([0.0])] - self._data_points: list[EstimatedDataPacket] = [] + self._data_packets: list[EstimatedDataPacket] = [] self._time_differences: npt.NDArray[np.float64] = np.array([0.0]) self._gravity_orientation: npt.NDArray[np.float64] | None = None self._gravity_upwards_index: int | None = 0 @@ -93,56 +93,25 @@ def max_vertical_velocity(self) -> float: """The maximum vertical velocity the rocket has attained during the flight, in m/s.""" return float(self._max_vertical_velocity) - def update(self, data_points: list[EstimatedDataPacket]) -> None: + def update(self, data_packets: list[EstimatedDataPacket]) -> None: """ Updates the data points to process. This will recompute all information such as altitude, velocity, etc. - :param data_points: A list of EstimatedDataPacket objects to process + :param data_packets: A list of EstimatedDataPacket objects to process """ # If the data points are empty, we don't want to try to process anything - if not data_points: + if not data_packets: return - self._data_points = data_points + self._data_packets = data_packets # If we don't have a last data point, we can't calculate the time differences needed # for velocity calculation: - if self._last_data_point is None: - # setting last data point as the first element, makes it so that the time diff + if self._last_data_packet is None: + # Setting last data point as the first element, makes it so that the time diff # automatically becomes 0, and the velocity becomes 0 - self._last_data_point = self._data_points[0] - # This is us getting the rocket's initial orientation - - self._current_orientation_quaternions = np.array( - [ - self._last_data_point.estOrientQuaternionW, - self._last_data_point.estOrientQuaternionX, - self._last_data_point.estOrientQuaternionY, - self._last_data_point.estOrientQuaternionZ, - ] - ) - - # We also get the initial gravity vector to determine which direction is up - # Important to note that when the compensated acceleration reads -9.8 when on the - # ground, the upwards direction of the gravity vector will be positive, not negative - gravity_orientation = np.array( - [ - self._last_data_point.estGravityVectorX, - self._last_data_point.estGravityVectorY, - self._last_data_point.estGravityVectorZ, - ] - ) - - # Gets the index for the direction (x, y, or z) that is pointing upwards - self._gravity_upwards_index = np.argmax(np.abs(gravity_orientation)) - - # on the physical IMU there is a depiction of the orientation. If a negative direction is - # pointing to the sky, by convention, we define the gravity direction as negative. Otherwise if - # a positive direction is pointing to the sky, we define the gravity direction as positive. - # For purposes of standardizing the sign of accelerations that come out of calculate_rotated_accelerations, - # we define acceleration from motor burn as positve, acceleration due to drag as negative, and - # acceleration on the ground to be +9.8. - self._gravity_direction = 1 if gravity_orientation[self._gravity_upwards_index] < 0 else -1 + self._last_data_packet = self._data_packets[0] + self._set_up() self._time_differences = self._calculate_time_differences() @@ -154,7 +123,7 @@ def update(self, data_points: list[EstimatedDataPacket]) -> None: self._max_altitude = max(self._current_altitudes.max(), self._max_altitude) # Store the last data point for the next update - self._last_data_point = data_points[-1] + self._last_data_packet = data_packets[-1] def get_processed_data_packets(self) -> deque[ProcessedDataPacket]: """ @@ -169,13 +138,13 @@ def get_processed_data_packets(self) -> deque[ProcessedDataPacket]: current_altitude=current_alt, vertical_velocity=vertical_velocity, vertical_acceleration=vertical_acceleration, - time_since_last_data_point=time_since_last_data_point, + time_since_last_data_packet=time_since_last_data_packet, ) for ( current_alt, vertical_velocity, vertical_acceleration, - time_since_last_data_point, + time_since_last_data_packet, ) in zip( self._current_altitudes, self._vertical_velocities, @@ -186,7 +155,9 @@ def get_processed_data_packets(self) -> deque[ProcessedDataPacket]: ) @staticmethod - def _multiply_quaternions(first_quaternion: npt.NDArray[np.float64], second_quaternion: npt.NDArray[np.float64]): + def _multiply_quaternions( + first_quaternion: npt.NDArray[np.float64], second_quaternion: npt.NDArray[np.float64] + ) -> npt.NDArray[np.float64]: """ Calculates the quaternion multiplication. quaternion multiplication is not commutative, e.g. q1q2 =/= q2q1 :param first_quaternion: numpy array with the first quaternion in row form @@ -203,7 +174,7 @@ def _multiply_quaternions(first_quaternion: npt.NDArray[np.float64], second_quat return np.array([w, x, y, z]) @staticmethod - def _calculate_quaternion_conjugate(quaternion: npt.NDArray[np.float64]): + def _calculate_quaternion_conjugate(quaternion: npt.NDArray[np.float64]) -> npt.NDArray[np.float64]: """ Calculates the conjugate of a quaternion :param quaternion: numpy array with a quaternion in row form @@ -212,16 +183,56 @@ def _calculate_quaternion_conjugate(quaternion: npt.NDArray[np.float64]): w, x, y, z = quaternion return np.array([w, -x, -y, -z]) + def _set_up(self) -> None: + """ + Sets up the initial values for the data processor. This includes setting the initial + altitude, the initial orientation of the rocket, and the initial gravity vector. This should + only be called once, when the first data packets are passed in. + """ + # This is us getting the rocket's initial altitude from the mean of the first data packets + self._initial_altitude = np.mean( + np.array([data_packet.estPressureAlt for data_packet in self._data_packets], dtype=np.float64) + ) + + # This is us getting the rocket's initial orientation + self._current_orientation_quaternions = np.array( + [ + self._last_data_packet.estOrientQuaternionW, + self._last_data_packet.estOrientQuaternionX, + self._last_data_packet.estOrientQuaternionY, + self._last_data_packet.estOrientQuaternionZ, + ] + ) + + # We also get the initial gravity vector to determine which direction is up + # Important to note that when the compensated acceleration reads -9.8 when on the + # ground, the upwards direction of the gravity vector will be positive, not negative + gravity_orientation = np.array( + [ + self._last_data_packet.estGravityVectorX, + self._last_data_packet.estGravityVectorY, + self._last_data_packet.estGravityVectorZ, + ] + ) + + # Gets the index for the direction (x, y, or z) that is pointing upwards + self._gravity_upwards_index = np.argmax(np.abs(gravity_orientation)) + + # on the physical IMU there is a depiction of the orientation. If a negative direction is + # pointing to the sky, by convention, we define the gravity direction as negative. Otherwise, if + # a positive direction is pointing to the sky, we define the gravity direction as positive. + # For purposes of standardizing the sign of accelerations that come out of calculate_rotated_accelerations, + # we define acceleration from motor burn as positive, acceleration due to drag as negative, and + # acceleration on the ground to be +9.8. + self._gravity_direction = 1 if gravity_orientation[self._gravity_upwards_index] < 0 else -1 + def _calculate_current_altitudes(self) -> npt.NDArray[np.float64]: """ Calculates the current altitudes, by zeroing out the initial altitude. :return: A numpy array of the current altitudes of the rocket at each data point """ # Get the pressure altitudes from the data points - altitudes = np.array([data_point.estPressureAlt for data_point in self._data_points], dtype=np.float64) - # Zero the altitude only once, during the first update: - if self._initial_altitude is None: - self._initial_altitude = np.mean(altitudes) + altitudes = np.array([data_packet.estPressureAlt for data_packet in self._data_packets], dtype=np.float64) # Zero out the initial altitude return altitudes - self._initial_altitude @@ -235,23 +246,23 @@ def _calculate_rotated_accelerations(self) -> list[npt.NDArray[np.float64]]: """ # We pre-allocate the space for our accelerations first - len_data_points = len(self._data_points) + len_data_packets = len(self._data_packets) rotated_accelerations = [ - np.zeros(len_data_points), - np.zeros(len_data_points), - np.zeros(len_data_points), + np.zeros(len_data_packets), + np.zeros(len_data_packets), + np.zeros(len_data_packets), ] # Iterates through the data points and time differences between the data points - for idx, (data_point, dt) in enumerate(zip(self._data_points, self._time_differences, strict=True)): + for idx, (data_packet, dt) in enumerate(zip(self._data_packets, self._time_differences, strict=True)): # Accelerations are in m/s^2 - x_accel = data_point.estCompensatedAccelX - y_accel = data_point.estCompensatedAccelY - z_accel = data_point.estCompensatedAccelZ + x_accel = data_packet.estCompensatedAccelX + y_accel = data_packet.estCompensatedAccelY + z_accel = data_packet.estCompensatedAccelZ # Angular rates are in rads/s - gyro_x = data_point.estAngularRateX - gyro_y = data_point.estAngularRateY - gyro_z = data_point.estAngularRateZ + gyro_x = data_packet.estAngularRateX + gyro_y = data_packet.estAngularRateY + gyro_z = data_packet.estAngularRateZ # If we are missing the data points, then say we didn't rotate if not any([x_accel, y_accel, z_accel, gyro_x, gyro_y, gyro_z]): @@ -318,11 +329,11 @@ def _calculate_vertical_velocity(self) -> npt.NDArray[np.float64]: def _calculate_time_differences(self) -> npt.NDArray[np.float64]: """ Calculates the time difference between each data point and the previous data point. This cannot - be called on the first update as _last_data_point is None. + be called on the first update as _last_data_packet is None. :return: A numpy array of the time difference between each data point and the previous data point. """ # calculate the time differences between each data point # We are converting from ns to s, since we don't want to have a velocity in m/ns^2 # We are using the last data point to calculate the time difference between the last data point from the # previous loop, and the first data point from the current loop - return np.diff([data_point.timestamp for data_point in [self._last_data_point, *self._data_points]]) * 1e-9 + return np.diff([data_packet.timestamp for data_packet in [self._last_data_packet, *self._data_packets]]) * 1e-9 From 8968d8a2899797af0cde2fd5e86ae7479131feae Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 26 Oct 2024 20:48:23 -0400 Subject: [PATCH 02/14] Added some tests --- tests/test_data_processor.py | 57 ++++++++++++++++++++++++++++++++---- 1 file changed, 52 insertions(+), 5 deletions(-) diff --git a/tests/test_data_processor.py b/tests/test_data_processor.py index 7caa8684..95f104ee 100644 --- a/tests/test_data_processor.py +++ b/tests/test_data_processor.py @@ -153,7 +153,7 @@ def test_first_update_no_data_packets(self, data_processor): d = data_processor d.update([]) assert d._last_data_point is None - assert len(d._data_points) == 0 + assert len(d._data_packets) == 0 assert len(d._current_altitudes) == 1 assert len(d._vertical_velocities) == 1 assert d.vertical_velocity == 0.0, "velocity should be the same as set in __init__" @@ -218,7 +218,7 @@ def test_first_update(self, data_processor, data_packets, init_alt, max_alt): # We should always have a last data point assert d._last_data_point assert d._last_data_point == data_packets[-1] - assert len(d._data_points) == len(data_packets) + assert len(d._data_packets) == len(data_packets) # the max() is there because if we only process one data packet, we just return early # and the variables set at __init__ are used: @@ -369,13 +369,60 @@ def test_max_altitude(self, data_processor): ], ) def test_calculate_rotations(self, data_packets, expected_value): - d = IMUDataProcessor([]) - d.update_data(data_packets) + d = IMUDataProcessor() + d.update(data_packets) rotations = d._rotated_accelerations assert len(rotations) == 3 for rot, expected_val in zip(rotations, expected_value, strict=False): assert rot == pytest.approx(expected_val) + def test_initial_orientation(self): + """Tests whether the initial orientation of the rocket is correctly calculated""" + d = IMUDataProcessor() + d.update( + [ + EstimatedDataPacket( + 1 * 1e9, + estLinearAccelX=1, + estLinearAccelY=2, + estLinearAccelZ=3, + estPressureAlt=1, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0.1, + estGravityVectorY=-0.6, + estGravityVectorZ=9.8, + ), + ] + ) + + assert d._current_orientation_quaternions == np.array([0.1, 0.2, 0.3, 0.4]) + assert d._gravity_upwards_index == 2 + + d = IMUDataProcessor() + d.update( + [ + EstimatedDataPacket( + 1 * 1e9, + estLinearAccelX=1, + estLinearAccelY=2, + estLinearAccelZ=3, + estPressureAlt=1, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=-9.6, + estGravityVectorY=-0.6, + estGravityVectorZ=0.5, + ), + ] + ) + + assert d._gravity_upwards_index == 0 + """ This is unit tests for apogee prediction. Does not work currently, will most likely be moved to @@ -444,7 +491,7 @@ def test_calculate_rotations(self, data_packets, expected_value): ], ) def test_apogee_pred(self, data_packets, set_state, expected_values): - d = IMUDataProcessor([]) + d = IMUDataProcessor() d.update_data([data_packets[0], data_packets[1]]) ap_pred = ApogeePredictor(set_state, d, []) ap_pred.update([data_packets[0], data_packets[1]]) From 93908c75340eff5686514d6232b5d751d557a0c4 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 26 Oct 2024 21:02:24 -0400 Subject: [PATCH 03/14] Added np testing --- tests/test_data_processor.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tests/test_data_processor.py b/tests/test_data_processor.py index 95f104ee..20393f02 100644 --- a/tests/test_data_processor.py +++ b/tests/test_data_processor.py @@ -2,6 +2,7 @@ import random import numpy as np +import numpy.testing as npt import pytest from airbrakes.data_handling.data_processor import IMUDataProcessor @@ -398,7 +399,7 @@ def test_initial_orientation(self): ] ) - assert d._current_orientation_quaternions == np.array([0.1, 0.2, 0.3, 0.4]) + npt.assert_array_equal(d._current_orientation_quaternions, np.array([0.1, 0.2, 0.3, 0.4])) assert d._gravity_upwards_index == 2 d = IMUDataProcessor() From 6e40e4661a8946af496dead4c170747ec07e959f Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 26 Oct 2024 21:18:58 -0400 Subject: [PATCH 04/14] Fixed a bug --- airbrakes/data_handling/processed_data_packet.py | 2 +- tests/test_data_processor.py | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/airbrakes/data_handling/processed_data_packet.py b/airbrakes/data_handling/processed_data_packet.py index e5f1eda9..985228d0 100644 --- a/airbrakes/data_handling/processed_data_packet.py +++ b/airbrakes/data_handling/processed_data_packet.py @@ -13,4 +13,4 @@ class ProcessedDataPacket(msgspec.Struct): current_altitude: np.float64 # This is the zeroed-out altitude of the rocket. vertical_velocity: np.float64 # This is the velocity of the rocket, in the upward axis (whichever way is up) vertical_acceleration: np.float64 # This is the rotated compensated acceleration of the vertical axis - time_since_last_data_point: np.float64 # dt is the time difference between the current and previous data point + time_since_last_data_packet: np.float64 # dt is the time difference between the current and previous data point diff --git a/tests/test_data_processor.py b/tests/test_data_processor.py index 20393f02..2f32d166 100644 --- a/tests/test_data_processor.py +++ b/tests/test_data_processor.py @@ -401,6 +401,7 @@ def test_initial_orientation(self): npt.assert_array_equal(d._current_orientation_quaternions, np.array([0.1, 0.2, 0.3, 0.4])) assert d._gravity_upwards_index == 2 + assert d._gravity_direction == -1 d = IMUDataProcessor() d.update( @@ -423,6 +424,7 @@ def test_initial_orientation(self): ) assert d._gravity_upwards_index == 0 + assert d._gravity_direction == 1 """ From bf084b561136ad0de73e2db917db5bb421d8afc4 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 26 Oct 2024 23:01:04 -0400 Subject: [PATCH 05/14] Fixed incorrect checking for None --- airbrakes/data_handling/data_processor.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/airbrakes/data_handling/data_processor.py b/airbrakes/data_handling/data_processor.py index ef0b949d..0fda8814 100644 --- a/airbrakes/data_handling/data_processor.py +++ b/airbrakes/data_handling/data_processor.py @@ -34,7 +34,6 @@ class IMUDataProcessor: "_rotated_accelerations", "_time_differences", "_vertical_velocities", - "upside_down", ) def __init__(self): @@ -108,9 +107,6 @@ def update(self, data_packets: list[EstimatedDataPacket]) -> None: # If we don't have a last data point, we can't calculate the time differences needed # for velocity calculation: if self._last_data_packet is None: - # Setting last data point as the first element, makes it so that the time diff - # automatically becomes 0, and the velocity becomes 0 - self._last_data_packet = self._data_packets[0] self._set_up() self._time_differences = self._calculate_time_differences() @@ -189,6 +185,10 @@ def _set_up(self) -> None: altitude, the initial orientation of the rocket, and the initial gravity vector. This should only be called once, when the first data packets are passed in. """ + # Setting last data point as the first element, makes it so that the time diff + # automatically becomes 0, and the velocity becomes 0 + self._last_data_packet = self._data_packets[0] + # This is us getting the rocket's initial altitude from the mean of the first data packets self._initial_altitude = np.mean( np.array([data_packet.estPressureAlt for data_packet in self._data_packets], dtype=np.float64) @@ -265,7 +265,7 @@ def _calculate_rotated_accelerations(self) -> list[npt.NDArray[np.float64]]: gyro_z = data_packet.estAngularRateZ # If we are missing the data points, then say we didn't rotate - if not any([x_accel, y_accel, z_accel, gyro_x, gyro_y, gyro_z]): + if any(val is None for val in [x_accel, y_accel, z_accel, gyro_x, gyro_y, gyro_z]): return rotated_accelerations # rotation matrix for rate of change quaternion, with epsilon and K used to drive the norm to 1 From 209647469322acb37ae023ea1d43273f0951eb29 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 26 Oct 2024 23:57:02 -0400 Subject: [PATCH 06/14] Got tests passing for data_processor --- airbrakes/data_handling/data_processor.py | 2 +- tests/test_data_processor.py | 331 +++++++++++++++------- 2 files changed, 222 insertions(+), 111 deletions(-) diff --git a/airbrakes/data_handling/data_processor.py b/airbrakes/data_handling/data_processor.py index 0fda8814..c94039f0 100644 --- a/airbrakes/data_handling/data_processor.py +++ b/airbrakes/data_handling/data_processor.py @@ -264,7 +264,7 @@ def _calculate_rotated_accelerations(self) -> list[npt.NDArray[np.float64]]: gyro_y = data_packet.estAngularRateY gyro_z = data_packet.estAngularRateZ - # If we are missing the data points, then say we didn't rotate + # If we are missing the data points, then say accelerations are zero if any(val is None for val in [x_accel, y_accel, z_accel, gyro_x, gyro_y, gyro_z]): return rotated_accelerations diff --git a/tests/test_data_processor.py b/tests/test_data_processor.py index 2f32d166..737d984c 100644 --- a/tests/test_data_processor.py +++ b/tests/test_data_processor.py @@ -41,8 +41,34 @@ class TestIMUDataProcessor: """Tests the IMUDataProcessor class""" packets = [ - EstimatedDataPacket(1 * 1e9, estLinearAccelX=1, estLinearAccelY=2, estLinearAccelZ=3, estPressureAlt=20), - EstimatedDataPacket(2 * 1e9, estLinearAccelX=2, estLinearAccelY=3, estLinearAccelZ=4, estPressureAlt=21), + EstimatedDataPacket( + 1 * 1e9, + estLinearAccelX=1, + estLinearAccelY=2, + estLinearAccelZ=3, + estPressureAlt=20, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=9.8, + ), + EstimatedDataPacket( + 2 * 1e9, + estLinearAccelX=2, + estLinearAccelY=3, + estLinearAccelZ=4, + estPressureAlt=21, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=9.8, + ), ] def test_slots(self): @@ -56,29 +82,20 @@ def test_slots(self): def test_init(self, data_processor): d = data_processor assert d._max_altitude == 0.0 - assert isinstance(d._previous_velocity, np.ndarray) - assert (d._previous_velocity == np.array([0.0, 0.0, 0.0])).all() + assert isinstance(d._previous_vertical_velocity, np.float64) + assert d._previous_vertical_velocity == 0.0 assert d._initial_altitude is None assert isinstance(d._current_altitudes, np.ndarray) assert isinstance(d._vertical_velocities, np.ndarray) assert list(d._vertical_velocities) == [0.0] assert d._max_vertical_velocity == 0.0 - assert d.upside_down is False assert d.current_altitude == 0.0 assert list(d._current_altitudes) == [0.0] # See the comment in _calculate_velocitys() for why velocity is 0 during init. assert d.vertical_velocity == 0.0 def test_str(self, data_processor): - data_str = ( - "IMUDataProcessor(" - "max_altitude=0.0, " - "current_altitude=0.0, " - # See the comment in _calculate_velocitys() for why velocity is 0 during init. - "velocity=0.0, " - "max_velocity=0.0, " - "rotated_accel=[0. 0. 0.],)" - ) + data_str = "IMUDataProcessor(max_altitude=0.0, current_altitude=0.0, velocity=0.0, " "max_velocity=0.0, " assert str(data_processor) == data_str def test_calculate_velocity(self, data_processor): @@ -86,74 +103,100 @@ def test_calculate_velocity(self, data_processor): d = data_processor assert d.vertical_velocity == 0.0 assert d._max_vertical_velocity == d.vertical_velocity - assert (d._previous_velocity == np.array([0.0, 0.0, 0.0])).all() - - d._last_data_point = EstimatedDataPacket( - 2 * 1e9, estLinearAccelX=2, estLinearAccelY=3, estLinearAccelZ=4, estPressureAlt=21 - ) + assert d._previous_vertical_velocity == 0 d.update( [ EstimatedDataPacket( - 3 * 1e9, estLinearAccelX=3, estLinearAccelY=4, estLinearAccelZ=5, estPressureAlt=22 + 2 * 1e9, + estCompensatedAccelX=0, + estCompensatedAccelY=0, + estCompensatedAccelZ=10, + estPressureAlt=21, + estOrientQuaternionW=0.08, + estOrientQuaternionX=0.0, + estOrientQuaternionY=0.0, + estOrientQuaternionZ=-.5, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=-9.8, + estAngularRateX=0.01, + estAngularRateY=0.02, + estAngularRateZ=0.03, ), EstimatedDataPacket( - 4 * 1e9, estLinearAccelX=4, estLinearAccelY=5, estLinearAccelZ=6, estPressureAlt=23 + 3 * 1e9, + estCompensatedAccelX=0, + estCompensatedAccelY=0, + estCompensatedAccelZ=15, + estPressureAlt=22, + estAngularRateX=0.01, + estAngularRateY=0.02, + estAngularRateZ=0.03, ), - ] - ) - # we use pytest.approx() because of floating point errors - assert d._previous_velocity == pytest.approx((7.0, 9.0, 11.0)) - assert d.vertical_velocity == math.sqrt(7.0**2 + 9.0**2 + 11.0**2) - assert len(d._vertical_velocities) == 2 - assert d._max_vertical_velocity == d.vertical_velocity - - d.update( - [ EstimatedDataPacket( - 5 * 1e9, estLinearAccelX=5, estLinearAccelY=6, estLinearAccelZ=7, estPressureAlt=24 - ), - EstimatedDataPacket( - 6 * 1e9, estLinearAccelX=6, estLinearAccelY=7, estLinearAccelZ=8, estPressureAlt=25 - ), - EstimatedDataPacket( - 7 * 1e9, estLinearAccelX=7, estLinearAccelY=8, estLinearAccelZ=9, estPressureAlt=26 + 4 * 1e9, + estCompensatedAccelX=0, + estCompensatedAccelY=0, + estCompensatedAccelZ=20, + estPressureAlt=23, + estAngularRateX=0.01, + estAngularRateY=0.02, + estAngularRateZ=0.03, ), ] ) - assert d._previous_velocity == pytest.approx((25.0, 30.0, 35.0)) - assert d.vertical_velocity == pytest.approx(math.sqrt(25.0**2 + 30.0**2 + 35.0**2)) + # we use pytest.approx() because of floating point errors + assert d._previous_vertical_velocity == pytest.approx(15.38026) assert len(d._vertical_velocities) == 3 assert d._max_vertical_velocity == d.vertical_velocity + # This tests that we are now falling (the accel is less than 9.8) d.update( [ EstimatedDataPacket( - 8 * 1e9, estLinearAccelX=2, estLinearAccelY=2, estLinearAccelZ=3, estPressureAlt=27 - ), - EstimatedDataPacket( - 9 * 1e9, estLinearAccelX=1, estLinearAccelY=-2, estLinearAccelZ=-1, estPressureAlt=28 + 5 * 1e9, + estCompensatedAccelX=0, + estCompensatedAccelY=0, + estCompensatedAccelZ=30, + estPressureAlt=24, + estAngularRateX=0.01, + estAngularRateY=0.02, + estAngularRateZ=0.03, ), EstimatedDataPacket( - 10 * 1e9, estLinearAccelX=-1, estLinearAccelY=-4, estLinearAccelZ=-5, estPressureAlt=29 + 6 * 1e9, + estCompensatedAccelX=6, + estCompensatedAccelY=7, + estCompensatedAccelZ=8, + estPressureAlt=25, + estAngularRateX=0.01, + estAngularRateY=0.02, + estAngularRateZ=0.03, ), EstimatedDataPacket( - 11 * 1e9, estLinearAccelX=-1, estLinearAccelY=-1, estLinearAccelZ=-1, estPressureAlt=30 + 7 * 1e9, + estCompensatedAccelX=0, + estCompensatedAccelY=0, + estCompensatedAccelZ=5, + estPressureAlt=26, + estAngularRateX=0.01, + estAngularRateY=0.02, + estAngularRateZ=0.03, ), ] ) - - assert d._previous_velocity == pytest.approx((26.0, 25.0, 31.0)) - assert d.vertical_velocity == pytest.approx(math.sqrt(26.0**2 + 25.0**2 + 31.0**2)) + assert d._previous_vertical_velocity == pytest.approx(28.70443) + assert d.vertical_velocity == pytest.approx(28.70443) + assert len(d._vertical_velocities) == 3 + # It's falling now so the max velocity should greater than the current velocity assert d._max_vertical_velocity != d.vertical_velocity - # Our max velocity is hit with the first est data packet on this update: - assert d._max_vertical_velocity == pytest.approx(math.sqrt(27.0**2 + 32.0**2 + 38.0**2)) def test_first_update_no_data_packets(self, data_processor): """Tests whether the update() method works correctly, when no data packets are passed.""" d = data_processor d.update([]) - assert d._last_data_point is None + assert d._last_data_packet is None assert len(d._data_packets) == 0 assert len(d._current_altitudes) == 1 assert len(d._vertical_velocities) == 1 @@ -161,7 +204,7 @@ def test_first_update_no_data_packets(self, data_processor): assert d.current_altitude == 0.0, "Current altitude should be the same as set in __init__" assert d._initial_altitude is None assert d._max_altitude == 0.0 - assert d._last_data_point is None + assert d._last_data_packet is None @pytest.mark.parametrize( ( @@ -173,7 +216,18 @@ def test_first_update_no_data_packets(self, data_processor): ( [ EstimatedDataPacket( - 0 * 1e9, estLinearAccelX=1, estLinearAccelY=2, estLinearAccelZ=3, estPressureAlt=20 + 0 * 1e9, + estLinearAccelX=1, + estLinearAccelY=2, + estLinearAccelZ=3, + estPressureAlt=20, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=9.8, ) ], 20.0, @@ -182,10 +236,32 @@ def test_first_update_no_data_packets(self, data_processor): ( [ EstimatedDataPacket( - 1 * 1e9, estLinearAccelX=1, estLinearAccelY=2, estLinearAccelZ=3, estPressureAlt=20 + 1 * 1e9, + estLinearAccelX=1, + estLinearAccelY=2, + estLinearAccelZ=3, + estPressureAlt=20, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=9.8, ), EstimatedDataPacket( - 2 * 1e9, estLinearAccelX=2, estLinearAccelY=3, estLinearAccelZ=4, estPressureAlt=30 + 2 * 1e9, + estLinearAccelX=2, + estLinearAccelY=3, + estLinearAccelZ=4, + estPressureAlt=30, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=9.8, ), ], 25.0, @@ -194,20 +270,53 @@ def test_first_update_no_data_packets(self, data_processor): ( [ EstimatedDataPacket( - 1 * 1e9, estLinearAccelX=1, estLinearAccelY=2, estLinearAccelZ=3, estPressureAlt=20 + 1 * 1e9, + estLinearAccelX=1, + estLinearAccelY=2, + estLinearAccelZ=3, + estPressureAlt=20, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=9.8, ), EstimatedDataPacket( - 2 * 1e9, estLinearAccelX=2, estLinearAccelY=3, estLinearAccelZ=4, estPressureAlt=30 + 2 * 1e9, + estLinearAccelX=2, + estLinearAccelY=3, + estLinearAccelZ=4, + estPressureAlt=30, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=9.8, ), EstimatedDataPacket( - 3 * 1e9, estLinearAccelX=3, estLinearAccelY=4, estLinearAccelZ=5, estPressureAlt=40 + 3 * 1e9, + estLinearAccelX=3, + estLinearAccelY=4, + estLinearAccelZ=5, + estPressureAlt=40, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=9.8, ), ], 30.0, 10.0, ), ], - ids=["one_data_point", "two_data_points", "three_data_points"], + ids=["one_data_packet", "two_data_packets", "three_data_packets"], ) def test_first_update(self, data_processor, data_packets, init_alt, max_alt): """ @@ -217,8 +326,8 @@ def test_first_update(self, data_processor, data_packets, init_alt, max_alt): d = data_processor d.update(data_packets.copy()) # We should always have a last data point - assert d._last_data_point - assert d._last_data_point == data_packets[-1] + assert d._last_data_packet + assert d._last_data_packet == data_packets[-1] assert len(d._data_packets) == len(data_packets) # the max() is there because if we only process one data packet, we just return early @@ -239,48 +348,7 @@ def test_first_update(self, data_processor, data_packets, init_alt, max_alt): assert len(processed_data) == len(data_packets) for idx, data in enumerate(processed_data): assert data.current_altitude == d._current_altitudes[idx] - assert data.velocity == d._vertical_velocities[idx] - - def test_previous_velocity_retained(self, data_processor): - """Test that previous velocity is retained correctly between updates.""" - # Initial data packet - x_accel = [1, 2, 3] - y_accel = [0, 0, 0] - z_accel = [0, 0, 0] - - packets = [ - EstimatedDataPacket( - 1.0 * 1e9, - estLinearAccelX=x_accel[0], - estLinearAccelY=y_accel[0], - estLinearAccelZ=z_accel[0], - estPressureAlt=0, - ), - EstimatedDataPacket( - 2.0 * 1e9, - estLinearAccelX=x_accel[1], - estLinearAccelY=y_accel[1], - estLinearAccelZ=z_accel[1], - estPressureAlt=0, - ), - ] - data_processor.update(packets) - first_vel = data_processor._previous_velocity - - # Additional data packet - new_packets = [ - EstimatedDataPacket( - 3.0 * 1e9, - estLinearAccelX=x_accel[2], - estLinearAccelY=y_accel[2], - estLinearAccelZ=z_accel[2], - estPressureAlt=0, - ), - ] - data_processor.update(new_packets) - second_vel = data_processor._previous_velocity - - assert second_vel[0] > first_vel[0], "Previous velocity should be updated after each data update." + assert data.vertical_velocity == d._vertical_velocities[idx] @pytest.mark.parametrize( # altitude reading - list of altitudes passed to the data processor (estPressureAlt) @@ -298,13 +366,37 @@ def test_altitude_zeroing(self, data_processor, altitude_reading, current_altitu """Tests whether the altitude is correctly zeroed""" d = data_processor # test_first_update tests the initial alt update, so we can skip that here: - d._last_data_point = EstimatedDataPacket( - 0, estLinearAccelX=1, estLinearAccelY=2, estLinearAccelZ=3, estPressureAlt=altitude_reading[0] + d._last_data_packet = EstimatedDataPacket( + 0, + estLinearAccelX=1, + estLinearAccelY=2, + estLinearAccelZ=3, + estPressureAlt=altitude_reading[0], + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=9.8, ) d._initial_altitude = 20.0 new_packets = [ - EstimatedDataPacket(idx + 3, estLinearAccelX=1, estLinearAccelY=2, estLinearAccelZ=3, estPressureAlt=alt) + EstimatedDataPacket( + idx + 3, + estLinearAccelX=1, + estLinearAccelY=2, + estLinearAccelZ=3, + estPressureAlt=alt, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=9.8, + ) for idx, alt in enumerate(altitude_reading) ] d.update(new_packets) @@ -319,7 +411,20 @@ def test_max_altitude(self, data_processor): for i in range(0, len(altitudes), 10): d.update( [ - EstimatedDataPacket(i, estLinearAccelX=1, estLinearAccelY=2, estLinearAccelZ=3, estPressureAlt=alt) + EstimatedDataPacket( + i, + estLinearAccelX=1, + estLinearAccelY=2, + estLinearAccelZ=3, + estPressureAlt=alt, + estOrientQuaternionW=0.1, + estOrientQuaternionX=0.2, + estOrientQuaternionY=0.3, + estOrientQuaternionZ=0.4, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=9.8, + ) for alt in altitudes[i : i + 10] ] ) @@ -346,6 +451,9 @@ def test_max_altitude(self, data_processor): estAngularRateY=0.1, estAngularRateZ=2, estPressureAlt=0.0, + estGravityVectorX=0, + estGravityVectorY=0, + estGravityVectorZ=-9.8, ), EstimatedDataPacket( timestamp=1.002 * 1e9, @@ -374,8 +482,11 @@ def test_calculate_rotations(self, data_packets, expected_value): d.update(data_packets) rotations = d._rotated_accelerations assert len(rotations) == 3 - for rot, expected_val in zip(rotations, expected_value, strict=False): - assert rot == pytest.approx(expected_val) + # Rotations has 3 arrays in it corresponding to x, y, and z + # I just could not understand how to get it to work with zip so I did this instead lol + assert rotations[0][-1] == pytest.approx(expected_value[0]) + assert rotations[1][-1] == pytest.approx(expected_value[1]) + assert rotations[2][-1] == pytest.approx(expected_value[2]) def test_initial_orientation(self): """Tests whether the initial orientation of the rocket is correctly calculated""" From be69a33f27cfc529efe13416303a80cece54394d Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 26 Oct 2024 23:59:24 -0400 Subject: [PATCH 07/14] Ruff --- tests/test_data_processor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_data_processor.py b/tests/test_data_processor.py index 737d984c..77f8576c 100644 --- a/tests/test_data_processor.py +++ b/tests/test_data_processor.py @@ -116,7 +116,7 @@ def test_calculate_velocity(self, data_processor): estOrientQuaternionW=0.08, estOrientQuaternionX=0.0, estOrientQuaternionY=0.0, - estOrientQuaternionZ=-.5, + estOrientQuaternionZ=-0.5, estGravityVectorX=0, estGravityVectorY=0, estGravityVectorZ=-9.8, From 329a57ebdc3aca84e220de78256e472e29de3c05 Mon Sep 17 00:00:00 2001 From: Jack <85963782+JacksonElia@users.noreply.github.com> Date: Sun, 27 Oct 2024 02:18:43 -0400 Subject: [PATCH 08/14] Update airbrakes/data_handling/data_processor.py Co-authored-by: Harshil <37377066+harshil21@users.noreply.github.com> --- airbrakes/data_handling/data_processor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/airbrakes/data_handling/data_processor.py b/airbrakes/data_handling/data_processor.py index c94039f0..6b1c0103 100644 --- a/airbrakes/data_handling/data_processor.py +++ b/airbrakes/data_handling/data_processor.py @@ -107,7 +107,7 @@ def update(self, data_packets: list[EstimatedDataPacket]) -> None: # If we don't have a last data point, we can't calculate the time differences needed # for velocity calculation: if self._last_data_packet is None: - self._set_up() + self._first_update() self._time_differences = self._calculate_time_differences() From 73b94bb37ebd077b1bb00369a5e3e3a871a00991 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sun, 27 Oct 2024 14:36:29 -0400 Subject: [PATCH 09/14] Improved init test --- airbrakes/data_handling/data_processor.py | 16 +++++++--------- tests/test_data_processor.py | 23 ++++++++++++++++------- 2 files changed, 23 insertions(+), 16 deletions(-) diff --git a/airbrakes/data_handling/data_processor.py b/airbrakes/data_handling/data_processor.py index 6b1c0103..60bc1599 100644 --- a/airbrakes/data_handling/data_processor.py +++ b/airbrakes/data_handling/data_processor.py @@ -22,10 +22,9 @@ class IMUDataProcessor: "_current_altitudes", "_current_orientation_quaternions", "_data_packets", - "_first_data_packet", "_gravity_direction", "_gravity_orientation", - "_gravity_upwards_index", + "_gravity_axis_index", "_initial_altitude", "_last_data_packet", "_max_altitude", @@ -51,13 +50,12 @@ def __init__(self): self._initial_altitude: np.float64 | None = None self._current_altitudes: npt.NDArray[np.float64] = np.array([0.0], dtype=np.float64) self._last_data_packet: EstimatedDataPacket | None = None - self._first_data_packet: EstimatedDataPacket | None = None self._current_orientation_quaternions: npt.NDArray[np.float64] | None = None self._rotated_accelerations: list[npt.NDArray[np.float64]] = [np.array([0.0]), np.array([0.0]), np.array([0.0])] self._data_packets: list[EstimatedDataPacket] = [] self._time_differences: npt.NDArray[np.float64] = np.array([0.0]) self._gravity_orientation: npt.NDArray[np.float64] | None = None - self._gravity_upwards_index: int | None = 0 + self._gravity_axis_index: int | None = 0 self._gravity_direction: np.float64 | None = None def __str__(self) -> str: @@ -144,7 +142,7 @@ def get_processed_data_packets(self) -> deque[ProcessedDataPacket]: ) in zip( self._current_altitudes, self._vertical_velocities, - self._rotated_accelerations[self._gravity_upwards_index], + self._rotated_accelerations[self._gravity_axis_index], self._time_differences, strict=True, ) @@ -179,7 +177,7 @@ def _calculate_quaternion_conjugate(quaternion: npt.NDArray[np.float64]) -> npt. w, x, y, z = quaternion return np.array([w, -x, -y, -z]) - def _set_up(self) -> None: + def _first_update(self) -> None: """ Sets up the initial values for the data processor. This includes setting the initial altitude, the initial orientation of the rocket, and the initial gravity vector. This should @@ -216,7 +214,7 @@ def _set_up(self) -> None: ) # Gets the index for the direction (x, y, or z) that is pointing upwards - self._gravity_upwards_index = np.argmax(np.abs(gravity_orientation)) + self._gravity_axis_index = np.argmax(np.abs(gravity_orientation)) # on the physical IMU there is a depiction of the orientation. If a negative direction is # pointing to the sky, by convention, we define the gravity direction as negative. Otherwise, if @@ -224,7 +222,7 @@ def _set_up(self) -> None: # For purposes of standardizing the sign of accelerations that come out of calculate_rotated_accelerations, # we define acceleration from motor burn as positive, acceleration due to drag as negative, and # acceleration on the ground to be +9.8. - self._gravity_direction = 1 if gravity_orientation[self._gravity_upwards_index] < 0 else -1 + self._gravity_direction = 1 if gravity_orientation[self._gravity_axis_index] < 0 else -1 def _calculate_current_altitudes(self) -> npt.NDArray[np.float64]: """ @@ -312,7 +310,7 @@ def _calculate_vertical_velocity(self) -> npt.NDArray[np.float64]: vertical_accelerations = np.array( [ deadband(vertical_acceleration - GRAVITY, ACCELERATION_NOISE_THRESHOLD) - for vertical_acceleration in self._rotated_accelerations[self._gravity_upwards_index] + for vertical_acceleration in self._rotated_accelerations[self._gravity_axis_index] ] ) diff --git a/tests/test_data_processor.py b/tests/test_data_processor.py index 77f8576c..bd2a2481 100644 --- a/tests/test_data_processor.py +++ b/tests/test_data_processor.py @@ -82,15 +82,24 @@ def test_slots(self): def test_init(self, data_processor): d = data_processor assert d._max_altitude == 0.0 - assert isinstance(d._previous_vertical_velocity, np.float64) - assert d._previous_vertical_velocity == 0.0 - assert d._initial_altitude is None - assert isinstance(d._current_altitudes, np.ndarray) assert isinstance(d._vertical_velocities, np.ndarray) assert list(d._vertical_velocities) == [0.0] assert d._max_vertical_velocity == 0.0 + assert d._previous_vertical_velocity == 0.0 + assert d._initial_altitude is None + assert isinstance(d._current_altitudes, np.ndarray) assert d.current_altitude == 0.0 assert list(d._current_altitudes) == [0.0] + assert d._last_data_packet is None + assert d._current_orientation_quaternions is None + assert isinstance(d._rotated_accelerations, list) + assert d._rotated_accelerations == [np.array([0.0]), np.array([0.0]), np.array([0.0])] + assert d._data_packets == [] + assert isinstance(d._time_differences, np.ndarray) + assert list(d._time_differences) == [0.0] + assert d._gravity_orientation is None + assert d._gravity_axis_index == 0 + assert d._gravity_direction is None # See the comment in _calculate_velocitys() for why velocity is 0 during init. assert d.vertical_velocity == 0.0 @@ -425,7 +434,7 @@ def test_max_altitude(self, data_processor): estGravityVectorY=0, estGravityVectorZ=9.8, ) - for alt in altitudes[i : i + 10] + for alt in altitudes[i: i + 10] ] ) assert d.max_altitude + d._initial_altitude == pytest.approx(max(altitudes)) @@ -511,7 +520,7 @@ def test_initial_orientation(self): ) npt.assert_array_equal(d._current_orientation_quaternions, np.array([0.1, 0.2, 0.3, 0.4])) - assert d._gravity_upwards_index == 2 + assert d._gravity_axis_index == 2 assert d._gravity_direction == -1 d = IMUDataProcessor() @@ -534,7 +543,7 @@ def test_initial_orientation(self): ] ) - assert d._gravity_upwards_index == 0 + assert d._gravity_axis_index == 0 assert d._gravity_direction == 1 From c60efe1996b50d19594c8922416aa93b6eec185a Mon Sep 17 00:00:00 2001 From: jgelia Date: Sun, 27 Oct 2024 15:04:48 -0400 Subject: [PATCH 10/14] Added quat tests --- tests/test_data_processor.py | 66 ++++++++++++++++++++++++++++++++---- 1 file changed, 60 insertions(+), 6 deletions(-) diff --git a/tests/test_data_processor.py b/tests/test_data_processor.py index bd2a2481..29807e87 100644 --- a/tests/test_data_processor.py +++ b/tests/test_data_processor.py @@ -80,7 +80,9 @@ def test_slots(self): assert getattr(inst, attr, "err") != "err", f"got extra slot '{attr}'" def test_init(self, data_processor): + """Tests whether the IMUDataProcessor is correctly initialized""" d = data_processor + # Test attributes on init assert d._max_altitude == 0.0 assert isinstance(d._vertical_velocities, np.ndarray) assert list(d._vertical_velocities) == [0.0] @@ -88,7 +90,6 @@ def test_init(self, data_processor): assert d._previous_vertical_velocity == 0.0 assert d._initial_altitude is None assert isinstance(d._current_altitudes, np.ndarray) - assert d.current_altitude == 0.0 assert list(d._current_altitudes) == [0.0] assert d._last_data_packet is None assert d._current_orientation_quaternions is None @@ -100,15 +101,19 @@ def test_init(self, data_processor): assert d._gravity_orientation is None assert d._gravity_axis_index == 0 assert d._gravity_direction is None - # See the comment in _calculate_velocitys() for why velocity is 0 during init. + + # Test properties on init + assert d.max_altitude == 0.0 + assert d.current_altitude == 0.0 assert d.vertical_velocity == 0.0 + assert d.max_vertical_velocity == 0.0 def test_str(self, data_processor): data_str = "IMUDataProcessor(max_altitude=0.0, current_altitude=0.0, velocity=0.0, " "max_velocity=0.0, " assert str(data_processor) == data_str def test_calculate_velocity(self, data_processor): - """Tests whether the velocity is correctly calculated""" + """Tests whether the vertical velocity is correctly calculated""" d = data_processor assert d.vertical_velocity == 0.0 assert d._max_vertical_velocity == d.vertical_velocity @@ -199,7 +204,7 @@ def test_calculate_velocity(self, data_processor): assert d.vertical_velocity == pytest.approx(28.70443) assert len(d._vertical_velocities) == 3 # It's falling now so the max velocity should greater than the current velocity - assert d._max_vertical_velocity != d.vertical_velocity + assert d._max_vertical_velocity > d.vertical_velocity def test_first_update_no_data_packets(self, data_processor): """Tests whether the update() method works correctly, when no data packets are passed.""" @@ -358,6 +363,8 @@ def test_first_update(self, data_processor, data_packets, init_alt, max_alt): for idx, data in enumerate(processed_data): assert data.current_altitude == d._current_altitudes[idx] assert data.vertical_velocity == d._vertical_velocities[idx] + assert data.vertical_acceleration == d._rotated_accelerations[d._gravity_axis_index][idx] + assert data.time_since_last_data_packet == d._time_differences[idx] @pytest.mark.parametrize( # altitude reading - list of altitudes passed to the data processor (estPressureAlt) @@ -497,8 +504,8 @@ def test_calculate_rotations(self, data_packets, expected_value): assert rotations[1][-1] == pytest.approx(expected_value[1]) assert rotations[2][-1] == pytest.approx(expected_value[2]) - def test_initial_orientation(self): - """Tests whether the initial orientation of the rocket is correctly calculated""" + def test_initial_orientation_and_gravity(self): + """Tests whether the initial orientation and gravity of the rocket is correctly calculated""" d = IMUDataProcessor() d.update( [ @@ -546,6 +553,53 @@ def test_initial_orientation(self): assert d._gravity_axis_index == 0 assert d._gravity_direction == 1 + @pytest.mark.parametrize("q1, q2, expected", [ + # Random quaternions + (np.array([4, 1, 2, 3]), np.array([8, 5, 6, 7]), np.array([-6, 24, 48, 48])), + # Test with negative numbers + (np.array([2, -1, -2, 1]), np.array([1, 2, -1, 3]), np.array([-1, -2, 1, 12])), + # Test with zeros in different positions + (np.array([1, 0, 2, 0]), np.array([2, 1, 0, 3]), np.array([2, 7, 4, 1])), + ]) + def test_multiply_quaternions(self, q1, q2, expected): + """ + Tests whether the quaternion multiplication works correctly for various cases + """ + d = IMUDataProcessor() + result = d._multiply_quaternions(q1, q2) + npt.assert_array_almost_equal(result, expected) + + @pytest.mark.parametrize("q", [ + np.array([2, 3, 4, 5]), # random quaternion + np.array([1, -2, 3, -4]), # quaternion with mixed signs + np.array([0, 1, 0, 1]), # pure quaternion + ]) + def test_multiply_by_identity(self, q): + """Tests multiplication with identity quaternion [1,0,0,0]""" + d = IMUDataProcessor() + identity = np.array([1, 0, 0, 0]) + result = d._multiply_quaternions(q, identity) + npt.assert_array_almost_equal(result, q) + # Also test right multiplication + result = d._multiply_quaternions(identity, q) + npt.assert_array_almost_equal(result, q) + + @pytest.mark.parametrize("q, expected", [ + # Random quaternion + (np.array([0.1, 0.2, 0.3, 0.4]), np.array([0.1, -0.2, -0.3, -0.4])), + # Pure quaternion (w = 0) + (np.array([0.0, 1.0, 2.0, 3.0]), np.array([0.0, -1.0, -2.0, -3.0])), + # Quaternion with negative components + (np.array([2.0, -1.0, -2.0, 3.0]), np.array([2.0, 1.0, 2.0, -3.0])), + ]) + def test_calculate_quaternion_conjugate(self, q, expected): + """ + Tests whether the quaternion conjugate is correctly calculated for various cases + """ + d = IMUDataProcessor() + result = d._calculate_quaternion_conjugate(q) + npt.assert_array_almost_equal(result, expected) + """ This is unit tests for apogee prediction. Does not work currently, will most likely be moved to From 7881a95be857456d6d3659baa9c228d366aad1a3 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sun, 27 Oct 2024 15:09:13 -0400 Subject: [PATCH 11/14] Ruff --- airbrakes/data_handling/data_processor.py | 2 +- tests/test_data_processor.py | 53 +++++++++++++---------- 2 files changed, 32 insertions(+), 23 deletions(-) diff --git a/airbrakes/data_handling/data_processor.py b/airbrakes/data_handling/data_processor.py index 60bc1599..511cb83e 100644 --- a/airbrakes/data_handling/data_processor.py +++ b/airbrakes/data_handling/data_processor.py @@ -22,9 +22,9 @@ class IMUDataProcessor: "_current_altitudes", "_current_orientation_quaternions", "_data_packets", + "_gravity_axis_index", "_gravity_direction", "_gravity_orientation", - "_gravity_axis_index", "_initial_altitude", "_last_data_packet", "_max_altitude", diff --git a/tests/test_data_processor.py b/tests/test_data_processor.py index 29807e87..e83db8ad 100644 --- a/tests/test_data_processor.py +++ b/tests/test_data_processor.py @@ -441,7 +441,7 @@ def test_max_altitude(self, data_processor): estGravityVectorY=0, estGravityVectorZ=9.8, ) - for alt in altitudes[i: i + 10] + for alt in altitudes[i : i + 10] ] ) assert d.max_altitude + d._initial_altitude == pytest.approx(max(altitudes)) @@ -553,14 +553,17 @@ def test_initial_orientation_and_gravity(self): assert d._gravity_axis_index == 0 assert d._gravity_direction == 1 - @pytest.mark.parametrize("q1, q2, expected", [ - # Random quaternions - (np.array([4, 1, 2, 3]), np.array([8, 5, 6, 7]), np.array([-6, 24, 48, 48])), - # Test with negative numbers - (np.array([2, -1, -2, 1]), np.array([1, 2, -1, 3]), np.array([-1, -2, 1, 12])), - # Test with zeros in different positions - (np.array([1, 0, 2, 0]), np.array([2, 1, 0, 3]), np.array([2, 7, 4, 1])), - ]) + @pytest.mark.parametrize( + ("q1", "q2", "expected"), + [ + # Random quaternions + (np.array([4, 1, 2, 3]), np.array([8, 5, 6, 7]), np.array([-6, 24, 48, 48])), + # Test with negative numbers + (np.array([2, -1, -2, 1]), np.array([1, 2, -1, 3]), np.array([-1, -2, 1, 12])), + # Test with zeros in different positions + (np.array([1, 0, 2, 0]), np.array([2, 1, 0, 3]), np.array([2, 7, 4, 1])), + ], + ) def test_multiply_quaternions(self, q1, q2, expected): """ Tests whether the quaternion multiplication works correctly for various cases @@ -569,11 +572,14 @@ def test_multiply_quaternions(self, q1, q2, expected): result = d._multiply_quaternions(q1, q2) npt.assert_array_almost_equal(result, expected) - @pytest.mark.parametrize("q", [ - np.array([2, 3, 4, 5]), # random quaternion - np.array([1, -2, 3, -4]), # quaternion with mixed signs - np.array([0, 1, 0, 1]), # pure quaternion - ]) + @pytest.mark.parametrize( + "q", + [ + np.array([2, 3, 4, 5]), # random quaternion + np.array([1, -2, 3, -4]), # quaternion with mixed signs + np.array([0, 1, 0, 1]), # pure quaternion + ], + ) def test_multiply_by_identity(self, q): """Tests multiplication with identity quaternion [1,0,0,0]""" d = IMUDataProcessor() @@ -584,14 +590,17 @@ def test_multiply_by_identity(self, q): result = d._multiply_quaternions(identity, q) npt.assert_array_almost_equal(result, q) - @pytest.mark.parametrize("q, expected", [ - # Random quaternion - (np.array([0.1, 0.2, 0.3, 0.4]), np.array([0.1, -0.2, -0.3, -0.4])), - # Pure quaternion (w = 0) - (np.array([0.0, 1.0, 2.0, 3.0]), np.array([0.0, -1.0, -2.0, -3.0])), - # Quaternion with negative components - (np.array([2.0, -1.0, -2.0, 3.0]), np.array([2.0, 1.0, 2.0, -3.0])), - ]) + @pytest.mark.parametrize( + ("q", "expected"), + [ + # Random quaternion + (np.array([0.1, 0.2, 0.3, 0.4]), np.array([0.1, -0.2, -0.3, -0.4])), + # Pure quaternion (w = 0) + (np.array([0.0, 1.0, 2.0, 3.0]), np.array([0.0, -1.0, -2.0, -3.0])), + # Quaternion with negative components + (np.array([2.0, -1.0, -2.0, 3.0]), np.array([2.0, 1.0, 2.0, -3.0])), + ], + ) def test_calculate_quaternion_conjugate(self, q, expected): """ Tests whether the quaternion conjugate is correctly calculated for various cases From ccc9ac012fea791bcb594cc0700ce2dce07d6989 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sun, 27 Oct 2024 16:21:42 -0400 Subject: [PATCH 12/14] mock is good to go --- airbrakes/data_handling/apogee_predictor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/airbrakes/data_handling/apogee_predictor.py b/airbrakes/data_handling/apogee_predictor.py index 970a4fd6..3990ab80 100644 --- a/airbrakes/data_handling/apogee_predictor.py +++ b/airbrakes/data_handling/apogee_predictor.py @@ -178,7 +178,7 @@ def _prediction_loop(self) -> None: for data_packet in data_packets: self._accelerations.append(data_packet.vertical_acceleration) - self._time_differences.append(data_packet.time_since_last_data_point) + self._time_differences.append(data_packet.time_since_last_data_packet) self._current_altitude = data_packet.current_altitude self._current_velocity = data_packet.vertical_velocity From ed2e713b2b9434cd850827eab95626a425d9b982 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sun, 27 Oct 2024 16:27:35 -0400 Subject: [PATCH 13/14] additional refactoring --- airbrakes/data_handling/apogee_predictor.py | 2 +- tests/test_airbrakes.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/airbrakes/data_handling/apogee_predictor.py b/airbrakes/data_handling/apogee_predictor.py index 3990ab80..b8188b16 100644 --- a/airbrakes/data_handling/apogee_predictor.py +++ b/airbrakes/data_handling/apogee_predictor.py @@ -24,7 +24,7 @@ class ApogeePredictor: :param: state: airbrakes state class :param: data_processor: IMUDataProcessor class - :param: data_points: A sequence of EstimatedDataPacket objects to process. + :param: data_packets: A sequence of EstimatedDataPacket objects to process. """ __slots__ = ( diff --git a/tests/test_airbrakes.py b/tests/test_airbrakes.py index 294f69d3..af306ac9 100644 --- a/tests/test_airbrakes.py +++ b/tests/test_airbrakes.py @@ -120,7 +120,7 @@ def log(self, state, extension, imu_data_packets, processed_data_packets): assert mocked_airbrakes.imu._data_queue.qsize() > 0 assert mocked_airbrakes.state.name == "StandByState" - assert mocked_airbrakes.data_processor._last_data_point is None + assert mocked_airbrakes.data_processor._last_data_packet is None monkeypatch.setattr(data_processor.__class__, "update", update) monkeypatch.setattr(mocked_airbrakes.state.__class__, "update", state) From a2e2fb4a5fafa2761df384931e0a677bfb25db1f Mon Sep 17 00:00:00 2001 From: Harshil <37377066+harshil21@users.noreply.github.com> Date: Sun, 27 Oct 2024 16:41:40 -0400 Subject: [PATCH 14/14] Update tests/test_data_processor.py --- tests/test_data_processor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_data_processor.py b/tests/test_data_processor.py index e83db8ad..e525a640 100644 --- a/tests/test_data_processor.py +++ b/tests/test_data_processor.py @@ -112,7 +112,7 @@ def test_str(self, data_processor): data_str = "IMUDataProcessor(max_altitude=0.0, current_altitude=0.0, velocity=0.0, " "max_velocity=0.0, " assert str(data_processor) == data_str - def test_calculate_velocity(self, data_processor): + def test_calculate_vertical_velocity(self, data_processor): """Tests whether the vertical velocity is correctly calculated""" d = data_processor assert d.vertical_velocity == 0.0