def test_update_orientation_half_rev_x_axis():
    """
    Test update_orientation() rotating 1/2 of a revolution about the x-axis.
    """
    test_rocket = rm.Rocket()
    test_rocket.gyro = sensors.Gyro()
    delta_time = 0.5  # 1/2 second
    angular_rates = np.array([2 * np.pi, 0, 0])  # 1 rev/s in x
    orientation_after_update = Quaternion(axis=[1, 0, 0], angle=np.pi).elements
    new_orientation = test_rocket.gyro.update(test_rocket, angular_rates,
                                              delta_time)
    np.testing.assert_allclose(new_orientation,
                               orientation_after_update,
                               atol=rm.TOLERANCE)
def test_update_orientation_eighth_rev_y_axis():
    """
    Test update_orientation() rotating 1/8 of a revolution about the y-axis.
    """
    test_rocket = rm.Rocket()
    test_rocket.gyro = sensors.Gyro()
    delta_time = 0.125  # 1/8 second
    angular_rates = np.array([0, 2 * np.pi, 0])  # 1 rev/s in y
    orientation_after_update = Quaternion(axis=[0, 1, 0],
                                          angle=(np.pi / 4)).elements
    new_orientation = test_rocket.gyro.update(test_rocket, angular_rates,
                                              delta_time)
    np.testing.assert_allclose(new_orientation,
                               orientation_after_update,
                               atol=rm.TOLERANCE)
def test_update_orientation_three_quarter_rev_z_axis():
    """
    Test update_orientation() rotating 3/4 of a revolution about the z-axis.
    """
    test_rocket = rm.Rocket()
    test_rocket.gyro = sensors.Gyro()
    delta_time = 0.75  # 3/4 second
    angular_rates = np.array([0, 0, 2 * np.pi])  # 1 rev/s in z
    orientation_after_update = Quaternion(axis=[0, 0, 1],
                                          angle=(3 * np.pi / 2)).elements
    new_orientation = test_rocket.gyro.update(test_rocket, angular_rates,
                                              delta_time)
    np.testing.assert_allclose(new_orientation,
                               orientation_after_update,
                               atol=rm.TOLERANCE)
def test_update_orientation_quarter_rev_x_z_axes():
    """
    Test update_orientation() rotating 1/4 of a revolution about the x and z
    axes.
    """
    test_rocket = rm.Rocket()
    test_rocket.gyro = sensors.Gyro()
    delta_time = 0.25  # 1/4 second
    angular_rates = np.array([5.61985144, 0, 2.80992617])
    orientation_after_update = Quaternion(axis=[2, 0, 1],
                                          angle=(np.pi / 2)).elements
    new_orientation = test_rocket.gyro.update(test_rocket, angular_rates,
                                              delta_time)
    np.testing.assert_allclose(new_orientation,
                               orientation_after_update,
                               atol=rm.TOLERANCE)
def test_update_orientation_from_non_identity_quat():
    """
    Test update_orientation() rotating from non-identity quaternion.
    Note: rotation is 1/4 revolution about x axis.
    """
    test_rocket = rm.Rocket()
    test_rocket.gyro = sensors.Gyro()
    orientation = Quaternion(axis=[-1, 0, 0], angle=(np.pi / 2))
    test_rocket.orientation = orientation.elements
    delta_time = 0.25  # 1/4 second
    angular_rates = np.array([2 * np.pi, 0, 0])
    orientation_after_update = Quaternion().elements
    new_orientation = test_rocket.gyro.update(test_rocket, angular_rates,
                                              delta_time)
    np.testing.assert_allclose(new_orientation,
                               orientation_after_update,
                               atol=rm.TOLERANCE)
def test_update_orientation_half_rev_all_axes():
    """
    Test update_orientation() rotating 1/2 of a revolution about all of the
    axes.
    """
    test_rocket = rm.Rocket()
    test_rocket.gyro = sensors.Gyro()
    delta_time = 0.5  # 1/2 second
    angular_rates = np.array(
        [np.pi / np.sqrt(3), np.pi / np.sqrt(3),
         np.pi / np.sqrt(3)])  # 1/6 rev/s for all
    orientation_after_update = Quaternion(axis=[1, 1, 1],
                                          angle=(np.pi / 2)).elements
    new_orientation = test_rocket.gyro.update(test_rocket, angular_rates,
                                              delta_time)
    np.testing.assert_allclose(new_orientation,
                               orientation_after_update,
                               atol=rm.TOLERANCE)