def test_orientation_dcm(self):
"""
Test orientation_dcm.
"""
test_dcm = self.move.orientation_dcm(self.time)
ref_dcm = euler2dcm(self.a[0], self.a[1], self.a[2])
result = np.ma.allclose(test_dcm, ref_dcm, atol=1e-08)
self.assertTrue(result)
def set_init_orientation(self, gamma, theta, psi):
"""
Set initial orientation of calibration table.
Parameters
----------
gamma: float, rad, roll
theta: float, rad, pitch
psi: float, rad, yaw
"""
self.dcm[0] = euler2dcm(gamma, theta, psi)
def orientation_dcm(self, t):
"""
Calculate angle at specified time.
Parameters
----------
t: float, sec, time
"""
a = self.turn_rate * (t - self.time_start)
turn_dcm = euler2dcm(a[0], a[1], a[2])
return np.dot(self.init_dcm, turn_dcm)
def __init__(self, time1, x_turn, y_turn, z_turn ):
"""
Init rotation object
Parameters
----------
time: float, sec
x_turn: float, rad, relative turn around x axis of IMU
y_turn: float, rad, relative turn around y axis of IMU
z_turn: float, rad, relative turn around z axis of IMU
"""
# start time of current rotation
self.time_start = 0.
# time between start and end of current rotation
self.time_total = time1
self.init_dcm = euler2dcm(0., 0., 0.)
self.angle_total = np.array([x_turn, y_turn, z_turn])
self.turn_rate = self._calc_rate(self.time_total, self.angle_total)
def add(self, move):
"""
Add calibration table movement.
Parameters
----------
move: obj, one simple rotation
"""
if isinstance(move, RotateIMU):
move.time_start = sum(self.time)
self.time.append(move.time_total)
move.init_dcm = self.dcm[-1]
turn_dcm = euler2dcm(move.angle_total[0], move.angle_total[1], move.angle_total[2])
self.dcm.append(np.dot(self.dcm[-1], turn_dcm))
self.sequence.append(move)
else:
raise TypeError('Must be instance of RotateIMU')
def set_orientation(self, roll, pitch, yaw):
"""
Set initial orientation.
"""
self._dcmbn = euler2dcm(roll, pitch, yaw)
def __init__(self):
self.time = [0.]
self.dcm = [euler2dcm(0., 0., 0.)]
self.sequence = []
