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)