def test_hor2body():
# Test with a pitch rotation
vector_hor = np.array([2 * 0.70710678118654757, 1, 0])
theta, phi, psi = np.deg2rad(45), 0, 0
vector_body_expected = np.array([1, 1, 1])
vector_body = hor2body(vector_hor, theta, phi, psi)
assert_array_almost_equal(vector_body, vector_body_expected)
# Test with a roll rotation
vector_hor = np.array([1, 0, 2 * 0.70710678118654757])
theta, phi, psi = 0, np.deg2rad(45), 0
vector_body_expected = np.array([1, 1, 1])
vector_body = hor2body(vector_hor, theta, phi, psi)
assert_array_almost_equal(vector_body, vector_body_expected)
# Test with a yaw rotation
vector_hor = np.array([0, 2 * 0.70710678118654757, 1])
theta, phi, psi = 0, 0, np.deg2rad(45)
vector_body_expected = np.array([1, 1, 1])
vector_body = hor2body(vector_hor, theta, phi, psi)
assert_array_almost_equal(vector_body, vector_body_expected)
def update(self, system):
r_squared = system.coord_geocentric @ system.coord_geocentric
self.magnitude = STD_GRAVITATIONAL_PARAMETER / r_squared
self.unitary_vector = hor2body(self._z_horizon,
theta=system.theta,
phi=system.phi,
psi=system.psi)
self.vector = self.magnitude * self.unitary_vector
def update(self, state):
self._versor = hor2body(self._z_horizon,
theta=state.attitude.theta,
phi=state.attitude.phi,
psi=state.attitude.psi
)
self._vector = self.magnitude * self.versor
def update(self, system):
r_squared = system.coord_geocentric @ system.coord_geocentric
self.magnitude = STD_GRAVITATIONAL_PARAMETER / r_squared
self.unitary_vector = hor2body(self._z_horizon,
theta=system.theta,
phi=system.phi,
psi=system.psi)
self.vector = self.magnitude * self.unitary_vector
def get_forces_and_moments(TAS, rho, alpha, beta, delta_e, ih, delta_ail,
delta_r, delta_t, attitude):
"""Return the total forces and moments including aerodynamics, thrust and
gravity.
Parameters
----------
TAS : float
velocity (m/s).
rho : float
density (kg/m3).
alpha : float
attack angle (rad).
beta : float
sideslip angle (rad).
delta_e : float
elevator deflection (rad).
ih : float
stabilator deflection (rad).
delta_ail : float
aileron deflection (rad).
delta_r : float
rudder deflection (rad).
delta_t : float
Thrust level (between 0-1).
attitude : array_like
Attitude angles: (theta, phi, psi).
Returns
-------
forces : array_like
3 dimensional vector with (F_x_s, F_y_s, F_z_s) forces in body axes.
(N)
moments : array_like
3 dimensional vector with (Mxs, Mys, Mzs) forces in body axes. (N·m)
"""
# As stability axes are coincident with wind axes at the moment of
# linearization (with alpha=0 and beta=0), stability axes are parallel to
# body axes.
aero_forces = get_aerodynamic_forces(TAS, rho, alpha, beta, delta_e, 0,
delta_ail, delta_r)
thrust = get_engine_force(delta_t)
# Assuming that all the engine thrust is prodecued in the x_body direction
engine_force = np.array([thrust, 0, 0])
g0 = 9.81 # m/s²
theta, phi, psi = attitude
gravity_force = hor2body((0, 0, g0), theta, phi, psi)
forces = aero_forces + engine_force + gravity_force
# It is assumed that the engine moments are zero.
moments = get_aerodynamic_moments(TAS, rho, alpha, beta, delta_e, ih,
delta_ail, delta_r)
return forces, moments
def update(self, state):
r_squared = (state.position.coord_geocentric @
state.position.coord_geocentric)
self._magnitude = STD_GRAVITATIONAL_PARAMETER / r_squared
self._versor = hor2body(self._z_horizon,
theta=state.attittude.theta,
phi=state.attittude.phi,
psi=state.attitude.psi
)
self._vector = self.magnitude * self._vector
def update(self, system):
self.unitary_vector = hor2body(self._z_horizon,
theta=system.theta,
phi=system.phi,
psi=system.psi)
self.vector = self.magnitude * self.unitary_vector
def update(self, value, attitude):
self._vel_NED[:] = value
self._vel_body = hor2body(value, attitude.theta, attitude.phi,
attitude.psi) # m/s
def update(self, coords, attitude):
self._accel_NED[:] = coords
self._accel_body = hor2body(coords, attitude.theta, attitude.phi,
attitude.psi)
def update(self, value, attitude):
self._vel_NED[:] = value
self._vel_body = hor2body(value,
attitude.theta,
attitude.phi,
attitude.psi) # m/s
def update(self, coords, attitude):
self._accel_NED[:] = coords
self._accel_body = hor2body(coords,
attitude.theta,
attitude.phi,
attitude.psi)
def update(self, system):
self.unitary_vector = hor2body(self._z_horizon,
theta=system.theta,
phi=system.phi,
psi=system.psi)
self.vector = self.magnitude * self.unitary_vector