def side_force_coeff_test_cases():
constants = SkywalkerX8Constants()
b = constants.wing_span
state1 = State()
state1.vx = 20.0
state1.vy = 0.0
state1.vz = 0.0
state2 = State()
state2.vx = 28.6362532829
state2.vy = 1.0
state2.vz = 0.0
state2.ang_rate_x = 5 * np.pi / 180
state2.ang_rate_z = 5 * np.pi / 180
wind = np.zeros(6)
airspeed = np.sqrt(np.sum(calc_airspeed(state2, wind)**2))
zero_input = ControlInput()
aileron_input = ControlInput()
aileron_input.aileron_deflection = 2.0 * np.pi / 180.0
return [
(state1, wind, 0.0, zero_input, 2.99968641720902E-08),
(state1, wind, 1.0, zero_input, -7.94977329537982E-06),
(state2, wind, 0.0, aileron_input,
-0.008604744865183 + -0.085 * b / (2 * airspeed) * state2.ang_rate_x +
0.005 * b / (2 * airspeed) * state2.ang_rate_z +
0.0433 * aileron_input.aileron_deflection),
(state2, wind, 1.0, aileron_input,
-0.007089388672593 + -0.133 * b / (2 * airspeed) * state2.ang_rate_x +
0.002 * b / (2 * airspeed) * state2.ang_rate_z +
0.0433 * aileron_input.aileron_deflection)
]
def yaw_moment_coeff_test_cases():
constants = SkywalkerX8Constants()
b = constants.wing_span
state1 = State()
state1.vx = 20.0
state1.vy = 0.0
state1.vz = 0.0
state2 = State()
state2.vx = 28.6362532829
state2.vy = 1.0
state2.vz = 0.0
state2.ang_rate_x = 5 * np.pi / 180
state2.ang_rate_z = 5 * np.pi / 180
wind = np.zeros(6)
airspeed = np.sqrt(np.sum(calc_airspeed(state2, wind)**2))
zero_input = ControlInput()
aileron_input = ControlInput()
aileron_input.aileron_deflection = 2.0 * np.pi / 180.0
return [
(state1, wind, 0.0, zero_input, 4.9176697574439E-06),
(state1, wind, 1.0, zero_input, 1.96093394589053E-05),
(state2, wind, 0.0, aileron_input,
0.000825947539055 + 0.027 * b / (2 * airspeed) * state2.ang_rate_x +
-0.022 * b / (2 * airspeed) * state2.ang_rate_z -
0.00339 * aileron_input.aileron_deflection),
(state2, wind, 1.0, aileron_input,
0.001052911121301 + 0.017 * b / (2 * airspeed) * state2.ang_rate_x +
-0.049 * b / (2 * airspeed) * state2.ang_rate_z -
0.00339 * aileron_input.aileron_deflection)
]
def roll_moment_coeff_test_cases():
constants = SkywalkerX8Constants()
b = constants.wing_span
state1 = State()
state1.vx = 20.0
state1.vy = 0.0
state1.vz = 0.0
state2 = State()
state2.vx = 28.6362532829
state2.vy = 1.0
state2.vz = 0.0
state2.ang_rate_x = 5 * np.pi / 180
state2.ang_rate_z = 5 * np.pi / 180
wind = np.zeros(6)
airspeed = np.sqrt(np.sum(calc_airspeed(state2, wind)**2))
zero_input = ControlInput()
aileron_input = ControlInput()
aileron_input.aileron_deflection = 2.0 * np.pi / 180.0
return [
(state1, wind, 0.0, zero_input, -8.40821757613653E-05),
(state1, wind, 1.0, zero_input, -7.34515369827804E-05),
(state2, wind, 0.0, aileron_input,
-0.00380800071177 + -0.409 * b / (2 * airspeed) * state2.ang_rate_x +
0.039 * b / (2 * airspeed) * state2.ang_rate_z +
0.12 * aileron_input.aileron_deflection),
(state2, wind, 1.0, aileron_input,
-0.003067251004494 + -0.407 * b / (2 * airspeed) * state2.ang_rate_x +
0.158 * b / (2 * airspeed) * state2.ang_rate_z +
0.12 * aileron_input.aileron_deflection)
]
def pitch_moment_coeff(self, state: State, wind: np.ndarray) -> float:
airspeed = np.sqrt(np.sum(calc_airspeed(state, wind)**2))
alpha = calc_angle_of_attack(state, wind) * 180.0 / np.pi
c = self.constants.mean_chord
delta_e = self.control_input.elevator_deflection
rot_airspeed_with_wind = calc_rotational_airspeed(state, wind)
ang_rate_y_r = rot_airspeed_with_wind[1]
return self.C_m_alpha(alpha, self.icing) + \
(self.C_m_q(alpha, self.icing) * c / (2*airspeed)) * ang_rate_y_r + \
self.C_m_delta_e(alpha, self.icing)*delta_e
def drag_coeff(self, state: State, wind: np.ndarray) -> float:
airspeed = np.sqrt(np.sum(calc_airspeed(state, wind)**2))
alpha = calc_angle_of_attack(state, wind) * 180.0 / np.pi
c = self.constants.mean_chord
delta_e = self.control_input.elevator_deflection
rot_airspeed_with_wind = calc_rotational_airspeed(state, wind)
ang_rate_y_r = rot_airspeed_with_wind[1]
# TODO: If C_D_q becomes non-zero, should test term containing C_D_q
return self.C_D_alpha(alpha, self.icing) + \
self.C_D_q(alpha, self.icing) * c / (2*airspeed) * ang_rate_y_r + \
self.C_D_delta_e(alpha, self.icing)*np.abs(delta_e)
def dynamics_kinetmatics_update(t: float, x: np.ndarray, u: np.ndarray, params: dict) -> np.ndarray:
prop = params['prop']
wind = params['wind'].get(t)
prop_updater = params.get('prop_updater', None)
state = State(init=x)
# wind_translational = inertial2body(wind[:3], state)
# wind = np.array([wind_translational[0], wind_translational[1], wind_translational[2], 0, 0, 0])
# Update the control inputs
prop.control_input = ControlInput(init=u)
if prop_updater is not None:
prop.update_params(prop_updater.get_param_dict(t))
update = np.zeros_like(x)
V_a = np.sqrt(np.sum(calc_airspeed(state, wind)**2))
b = prop.wing_span()
S = prop.wing_area()
c = prop.mean_chord()
S_prop = prop.propeller_area()
C_prop = prop.motor_efficiency_fact()
k_motor = prop.motor_constant()
qS = 0.5*AIR_DENSITY*S*V_a**2
force_aero_wind_frame = qS*np.array([-prop.drag_coeff(state, wind),
prop.side_force_coeff(state, wind),
-prop.lift_coeff(state, wind)])
force_aero_body_frame = wind2body(force_aero_wind_frame, state, wind)
moment_coeff_vec = np.array([b*prop.roll_moment_coeff(state, wind),
c*prop.pitch_moment_coeff(state, wind),
b*prop.yaw_moment_coeff(state, wind)])
moment_vec = qS*moment_coeff_vec
omega = np.array([state.ang_rate_x, state.ang_rate_y, state.ang_rate_z]) - wind[3:]
velocity = np.array([state.vx, state.vy, state.vz]) - wind[:3]
gravity_body_frame = inertial2body([0.0, 0.0, prop.mass()*GRAVITY_CONST], state)
F_propulsion = 0.5*AIR_DENSITY*S_prop*C_prop * \
np.array([(k_motor*prop.control_input.throttle)**2-V_a**2, 0, 0])
v_dot = (force_aero_body_frame + gravity_body_frame + F_propulsion) / \
prop.mass() - np.cross(omega, velocity)
# Velocity update
update[StateVecIndices.V_X:StateVecIndices.V_Z+1] = v_dot
# Momentum equations
omega_dot = \
prop.inv_inertia_matrix().dot(moment_vec - np.cross(omega, prop.inertia_matrix().dot(omega)))
update[StateVecIndices.ANG_RATE_X:StateVecIndices.ANG_RATE_Z+1] = omega_dot
# Kinematics
# Position updates
update[StateVecIndices.X:StateVecIndices.Z +
1] = body2inertial([state.vx, state.vy, state.vz], state)
# Angle updates
update[StateVecIndices.ROLL:StateVecIndices.YAW +
1] = body2euler([state.ang_rate_x, state.ang_rate_y, state.ang_rate_z], state)
return update
def yaw_moment_coeff(self, state: State, wind: np.ndarray) -> float:
airspeed = np.sqrt(np.sum(calc_airspeed(state, wind)**2))
beta = calc_angle_of_sideslip(state, wind) * 180.0 / np.pi
b = self.constants.wing_span
delta_a = self.control_input.aileron_deflection
rot_airspeed_with_wind = calc_rotational_airspeed(state, wind)
ang_rate_x_r = rot_airspeed_with_wind[0]
ang_rate_z_r = rot_airspeed_with_wind[2]
return self.C_n_beta(beta, self.icing) + \
(self.C_n_p(beta, self.icing) * b / (2*airspeed)) * ang_rate_x_r + \
(self.C_n_r(beta, self.icing) * b / (2*airspeed)) * \
ang_rate_z_r + self.C_n_delta_a(beta, self.icing)*delta_a
def test_dynamics_forces():
control_input = ControlInput()
prop = SimpleTestAircraftNoMoments(control_input)
t = 0
for i in range(-50, 101, 50):
control_input.throttle = 0.8
control_input.elevator_deflection = i
control_input.aileron_deflection = i
control_input.rudder_deflection = i
state = State()
state.vx = 20.0
state.vy = 1
state.vz = 0
params = {"prop": prop, "wind": no_wind()}
update = dynamics_kinetmatics_update(t=t,
x=state.state,
u=control_input.control_input,
params=params)
V_a = np.sqrt(np.sum(calc_airspeed(state, params['wind'].get(0.0))**2))
forces_aero_wind_frame = np.array([
-np.abs(control_input.elevator_deflection),
control_input.aileron_deflection, -control_input.rudder_deflection
])
forces_aero_body_frame = wind2body(forces_aero_wind_frame, state,
params['wind'].get(0))
force_propulsion = np.array([(2 * control_input.throttle)**2 - V_a**2,
0, 0])
force_gravity = inertial2body(
np.array([0, 0, prop.mass() * GRAVITY_CONST]), state)
forces_body = forces_aero_body_frame + force_propulsion + force_gravity
vx_update_expect = (1 / prop.mass()) * forces_body[0]
vy_update_expect = (1 / prop.mass()) * forces_body[1]
vz_update_expect = (1 / prop.mass()) * forces_body[2]
# No moments
ang_rate_x_update_expect = 0
ang_rate_y_update_expect = 0
ang_rate_z_update_expect = 0
assert np.allclose(vx_update_expect, update[6])
assert np.allclose(vy_update_expect, update[7])
assert np.allclose(vz_update_expect, update[8])
assert np.allclose(ang_rate_x_update_expect, update[9])
assert np.allclose(ang_rate_y_update_expect, update[10])
assert np.allclose(ang_rate_z_update_expect, update[11])
def lift_coeff_test_cases():
constants = SkywalkerX8Constants()
c = constants.mean_chord
state1 = State()
state1.vx = 20.0
state1.vz = 0.0
state2 = State()
state2.vx = 20.0
state2.vz = 0.0
state2.ang_rate_y = 5 * np.pi / 180
wind1 = np.zeros(6)
wind2 = np.zeros(6)
wind3 = np.zeros(6)
wind2[0] = 1.0
wind2[2] = -19.0 * np.tan(8 * np.pi / 180.0)
wind3[0] = 1.0
wind3[2] = -19.0 * np.tan(8 * np.pi / 180.0)
wind3[4] = 3 * np.pi / 180
ang_rate_y2 = state2.ang_rate_y - wind3[4]
airspeed2 = np.sqrt(np.sum(calc_airspeed(state2, wind2)**2))
zero_input = ControlInput()
elevator_input = ControlInput()
elevator_input.elevator_deflection = 2.0 * np.pi / 180.0
return [(state1, wind1, 0.0, zero_input, 0.030075562375465),
(state1, wind1, 1.0, zero_input, 0.018798581619545),
(state1, wind2, 0.0, zero_input, 0.609296679062686),
(state1, wind2, 1.0, zero_input, 0.454153721254944),
(state1, wind1, 0.0, elevator_input,
0.030075562375465 + 0.278 * elevator_input.elevator_deflection),
(state1, wind1, 1.0, elevator_input,
0.018798581619545 + 0.278 * elevator_input.elevator_deflection),
(state2, wind3, 0.0, elevator_input,
0.609296679062686 + 4.60 * c / (2 * airspeed2) * ang_rate_y2 +
0.278 * elevator_input.elevator_deflection),
(state2, wind3, 1.0, elevator_input,
0.454153721254944 - 3.51 * c / (2 * airspeed2) * ang_rate_y2 +
0.278 * elevator_input.elevator_deflection)]
def side_force_coeff(self, state: State, wind: np.ndarray) -> float:
V_a = np.sqrt(np.sum(calc_airspeed(state, wind)**2))
# Test func: F_side_force = delta_r -> C_lift = 2/(wing_area*AIR_DENSITY*V_a^2)
return 2 / (self.wing_area() * AIR_DENSITY *
V_a**2) * self.control_input.rudder_deflection
def drag_coeff(self, state: State, wind: np.ndarray) -> float:
V_a = np.sqrt(np.sum(calc_airspeed(state, wind)**2))
# Test func: F_drag = abs(delta_e)-> C_drag = (2/(wing_area*AIR_DENSITY*V_a^2)* abs(delta_e)
return 2 / (self.wing_area() * AIR_DENSITY * V_a**2) * np.abs(
self.control_input.elevator_deflection)
def yaw_moment_coeff(self, state: State, wind: np.ndarray) -> float:
V_a = np.sqrt(np.sum(calc_airspeed(state, wind)**2))
# Test func: yaw_moment = delta_r
# -> C_yaw_moment = (2/(wing_area*AIR_DENSITY*wing_span*V_a^2)*delta_r
return 2/(self.wing_area()*self.wing_span()*AIR_DENSITY*V_a**2) *\
self.control_input.rudder_deflection
def pitch_moment_coeff(self, state: State, wind: np.ndarray) -> float:
V_a = np.sqrt(np.sum(calc_airspeed(state, wind)**2))
# Test func: Pitch_moment = delta_a
# -> C_pitch_moment = (2/(wing_area*mean_chord*AIR_DENSITY*V_a^2)*delta_a
return 2/(self.wing_area()*self.mean_chord()*AIR_DENSITY*V_a**2) *\
self.control_input.aileron_deflection
def roll_moment_coeff(self, state: State, wind: np.ndarray) -> float:
V_a = np.sqrt(np.sum(calc_airspeed(state, wind)**2))
# Test func: Roll_moment = delta_e
# -> C_roll_moment = (2/(wing_area*wing_span*AIR_DENSITY*V_a^2)*delta_e
return 2/(self.wing_area()*self.wing_span()*AIR_DENSITY*V_a**2) *\
self.control_input.elevator_deflection