def test_body2hor():
# Test with a pitch rotation
vector_body = np.array([1, 1, 1])
theta, phi, psi = np.deg2rad(45), 0, 0
vector_hor = body2hor(vector_body, theta, phi, psi)
vector_hor_expected = np.array([2 * 0.70710678118654757, 1, 0])
assert_array_almost_equal(vector_hor, vector_hor_expected)
# Test with a roll rotation
vector_body = np.array([1, 1, 1])
theta, phi, psi = 0, np.deg2rad(45), 0
vector_hor = body2hor(vector_body, theta, phi, psi)
vector_hor_expected = np.array([1, 0, 2 * 0.70710678118654757])
assert_array_almost_equal(vector_hor, vector_hor_expected)
# Test with a yaw rotation
vector_body = np.array([1, 1, 1])
theta, phi, psi = 0, 0, np.deg2rad(45)
vector_hor = body2hor(vector_body, theta, phi, psi)
vector_hor_expected = np.array([0, 2 * 0.70710678118654757, 1])
assert_array_almost_equal(vector_hor, vector_hor_expected)
def propagate(self, aircraft, dt=0.01):
"""Propagate the state vector and update the rest of variables.
Parameters
----------
aircraft : Aircraft
Aircraft model for simulation is used to get forces.
"""
self.dt = dt
self._propagate_state_vector(aircraft, dt)
# TODO: update the rest of variables.
self.vel_body = self.state_vector[0:3]
self.vel_ang = self.state_vector[3:6]
self.euler_angles[0] = self.state_vector[8] # psi
self.euler_angles[1] = self.state_vector[6] # theta
self.euler_angles[2] = self.state_vector[7] # phi
self.coord_earth = self.state_vector[9:12]
# Set psi between 0 and 2*pi
# FIXME: check the conversion to keep angle between 0 and 2pi again
self.euler_angles[0] = np.arctan2(np.sin(self.psi), np.cos(
self.psi)) % (2*np.pi)
self.euler_angles[2] = np.arctan2(np.sin(self.phi), np.cos(self.phi))
self.vel_NED = body2hor(self.vel_body, theta=self.theta, phi=self.phi,
psi=self.psi)
def propagate(self, aircraft, dt=0.01):
"""Propagate the state vector and update the rest of variables.
Parameters
----------
environment
"""
self.dt = dt
self._propagate_state_vector(aircraft, dt)
# TODO: update the rest of variables.
self.vel_body = self.state_vector[0:3]
self.vel_ang = self.state_vector[3:6]
self.euler_angles[0] = self.state_vector[8] # psi
self.euler_angles[1] = self.state_vector[6] # theta
self.euler_angles[2] = self.state_vector[7] # phi
self.coord_earth = self.state_vector[9:12]
# Set psi between 0 and 2*pi
# FIXME: check the conversion to keep angle between 0 and 2pi again
self.euler_angles[0] = np.arctan2(np.sin(self.psi), np.cos(
self.psi)) % (2*np.pi)
self.euler_angles[2] = np.arctan2(np.sin(self.phi), np.cos(self.phi))
self.vel_NED = body2hor(self.vel_body, theta=self.theta, phi=self.phi,
psi=self.psi)
def set_initial_state_vector(self):
"""
Set the initial values of the required variables
"""
self.vel_NED = body2hor(self.vel_body, theta=self.theta,
phi=self.phi, psi=self.psi)
self.state_vector = np.array([
self.u, self.v, self.w,
self.p, self.q, self.r,
self.theta, self.phi, self.psi,
self.x_earth, self.y_earth, self.z_earth
])
self._ode_lamceq.set_initial_value(y=self.state_vector[0:6])
self._ode_kaqeq.set_initial_value(y=self.state_vector[6:9])
self._ode_kleq.set_initial_value(y=self.state_vector[9:12])
def set_initial_state_vector(self):
"""
Set the initial values of the state vector for system integration
once the values for the involved variables have been assigned or the
system has been trimmed.
"""
self.vel_NED = body2hor(self.vel_body, theta=self.theta,
phi=self.phi, psi=self.psi)
self.state_vector = np.array([
self.u, self.v, self.w,
self.p, self.q, self.r,
self.theta, self.phi, self.psi,
self.x_earth, self.y_earth, self.z_earth
])
self._ode_lamceq.set_initial_value(y=self.state_vector[0:6])
self._ode_kaqeq.set_initial_value(y=self.state_vector[6:9])
self._ode_kleq.set_initial_value(y=self.state_vector[9:12])
def set_initial_state_vector(self):
"""
Set the initial values of the state vector for system integration
once the values for the involved variables have been assigned or the
system has been trimmed.
"""
self.vel_NED = body2hor(self.vel_body, theta=self.theta,
phi=self.phi, psi=self.psi)
self.state_vector = np.array([
self.u, self.v, self.w,
self.p, self.q, self.r,
self.theta, self.phi, self.psi,
self.x_earth, self.y_earth, self.z_earth
])
self._ode_lamceq.set_initial_value(y=self.state_vector[0:6])
self._ode_kaqeq.set_initial_value(y=self.state_vector[6:9])
self._ode_kleq.set_initial_value(y=self.state_vector[9:12])
def update(self, value, attitude):
self._vel_body[:] = value
self._vel_NED = body2hor(value, attitude.theta, attitude.phi,
attitude.psi) # m/s
def update(self, coords, attitude):
self._accel_body[:] = coords
self._accel_NED = body2hor(coords, attitude.theta, attitude.phi,
attitude.psi)
def update(self, value, attitude):
self._vel_body[:] = value
self._vel_NED = body2hor(value,
attitude.theta,
attitude.phi,
attitude.psi) # m/s
def update(self, coords, attitude):
self._accel_body[:] = coords
self._accel_NED = body2hor(coords,
attitude.theta,
attitude.phi,
attitude.psi)
