diff --git a/examples/example_001.py b/examples/example_001.py index 2803c98..8acce8d 100644 --- a/examples/example_001.py +++ b/examples/example_001.py @@ -4,148 +4,108 @@ Copyright (c) AeroPython Development Team. Distributed under the terms of the MIT License. -Example with trimmed aircraft. +Example +------- + +Cessna 310, ISA1976 integrated with Flat Earth (euler angles). + +Example with trimmed aircraft: stationary, horizontal, symmetric, +wings level flight. + The main purpose of this example is to check if the aircraft trimmed in a given state maintains the trimmed flight condition. -Trimmed in stationary, horizontal, symmetric, wings level flight. """ - import numpy as np import matplotlib.pyplot as plt - -from pyfme.aircrafts import cessna_310 -from pyfme.models.system import System -from pyfme.utils.trimmer import steady_state_flight_trim -from pyfme.utils.anemometry import calculate_alpha_beta_TAS -from pyfme.environment.isa import atm - - -if __name__ == '__main__': - - # Aircraft parameters. - mass, inertia = cessna_310.mass_and_inertial_data() - - # Initial conditions. - TAS_ = 312 * 0.3048 # m/s - h = 8000 * 0.3048 # m - psi_0 = 3 # rad - x_0, y_0 = 0, 0 # m - - # Trimming. - trim_results = steady_state_flight_trim(cessna_310, h, TAS_, gamma=0, - turn_rate=0) - - lin_vel, ang_vel, theta, phi, alpha_, beta_, control_vector = trim_results - - # Time. - t0 = 0 # s - tf = 30 # s - dt = 1e-2 # s - - time = np.arange(t0, tf, dt) - - # Results initialization. - results = np.zeros([time.size, 12]) - - results[0, 0:3] = lin_vel - results[0, 3:6] = ang_vel - results[0, 6:9] = theta, phi, psi_0 - results[0, 9:12] = x_0, y_0, h - alpha = np.empty_like(time) - alpha[0] = alpha_ - beta = np.empty_like(time) - beta[0] = beta_ - TAS = np.empty_like(time) - TAS[0] = TAS_ - - # Linear Momentum and Angular Momentum eqs. - equations = System(integrator='dopri5', - model='euler_flat_earth', - jac=False) - u, v, w = lin_vel - p, q, r = ang_vel - - equations.set_initial_values(u, v, w, - p, q, r, - theta, phi, psi_0, - x_0, y_0, h) - - _, _, rho, _ = atm(h) - - # Define control vectors. - d_e, d_a, d_r, d_t = control_vector - - attitude = theta, phi, psi_0 - - # Rename function to make it shorter - forces_and_moments = cessna_310.get_forces_and_moments - for ii, t in enumerate(time[1:]): - - forces, moments = forces_and_moments(TAS[ii], rho, alpha[ii], beta[ii], - d_e, 0, d_a, d_r, d_t, attitude) - - results[ii+1, :] = equations.propagate(mass, inertia, forces, moments, - dt) - - lin_vel = results[ii+1, 0:3] - ang_vel = results[ii+1, 3:6] - attitude = results[ii+1, 6:9] - position = results[ii+1, 9:12] - - alpha[ii+1], beta[ii+1], TAS[ii+1] = calculate_alpha_beta_TAS(*lin_vel) - - _, _, rho, _ = atm(position[2]) - - velocities = results[:, 0:6] - attitude_angles = results[:, 6:9] - position = results[:, 9:12] - - # PLOTS - plt.close('all') - plt.style.use('ggplot') - - plt.figure('pos') - plt.plot(time, position[:, 0], label='x') - plt.plot(time, position[:, 1], label='y') - plt.plot(time, position[:, 2], label='z') - plt.xlabel('time (s)') - plt.ylabel('position (m)') - plt.legend() - - plt.figure('angles') - plt.plot(time, attitude_angles[:, 0], label='theta') - plt.plot(time, attitude_angles[:, 1], label='phi') - plt.plot(time, attitude_angles[:, 2], label='psi') - plt.xlabel('time (s)') - plt.ylabel('attitude (rad)') - plt.legend() - - plt.figure('velocities') - plt.plot(time, velocities[:, 0], label='u') - plt.plot(time, velocities[:, 1], label='v') - plt.plot(time, velocities[:, 2], label='w') - plt.plot(time, TAS, label='TAS') - plt.xlabel('time (s)') - plt.ylabel('velocity (m/s)') - plt.legend() - - plt.figure('ang velocities') - plt.plot(time, velocities[:, 3], label='p') - plt.plot(time, velocities[:, 4], label='q') - plt.plot(time, velocities[:, 5], label='r') - plt.xlabel('time (s)') - plt.ylabel('angular velocity (rad/s)') - plt.legend() - - plt.figure('aero angles') - plt.plot(time, alpha, label='alpha') - plt.plot(time, beta, label='beta') - plt.xlabel('time (s)') - plt.ylabel('angle (rad)') - plt.legend() - - plt.figure('2D Trajectory') - plt.plot(position[:, 0], position[:, 1]) - plt.xlabel('X (m)') - plt.ylabel('Y (m)') - plt.legend() +from mpl_toolkits.mplot3d import Axes3D + +from pyfme.aircrafts import Cessna310 +from pyfme.environment.environment import Environment +from pyfme.environment.atmosphere import ISA1976 +from pyfme.environment.gravity import VerticalConstant +from pyfme.environment.wind import NoWind +from pyfme.models.systems import EulerFlatEarth +from pyfme.simulator import BatchSimulation +from pyfme.utils.trimmer import steady_state_flight_trimmer + +aircraft = Cessna310() +atmosphere = ISA1976() +gravity = VerticalConstant() +wind = NoWind() +environment = Environment(atmosphere, gravity, wind) + +# Initial conditions. +TAS = 312.5 * 0.3048 # m/s +h0 = 8000 * 0.3048 # m +psi0 = 1.0 # rad +x0, y0 = 0, 0 # m +turn_rate = 0.0 # rad/s +gamma0 = 0.0 # rad + +system = EulerFlatEarth(lat=0, lon=0, h=h0, psi=psi0, x_earth=x0, y_earth=y0) + +not_trimmed_controls = {'delta_elevator': 0.05, + 'hor_tail_incidence': 0.00, + 'delta_aileron': 0.01 * np.sign(turn_rate), + 'delta_rudder': 0.01 * np.sign(turn_rate), + 'delta_t': 0.5} + +controls2trim = ['delta_elevator', 'delta_aileron', 'delta_rudder', 'delta_t'] + +trimmed_ac, trimmed_sys, trimmed_env, results = steady_state_flight_trimmer( + aircraft, system, environment, TAS=TAS, controls_0=not_trimmed_controls, + controls2trim=controls2trim, gamma=gamma0, turn_rate=turn_rate, verbose=2) + +print(results) + +my_simulation = BatchSimulation(trimmed_ac, trimmed_sys, trimmed_env) + +tfin = 150 # seconds +N = tfin * 100 + 1 +time = np.linspace(0, tfin, N) +initial_controls = trimmed_ac.controls + +controls = {} +for control_name, control_value in initial_controls.items(): + controls[control_name] = np.ones_like(time) * control_value + +my_simulation.set_controls(time, controls) + +par_list = ['x_earth', 'y_earth', 'height', + 'psi', 'theta', 'phi', + 'u', 'v', 'w', + 'v_north', 'v_east', 'v_down', + 'p', 'q', 'r', + 'alpha', 'beta', 'TAS', + 'F_xb', 'F_yb', 'F_zb', + 'M_xb', 'M_yb', 'M_zb'] + +my_simulation.set_par_dict(par_list) +my_simulation.run_simulation() + +# print(my_simulation.par_dict) + +plt.style.use('ggplot') + +for ii in range(len(par_list) // 3): + three_params = par_list[3*ii:3*ii+3] + fig, ax = plt.subplots(3, 1, sharex=True) + for jj, par in enumerate(three_params): + ax[jj].plot(time, my_simulation.par_dict[par]) + ax[jj].set_ylabel(par) + ax[jj].set_xlabel('time (s)') + +fig = plt.figure() +ax = Axes3D(fig) +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height']) + +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height'] * 0) +ax.set_xlabel('x_earth') +ax.set_ylabel('y_earth') +ax.set_zlabel('z_earth') + +plt.show() diff --git a/examples/example_002.py b/examples/example_002.py index 40c8b9a..2eb2ec5 100644 --- a/examples/example_002.py +++ b/examples/example_002.py @@ -4,148 +4,109 @@ Copyright (c) AeroPython Development Team. Distributed under the terms of the MIT License. -Example with trimmed aircraft. +Example +------- + +Cessna 310, ISA1976 integrated with Flat Earth (euler angles). + +Example with trimmed aircraft: stationary descent, symmetric, wings level +flight. + The main purpose of this example is to check if the aircraft trimmed in a given state maintains the trimmed flight condition. -Trimmed in stationary descent, symmetric, wings level flight. """ import numpy as np import matplotlib.pyplot as plt - -from pyfme.aircrafts import cessna_310 -from pyfme.models.system import System -from pyfme.utils.trimmer import steady_state_flight_trim -from pyfme.utils.anemometry import calculate_alpha_beta_TAS -from pyfme.environment.isa import atm - - -if __name__ == '__main__': - - # Aircraft parameters. - mass, inertia = cessna_310.mass_and_inertial_data() - - # Initial conditions. - TAS_ = 312 * 0.3048 # m/s - h = 8000 * 0.3048 # m - psi_0 = 3 # rad - x_0, y_0 = 0, 0 # m - - # Trimming. - trim_results = steady_state_flight_trim(cessna_310, h, TAS_, gamma=0.20, - turn_rate=0) - - lin_vel, ang_vel, theta, phi, alpha_, beta_, control_vector = trim_results - - # Time. - t0 = 0 # s - tf = 120 # s - dt = 1e-2 # s - - time = np.arange(t0, tf, dt) - - # Results initialization. - results = np.zeros([time.size, 12]) - - results[0, 0:3] = lin_vel - results[0, 3:6] = ang_vel - results[0, 6:9] = theta, phi, psi_0 - results[0, 9:12] = x_0, y_0, h - alpha = np.empty_like(time) - alpha[0] = alpha_ - beta = np.empty_like(time) - beta[0] = beta_ - TAS = np.empty_like(time) - TAS[0] = TAS_ - - # Linear Momentum and Angular Momentum eqs. - equations = System(integrator='dopri5', - model='euler_flat_earth', - jac=False) - u, v, w = lin_vel - p, q, r = ang_vel - - equations.set_initial_values(u, v, w, - p, q, r, - theta, phi, psi_0, - x_0, y_0, h) - - _, _, rho, _ = atm(h) - - # Define control vectors. - d_e, d_a, d_r, d_t = control_vector - - attitude = theta, phi, psi_0 - - # Rename function to make it shorter - forces_and_moments = cessna_310.get_forces_and_moments - for ii, t in enumerate(time[1:]): - - forces, moments = forces_and_moments(TAS[ii], rho, alpha[ii], beta[ii], - d_e, 0, d_a, d_r, d_t, attitude) - - results[ii+1, :] = equations.propagate(mass, inertia, forces, moments, - dt) - - lin_vel = results[ii+1, 0:3] - ang_vel = results[ii+1, 3:6] - attitude = results[ii+1, 6:9] - position = results[ii+1, 9:12] - - alpha[ii+1], beta[ii+1], TAS[ii+1] = calculate_alpha_beta_TAS(*lin_vel) - - _, _, rho, _ = atm(position[2]) - - velocities = results[:, 0:6] - attitude_angles = results[:, 6:9] - position = results[:, 9:12] - - # PLOTS - plt.close('all') - plt.style.use('ggplot') - - plt.figure('pos') - plt.plot(time, position[:, 0], label='x') - plt.plot(time, position[:, 1], label='y') - plt.plot(time, position[:, 2], label='z') - plt.xlabel('time (s)') - plt.ylabel('position (m)') - plt.legend() - - plt.figure('angles') - plt.plot(time, attitude_angles[:, 0], label='theta') - plt.plot(time, attitude_angles[:, 1], label='phi') - plt.plot(time, attitude_angles[:, 2], label='psi') - plt.xlabel('time (s)') - plt.ylabel('attitude (rad)') - plt.legend() - - plt.figure('velocities') - plt.plot(time, velocities[:, 0], label='u') - plt.plot(time, velocities[:, 1], label='v') - plt.plot(time, velocities[:, 2], label='w') - plt.plot(time, TAS, label='TAS') - plt.xlabel('time (s)') - plt.ylabel('velocity (m/s)') - plt.legend() - - plt.figure('ang velocities') - plt.plot(time, velocities[:, 3], label='p') - plt.plot(time, velocities[:, 4], label='q') - plt.plot(time, velocities[:, 5], label='r') - plt.xlabel('time (s)') - plt.ylabel('angular velocity (rad/s)') - plt.legend() - - plt.figure('aero angles') - plt.plot(time, alpha, label='alpha') - plt.plot(time, beta, label='beta') - plt.xlabel('time (s)') - plt.ylabel('angle (rad)') - plt.legend() - - plt.figure('2D Trajectory') - plt.plot(position[:, 0], position[:, 1]) - plt.xlabel('X (m)') - plt.ylabel('Y (m)') - plt.legend() +from mpl_toolkits.mplot3d import Axes3D + +from pyfme.aircrafts import Cessna310 +from pyfme.environment.environment import Environment +from pyfme.environment.atmosphere import ISA1976 +from pyfme.environment.gravity import VerticalConstant +from pyfme.environment.wind import NoWind +from pyfme.models.systems import EulerFlatEarth +from pyfme.simulator import BatchSimulation +from pyfme.utils.trimmer import steady_state_flight_trimmer + +aircraft = Cessna310() +atmosphere = ISA1976() +gravity = VerticalConstant() +wind = NoWind() +environment = Environment(atmosphere, gravity, wind) + +# Initial conditions. +TAS = 312.5 * 0.3048 # m/s +h0 = 8000 * 0.3048 # m +psi0 = 1 # rad +x0, y0 = 0, 0 # m +turn_rate = 0.0 # rad/s +gamma0 = -0.05 # rad + +system = EulerFlatEarth(lat=0, lon=0, h=h0, psi=psi0, x_earth=x0, y_earth=y0) + +not_trimmed_controls = {'delta_elevator': 0.05, + 'hor_tail_incidence': 0.00, + 'delta_aileron': 0.01 * np.sign(turn_rate), + 'delta_rudder': 0.01 * np.sign(turn_rate), + 'delta_t': 0.5} + +controls2trim = ['delta_elevator', 'delta_aileron', 'delta_rudder', 'delta_t'] + +trimmed_ac, trimmed_sys, trimmed_env, results = steady_state_flight_trimmer( + aircraft, system, environment, TAS=TAS, controls_0=not_trimmed_controls, + controls2trim=controls2trim, gamma=gamma0, turn_rate=turn_rate, verbose=2) + +print(results) + +my_simulation = BatchSimulation(trimmed_ac, trimmed_sys, trimmed_env) + +tfin = 150 # seconds +N = tfin * 100 + 1 +time = np.linspace(0, tfin, N) +initial_controls = trimmed_ac.controls + +controls = {} +for control_name, control_value in initial_controls.items(): + controls[control_name] = np.ones_like(time) * control_value + +my_simulation.set_controls(time, controls) + +par_list = ['x_earth', 'y_earth', 'height', + 'psi', 'theta', 'phi', + 'u', 'v', 'w', + 'v_north', 'v_east', 'v_down', + 'p', 'q', 'r', + 'alpha', 'beta', 'TAS', + 'F_xb', 'F_yb', 'F_zb', + 'M_xb', 'M_yb', 'M_zb'] + +my_simulation.set_par_dict(par_list) +my_simulation.run_simulation() + +# print(my_simulation.par_dict) + +plt.style.use('ggplot') + +for ii in range(len(par_list) // 3): + three_params = par_list[3*ii:3*ii+3] + fig, ax = plt.subplots(3, 1, sharex=True) + for jj, par in enumerate(three_params): + ax[jj].plot(time, my_simulation.par_dict[par]) + ax[jj].set_ylabel(par) + ax[jj].set_xlabel('time (s)') + +fig = plt.figure() +ax = Axes3D(fig) +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height']) + +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height'] * 0) +ax.set_xlabel('x_earth') +ax.set_ylabel('y_earth') +ax.set_zlabel('z_earth') + +plt.show() diff --git a/examples/example_003.py b/examples/example_003.py index e4d22a9..045d30f 100644 --- a/examples/example_003.py +++ b/examples/example_003.py @@ -4,148 +4,109 @@ Copyright (c) AeroPython Development Team. Distributed under the terms of the MIT License. -Example with trimmed aircraft. +Example +------- + +Cessna 310, ISA1976 integrated with Flat Earth (euler angles). + +Example with trimmed aircraft: stationary ascent, symmetric, wings level +flight. + The main purpose of this example is to check if the aircraft trimmed in a given state maintains the trimmed flight condition. -Trimmed in stationary, horizontal, turn. """ import numpy as np import matplotlib.pyplot as plt - -from pyfme.aircrafts import cessna_310 -from pyfme.models.system import System -from pyfme.utils.trimmer import steady_state_flight_trim -from pyfme.utils.anemometry import calculate_alpha_beta_TAS -from pyfme.environment.isa import atm - - -if __name__ == '__main__': - - # Aircraft parameters. - mass, inertia = cessna_310.mass_and_inertial_data() - - # Initial conditions. - TAS_ = 312 * 0.3048 # m/s - h = 8000 * 0.3048 # m - psi_0 = 3 # rad - x_0, y_0 = 0, 0 # m - - # Trimming. - trim_results = steady_state_flight_trim(cessna_310, h, TAS_, gamma=0.02, - turn_rate=0.02) - - lin_vel, ang_vel, theta, phi, alpha_, beta_, control_vector = trim_results - - # Time. - t0 = 0 # s - tf = 120 # s - dt = 1e-2 # s - - time = np.arange(t0, tf, dt) - - # Results initialization. - results = np.zeros([time.size, 12]) - - results[0, 0:3] = lin_vel - results[0, 3:6] = ang_vel - results[0, 6:9] = theta, phi, psi_0 - results[0, 9:12] = x_0, y_0, h - alpha = np.empty_like(time) - alpha[0] = alpha_ - beta = np.empty_like(time) - beta[0] = beta_ - TAS = np.empty_like(time) - TAS[0] = TAS_ - - # Linear Momentum and Angular Momentum eqs. - equations = System(integrator='dopri5', - model='euler_flat_earth', - jac=False) - u, v, w = lin_vel - p, q, r = ang_vel - - equations.set_initial_values(u, v, w, - p, q, r, - theta, phi, psi_0, - x_0, y_0, h) - - _, _, rho, _ = atm(h) - - # Define control vectors. - d_e, d_a, d_r, d_t = control_vector - - attitude = theta, phi, psi_0 - - # Rename function to make it shorter - forces_and_moments = cessna_310.get_forces_and_moments - for ii, t in enumerate(time[1:]): - - forces, moments = forces_and_moments(TAS[ii], rho, alpha[ii], beta[ii], - d_e, 0, d_a, d_r, d_t, attitude) - - results[ii+1, :] = equations.propagate(mass, inertia, forces, moments, - dt) - - lin_vel = results[ii+1, 0:3] - ang_vel = results[ii+1, 3:6] - attitude = results[ii+1, 6:9] - position = results[ii+1, 9:12] - - alpha[ii+1], beta[ii+1], TAS[ii+1] = calculate_alpha_beta_TAS(*lin_vel) - - _, _, rho, _ = atm(position[2]) - - velocities = results[:, 0:6] - attitude_angles = results[:, 6:9] - position = results[:, 9:12] - - # PLOTS - plt.close('all') - plt.style.use('ggplot') - - plt.figure('pos') - plt.plot(time, position[:, 0], label='x') - plt.plot(time, position[:, 1], label='y') - plt.plot(time, position[:, 2], label='z') - plt.xlabel('time (s)') - plt.ylabel('position (m)') - plt.legend() - - plt.figure('angles') - plt.plot(time, attitude_angles[:, 0], label='theta') - plt.plot(time, attitude_angles[:, 1], label='phi') - plt.plot(time, attitude_angles[:, 2], label='psi') - plt.xlabel('time (s)') - plt.ylabel('attitude (rad)') - plt.legend() - - plt.figure('velocities') - plt.plot(time, velocities[:, 0], label='u') - plt.plot(time, velocities[:, 1], label='v') - plt.plot(time, velocities[:, 2], label='w') - plt.plot(time, TAS, label='TAS') - plt.xlabel('time (s)') - plt.ylabel('velocity (m/s)') - plt.legend() - - plt.figure('ang velocities') - plt.plot(time, velocities[:, 3], label='p') - plt.plot(time, velocities[:, 4], label='q') - plt.plot(time, velocities[:, 5], label='r') - plt.xlabel('time (s)') - plt.ylabel('angular velocity (rad/s)') - plt.legend() - - plt.figure('aero angles') - plt.plot(time, alpha, label='alpha') - plt.plot(time, beta, label='beta') - plt.xlabel('time (s)') - plt.ylabel('angle (rad)') - plt.legend() - - plt.figure('2D Trajectory') - plt.plot(position[:, 0], position[:, 1]) - plt.xlabel('X (m)') - plt.ylabel('Y (m)') - plt.legend() +from mpl_toolkits.mplot3d import Axes3D + +from pyfme.aircrafts import Cessna310 +from pyfme.environment.environment import Environment +from pyfme.environment.atmosphere import ISA1976 +from pyfme.environment.gravity import VerticalConstant +from pyfme.environment.wind import NoWind +from pyfme.models.systems import EulerFlatEarth +from pyfme.simulator import BatchSimulation +from pyfme.utils.trimmer import steady_state_flight_trimmer + +aircraft = Cessna310() +atmosphere = ISA1976() +gravity = VerticalConstant() +wind = NoWind() +environment = Environment(atmosphere, gravity, wind) + +# Initial conditions. +TAS = 312.5 * 0.3048 # m/s +h0 = 8000 * 0.3048 # m +psi0 = 1 # rad +x0, y0 = 0, 0 # m +turn_rate = 0.0 # rad/s +gamma0 = +0.05 # rad + +system = EulerFlatEarth(lat=0, lon=0, h=h0, psi=psi0, x_earth=x0, y_earth=y0) + +not_trimmed_controls = {'delta_elevator': 0.05, + 'hor_tail_incidence': 0.00, + 'delta_aileron': 0.01 * np.sign(turn_rate), + 'delta_rudder': 0.01 * np.sign(turn_rate), + 'delta_t': 0.5} + +controls2trim = ['delta_elevator', 'delta_aileron', 'delta_rudder', 'delta_t'] + +trimmed_ac, trimmed_sys, trimmed_env, results = steady_state_flight_trimmer( + aircraft, system, environment, TAS=TAS, controls_0=not_trimmed_controls, + controls2trim=controls2trim, gamma=gamma0, turn_rate=turn_rate, verbose=2) + +print(results) + +my_simulation = BatchSimulation(trimmed_ac, trimmed_sys, trimmed_env) + +tfin = 150 # seconds +N = tfin * 100 + 1 +time = np.linspace(0, tfin, N) +initial_controls = trimmed_ac.controls + +controls = {} +for control_name, control_value in initial_controls.items(): + controls[control_name] = np.ones_like(time) * control_value + +my_simulation.set_controls(time, controls) + +par_list = ['x_earth', 'y_earth', 'height', + 'psi', 'theta', 'phi', + 'u', 'v', 'w', + 'v_north', 'v_east', 'v_down', + 'p', 'q', 'r', + 'alpha', 'beta', 'TAS', + 'F_xb', 'F_yb', 'F_zb', + 'M_xb', 'M_yb', 'M_zb'] + +my_simulation.set_par_dict(par_list) +my_simulation.run_simulation() + +# print(my_simulation.par_dict) + +plt.style.use('ggplot') + +for ii in range(len(par_list) // 3): + three_params = par_list[3*ii:3*ii+3] + fig, ax = plt.subplots(3, 1, sharex=True) + for jj, par in enumerate(three_params): + ax[jj].plot(time, my_simulation.par_dict[par]) + ax[jj].set_ylabel(par) + ax[jj].set_xlabel('time (s)') + +fig = plt.figure() +ax = Axes3D(fig) +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height']) + +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height'] * 0) +ax.set_xlabel('x_earth') +ax.set_ylabel('y_earth') +ax.set_zlabel('z_earth') + +plt.show() diff --git a/examples/example_004.py b/examples/example_004.py index 4695947..d905149 100644 --- a/examples/example_004.py +++ b/examples/example_004.py @@ -4,162 +4,108 @@ Copyright (c) AeroPython Development Team. Distributed under the terms of the MIT License. -Example with trimmed aircraft. -The main purpose of this example is to see the evolution of the aircraft after -a longitudinal perturbation (delta doublet). -Trimmed in stationary, horizontal, symmetric, wings level flight. +Example +------- + +Cessna 310, ISA1976 integrated with Flat Earth (euler angles). + +Example with trimmed aircraft: stationary, horizontal turn. + +The main purpose of this example is to check if the aircraft trimmed in a given +state maintains the trimmed flight condition. """ import numpy as np import matplotlib.pyplot as plt - -from pyfme.aircrafts import cessna_310 -from pyfme.models.system import System -from pyfme.utils.trimmer import steady_state_flight_trim -from pyfme.utils.anemometry import calculate_alpha_beta_TAS -from pyfme.environment.isa import atm - - -if __name__ == '__main__': - - # Aircraft parameters. - mass, inertia = cessna_310.mass_and_inertial_data() - - # Initial conditions. - TAS_ = 312 * 0.3048 # m/s - h = 8000 * 0.3048 # m - psi_0 = 3 # rad - x_0, y_0 = 0, 0 # m - - # Trimming. - trim_results = steady_state_flight_trim(cessna_310, h, TAS_, gamma=0, - turn_rate=0) - - lin_vel, ang_vel, theta, phi, alpha_, beta_, control_vector = trim_results - - # Time. - t0 = 0 # s - tf = 30 # s - dt = 1e-2 # s - - time = np.arange(t0, tf, dt) - - # Results initialization. - results = np.zeros([time.size, 12]) - - results[0, 0:3] = lin_vel - results[0, 3:6] = ang_vel - results[0, 6:9] = theta, phi, psi_0 - results[0, 9:12] = x_0, y_0, h - alpha = np.empty_like(time) - alpha[0] = alpha_ - beta = np.empty_like(time) - beta[0] = beta_ - TAS = np.empty_like(time) - TAS[0] = TAS_ - - # Linear Momentum and Angular Momentum eqs. - equations = System(integrator='dopri5', - model='euler_flat_earth', - jac=False) - u, v, w = lin_vel - p, q, r = ang_vel - - equations.set_initial_values(u, v, w, - p, q, r, - theta, phi, psi_0, - x_0, y_0, h) - - _, _, rho, _ = atm(h) - - # Define control vectors. - delta_e, delta_ail, delta_r, delta_t = control_vector - d_e = np.ones_like(time) * delta_e - d_e[np.where(time<2)] = delta_e * 1.30 - d_e[np.where(time<1)] = delta_e * 0.70 - d_a = np.ones_like(time) * delta_ail - d_r = np.ones_like(time) * delta_r - d_t = np.ones_like(time) * delta_t - - attitude = theta, phi, psi_0 - - # Rename function to make it shorter - forces_and_moments = cessna_310.get_forces_and_moments - for ii, t in enumerate(time[1:]): - - forces, moments = forces_and_moments( - TAS[ii], rho, alpha[ii], beta[ii], d_e[ii], 0, - d_a[ii], d_r[ii], d_t[ii], attitude) - - results[ii+1, :] = equations.propagate(mass, inertia, forces, moments, - dt) - - lin_vel = results[ii+1, 0:3] - ang_vel = results[ii+1, 3:6] - attitude = results[ii+1, 6:9] - position = results[ii+1, 9:12] - - alpha[ii+1], beta[ii+1], TAS[ii+1] = calculate_alpha_beta_TAS(*lin_vel) - - _, _, rho, _ = atm(position[2]) - - velocities = results[:, 0:6] - attitude_angles = results[:, 6:9] - position = results[:, 9:12] - - # PLOTS - plt.close('all') - plt.style.use('ggplot') - - plt.figure('pos') - plt.plot(time, position[:, 0], label='x') - plt.plot(time, position[:, 1], label='y') - plt.plot(time, position[:, 2], label='z') - plt.xlabel('time (s)') - plt.ylabel('position (m)') - plt.legend() - - plt.figure('angles') - plt.plot(time, attitude_angles[:, 0], label='theta') - plt.plot(time, attitude_angles[:, 1], label='phi') - plt.plot(time, attitude_angles[:, 2], label='psi') - plt.xlabel('time (s)') - plt.ylabel('attitude (rad)') - plt.legend() - - plt.figure('velocities') - plt.plot(time, velocities[:, 0], label='u') - plt.plot(time, velocities[:, 1], label='v') - plt.plot(time, velocities[:, 2], label='w') - plt.plot(time, TAS, label='TAS') - plt.xlabel('time (s)') - plt.ylabel('velocity (m/s)') - plt.legend() - - plt.figure('ang velocities') - plt.plot(time, velocities[:, 3], label='p') - plt.plot(time, velocities[:, 4], label='q') - plt.plot(time, velocities[:, 5], label='r') - plt.xlabel('time (s)') - plt.ylabel('angular velocity (rad/s)') - plt.legend() - - plt.figure('aero angles') - plt.plot(time, alpha, label='alpha') - plt.plot(time, beta, label='beta') - plt.xlabel('time (s)') - plt.ylabel('angle (rad)') - plt.legend() - - plt.figure('2D Trajectory') - plt.plot(position[:, 0], position[:, 1]) - plt.xlabel('X (m)') - plt.ylabel('Y (m)') - plt.legend() - - plt.figure() - plt.plot(time, alpha, label='alpha') - plt.plot(time, attitude_angles[:, 0], label='theta') - plt.plot(time, d_e, label='delta_e') - plt.plot(time, velocities[:, 4], label='q') - plt.legend() +from mpl_toolkits.mplot3d import Axes3D + +from pyfme.aircrafts import Cessna310 +from pyfme.environment.environment import Environment +from pyfme.environment.atmosphere import ISA1976 +from pyfme.environment.gravity import VerticalConstant +from pyfme.environment.wind import NoWind +from pyfme.models.systems import EulerFlatEarth +from pyfme.simulator import BatchSimulation +from pyfme.utils.trimmer import steady_state_flight_trimmer + +aircraft = Cessna310() +atmosphere = ISA1976() +gravity = VerticalConstant() +wind = NoWind() +environment = Environment(atmosphere, gravity, wind) + +# Initial conditions. +TAS = 312.5 * 0.3048 # m/s +h0 = 8000 * 0.3048 # m +psi0 = 1 # rad +x0, y0 = 0, 0 # m +turn_rate = 0.1 # rad/s +gamma0 = 0.00 # rad + +system = EulerFlatEarth(lat=0, lon=0, h=h0, psi=psi0, x_earth=x0, y_earth=y0) + +not_trimmed_controls = {'delta_elevator': 0.05, + 'hor_tail_incidence': 0.00, + 'delta_aileron': 0.01 * np.sign(turn_rate), + 'delta_rudder': 0.01 * np.sign(turn_rate), + 'delta_t': 0.5} + +controls2trim = ['delta_elevator', 'delta_aileron', 'delta_rudder', 'delta_t'] + +trimmed_ac, trimmed_sys, trimmed_env, results = steady_state_flight_trimmer( + aircraft, system, environment, TAS=TAS, controls_0=not_trimmed_controls, + controls2trim=controls2trim, gamma=gamma0, turn_rate=turn_rate, verbose=2) + +print(results) + +my_simulation = BatchSimulation(trimmed_ac, trimmed_sys, trimmed_env) + +tfin = 150 # seconds +N = tfin * 100 + 1 +time = np.linspace(0, tfin, N) +initial_controls = trimmed_ac.controls + +controls = {} +for control_name, control_value in initial_controls.items(): + controls[control_name] = np.ones_like(time) * control_value + +my_simulation.set_controls(time, controls) + +par_list = ['x_earth', 'y_earth', 'height', + 'psi', 'theta', 'phi', + 'u', 'v', 'w', + 'v_north', 'v_east', 'v_down', + 'p', 'q', 'r', + 'alpha', 'beta', 'TAS', + 'F_xb', 'F_yb', 'F_zb', + 'M_xb', 'M_yb', 'M_zb'] + +my_simulation.set_par_dict(par_list) +my_simulation.run_simulation() + +# print(my_simulation.par_dict) + +plt.style.use('ggplot') + +for ii in range(len(par_list) // 3): + three_params = par_list[3*ii:3*ii+3] + fig, ax = plt.subplots(3, 1, sharex=True) + for jj, par in enumerate(three_params): + ax[jj].plot(time, my_simulation.par_dict[par]) + ax[jj].set_ylabel(par) + ax[jj].set_xlabel('time (s)') + +fig = plt.figure() +ax = Axes3D(fig) +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height']) + +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height'] * 0) +ax.set_xlabel('x_earth') +ax.set_ylabel('y_earth') +ax.set_zlabel('z_earth') + +plt.show() diff --git a/examples/example_005.py b/examples/example_005.py new file mode 100644 index 0000000..4385329 --- /dev/null +++ b/examples/example_005.py @@ -0,0 +1,111 @@ +# -*- coding: utf-8 -*- +""" +Python Flight Mechanics Engine (PyFME). +Copyright (c) AeroPython Development Team. +Distributed under the terms of the MIT License. + +Example +------- + +Cessna 310, ISA1976 integrated with Flat Earth (euler angles). + +Example with trimmed aircraft: stationary, turn during ascent. + +The main purpose of this example is to check if the aircraft trimmed in a given +state maintains the trimmed flight condition. +""" + +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D + +from pyfme.aircrafts import Cessna310 +from pyfme.environment.environment import Environment +from pyfme.environment.atmosphere import ISA1976 +from pyfme.environment.gravity import VerticalConstant +from pyfme.environment.wind import NoWind +from pyfme.models.systems import EulerFlatEarth +from pyfme.simulator import BatchSimulation +from pyfme.utils.trimmer import steady_state_flight_trimmer + +aircraft = Cessna310() +atmosphere = ISA1976() +gravity = VerticalConstant() +wind = NoWind() +environment = Environment(atmosphere, gravity, wind) + +# Initial conditions. +TAS = 312.5 * 0.3048 # m/s +h0 = 8000 * 0.3048 # m +psi0 = 1 # rad +x0, y0 = 0, 0 # m +turn_rate = 0.1 # rad/s +gamma0 = 0.05 # rad + +system = EulerFlatEarth(lat=0, lon=0, h=h0, psi=psi0, x_earth=x0, y_earth=y0) + +not_trimmed_controls = {'delta_elevator': 0.05, + 'hor_tail_incidence': 0.00, + 'delta_aileron': 0.01 * np.sign(turn_rate), + 'delta_rudder': 0.01 * np.sign(turn_rate), + 'delta_t': 0.5} + +controls2trim = ['delta_elevator', 'delta_aileron', 'delta_rudder', 'delta_t'] + +trimmed_ac, trimmed_sys, trimmed_env, results = steady_state_flight_trimmer( + aircraft, system, environment, TAS=TAS, controls_0=not_trimmed_controls, + controls2trim=controls2trim, gamma=gamma0, turn_rate=turn_rate, verbose=2) + +print(results) + +my_simulation = BatchSimulation(trimmed_ac, trimmed_sys, trimmed_env) + +tfin = 150 # seconds +N = tfin * 100 + 1 +time = np.linspace(0, tfin, N) +initial_controls = trimmed_ac.controls + +controls = {} +for control_name, control_value in initial_controls.items(): + controls[control_name] = np.ones_like(time) * control_value + +my_simulation.set_controls(time, controls) + +par_list = ['x_earth', 'y_earth', 'height', + 'psi', 'theta', 'phi', + 'u', 'v', 'w', + 'v_north', 'v_east', 'v_down', + 'p', 'q', 'r', + 'alpha', 'beta', 'TAS', + 'F_xb', 'F_yb', 'F_zb', + 'M_xb', 'M_yb', 'M_zb'] + +my_simulation.set_par_dict(par_list) +my_simulation.run_simulation() + +# print(my_simulation.par_dict) + +plt.style.use('ggplot') + +for ii in range(len(par_list) // 3): + three_params = par_list[3*ii:3*ii+3] + fig, ax = plt.subplots(3, 1, sharex=True) + for jj, par in enumerate(three_params): + ax[jj].plot(time, my_simulation.par_dict[par]) + ax[jj].set_ylabel(par) + ax[jj].set_xlabel('time (s)') + +fig = plt.figure() +ax = Axes3D(fig) +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height']) + +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height'] * 0) +ax.set_xlabel('x_earth') +ax.set_ylabel('y_earth') +ax.set_zlabel('z_earth') + +plt.show() diff --git a/examples/example_006.py b/examples/example_006.py new file mode 100644 index 0000000..f704474 --- /dev/null +++ b/examples/example_006.py @@ -0,0 +1,116 @@ +# -*- coding: utf-8 -*- +""" +Python Flight Mechanics Engine (PyFME). +Copyright (c) AeroPython Development Team. +Distributed under the terms of the MIT License. + +Example +------- + +Cessna 310, ISA1976 integrated with Flat Earth (euler angles). + +Evolution of the aircraft after a longitudinal perturbation (delta doublet). +Trimmed in stationary, horizontal, symmetric, wings level flight. +""" + +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D + +from pyfme.aircrafts import Cessna310 +from pyfme.environment.environment import Environment +from pyfme.environment.atmosphere import ISA1976 +from pyfme.environment.gravity import VerticalConstant +from pyfme.environment.wind import NoWind +from pyfme.models.systems import EulerFlatEarth +from pyfme.simulator import BatchSimulation +from pyfme.utils.trimmer import steady_state_flight_trimmer + +aircraft = Cessna310() +atmosphere = ISA1976() +gravity = VerticalConstant() +wind = NoWind() +environment = Environment(atmosphere, gravity, wind) + +# Initial conditions. +TAS = 312.5 * 0.3048 # m/s +h0 = 8000 * 0.3048 # m +psi0 = 1 # rad +x0, y0 = 0, 0 # m +turn_rate = 0.0 # rad/s +gamma0 = 0.00 # rad + +system = EulerFlatEarth(lat=0, lon=0, h=h0, psi=psi0, x_earth=x0, y_earth=y0) + +not_trimmed_controls = {'delta_elevator': 0.05, + 'hor_tail_incidence': 0.00, + 'delta_aileron': 0.01 * np.sign(turn_rate), + 'delta_rudder': 0.01 * np.sign(turn_rate), + 'delta_t': 0.5} + +controls2trim = ['delta_elevator', 'delta_aileron', 'delta_rudder', 'delta_t'] + +trimmed_ac, trimmed_sys, trimmed_env, results = steady_state_flight_trimmer( + aircraft, system, environment, TAS=TAS, controls_0=not_trimmed_controls, + controls2trim=controls2trim, gamma=gamma0, turn_rate=turn_rate, verbose=2) + +print(results) + +my_simulation = BatchSimulation(trimmed_ac, trimmed_sys, trimmed_env) + +tfin = 45 # seconds +N = tfin * 100 + 1 +time = np.linspace(0, tfin, N) +initial_controls = trimmed_ac.controls + +controls = {} +for control_name, control_value in initial_controls.items(): + controls[control_name] = np.ones_like(time) * control_value + +# Elevator doublet +controls['delta_elevator'][np.where(time<2)] = \ + initial_controls['delta_elevator'] * 1.30 + +controls['delta_elevator'][np.where(time<1)] = \ + initial_controls['delta_elevator'] * 0.70 + +my_simulation.set_controls(time, controls) + +par_list = ['x_earth', 'y_earth', 'height', + 'psi', 'theta', 'phi', + 'u', 'v', 'w', + 'v_north', 'v_east', 'v_down', + 'p', 'q', 'r', + 'alpha', 'beta', 'TAS', + 'F_xb', 'F_yb', 'F_zb', + 'M_xb', 'M_yb', 'M_zb'] + +my_simulation.set_par_dict(par_list) +my_simulation.run_simulation() + +# print(my_simulation.par_dict) + +plt.style.use('ggplot') + +for ii in range(len(par_list) // 3): + three_params = par_list[3*ii:3*ii+3] + fig, ax = plt.subplots(3, 1, sharex=True) + for jj, par in enumerate(three_params): + ax[jj].plot(time, my_simulation.par_dict[par]) + ax[jj].set_ylabel(par) + ax[jj].set_xlabel('time (s)') + +fig = plt.figure() +ax = Axes3D(fig) +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height']) + +ax.plot(my_simulation.par_dict['x_earth'], + my_simulation.par_dict['y_earth'], + my_simulation.par_dict['height'] * 0) +ax.set_xlabel('x_earth') +ax.set_ylabel('y_earth') +ax.set_zlabel('z_earth') + +plt.show() diff --git a/examples/trim_example_001.py b/examples/trim_example_001.py deleted file mode 100644 index 3c8c2d1..0000000 --- a/examples/trim_example_001.py +++ /dev/null @@ -1,38 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Python Flight Mechanics Engine (PyFME). -Copyright (c) AeroPython Development Team. -Distributed under the terms of the MIT License. - - - -""" -from pyfme.aircrafts import cessna_310 -from pyfme.utils.trimmer import steady_state_flight_trim -from pyfme.utils.coordinates import wind2body - -TAS = 80 # m/s -h = 1000 - -results = steady_state_flight_trim(cessna_310, h, TAS, gamma=0, turn_rate=0) -# lin_vel, ang_vel, theta, phi, alpha, beta, control_vector - -alpha = results[3] -beta = results[4] -lin_vel = wind2body((TAS, 0, 0), alpha, beta) - -print( -""" -Results -------- -Linear velocity: {lin_vel} (m/s) -Angular velocity: {ang_vel} (rad/s) -Theta, Phi: {angles} (rad) -Alpha, Beta: {wind_angles} (rad) -Control: {control} -""".format(lin_vel=lin_vel, - ang_vel=results[1], - angles=results[2:4], - wind_angles=results[4:6], - control=results[6]) - ) diff --git a/src/pyfme/__init__.py b/src/pyfme/__init__.py index 139597f..9b62a03 100644 --- a/src/pyfme/__init__.py +++ b/src/pyfme/__init__.py @@ -1,2 +1,5 @@ - - +""" +Python Flight Mechanics Engine (PyFME). +Copyright (c) AeroPython Development Team. +Distributed under the terms of the MIT License. +""" diff --git a/src/pyfme/aircrafts/__init__.py b/src/pyfme/aircrafts/__init__.py index 139597f..8da8e0b 100644 --- a/src/pyfme/aircrafts/__init__.py +++ b/src/pyfme/aircrafts/__init__.py @@ -1,2 +1 @@ - - +from .cessna_310 import Cessna310 diff --git a/src/pyfme/aircrafts/aircraft.py b/src/pyfme/aircrafts/aircraft.py index e69de29..11a536c 100644 --- a/src/pyfme/aircrafts/aircraft.py +++ b/src/pyfme/aircrafts/aircraft.py @@ -0,0 +1,101 @@ +""" +Python Flight Mechanics Engine (PyFME). +Copyright (c) AeroPython Development Team. +Distributed under the terms of the MIT License. + +Generic Aircraft +---------------- + +""" +from abc import abstractmethod +from copy import deepcopy +from warnings import warn + +import numpy as np +from scipy.optimize import least_squares + +from pyfme.environment.environment import Environment +from pyfme.models.systems import System +from pyfme.utils.anemometry import tas2cas, tas2eas, calculate_alpha_beta_TAS +from pyfme.utils.trimmer import trimming_cost_func + + +class Aircraft(object): + + def __init__(self): + # Mass & Inertia + self.mass = 0 # kg + self.inertia = 0 # kg·m² + # Geometry + self.Sw = 0 # m2 + self.chord = 0 # m + self.span = 0 # m + # Controls + self.controls = {} + self.control_limits = {} + # Coefficients + # Aero + self.CL, self.CD, self.Cm = 0, 0, 0 + self.CY, self.Cl, self.Cn = 0, 0, 0 + # Thrust + self.Ct = 0 + # Forces & moments + self.gravity_force = np.zeros(3) + self.total_forces = np.zeros(3) + self.total_moments = np.zeros(3) + self.load_factor = 0 + + # Velocities + self.TAS = 0 # True Air Speed. + self.CAS = 0 # Calibrated Air Speed. + self.EAS = 0 # Equivalent Air Speed. + self.Mach = 0 # Mach number + + self.q_inf = 0 # Dynamic pressure at infty (Pa) + + # Angles + self.alpha = 0 # Angle of attack (AOA). + self.beta = 0 # Angle of sideslip (AOS). + # Not present in this model: + self.Dalpha_Dt = 0 # Rate of change of AOA. + self.Dbeta_Dt = 0 # Rate of change of AOS. + + @property + def Ixx(self): + return self.inertia[0, 0] + + @property + def Iyy(self): + return self.inertia[1, 1] + + @property + def Izz(self): + return self.inertia[2, 2] + + def update(self, controls, system, environment): + + # If a control is not given, the previous value is assigned. + for control_name, control_value in controls.items(): + limits = self.control_limits[control_name] + if limits[0] <= control_value <= limits[1]: + self.controls[control_name] = control_value + else: + # TODO: maybe raise a warning and assign max deflection + msg = "Control {} out of range ({} when max={} and min={" \ + "}".format(control_name, limits[1], limits[0]) + raise ValueError(msg) + + # Velocity relative to air: aerodynamic velocity. + aero_vel = system.vel_body - environment.body_wind + + self.alpha, self.beta, self.TAS = calculate_alpha_beta_TAS( + u=aero_vel[0], v=aero_vel[1], w=aero_vel[2]) + + # Setting velocities & dynamic pressure + self.CAS = tas2cas(self.TAS, environment.p, environment.rho) + self.EAS = tas2eas(self.TAS, environment.rho) + self.Mach = self.TAS / environment.a + self.q_inf = 0.5 * environment.rho * self.TAS ** 2 + + # Gravity force + self.gravity_force = environment.gravity_vector * self.mass diff --git a/src/pyfme/aircrafts/cessna_310.py b/src/pyfme/aircrafts/cessna_310.py index 2b177ec..f1dfe04 100644 --- a/src/pyfme/aircrafts/cessna_310.py +++ b/src/pyfme/aircrafts/cessna_310.py @@ -6,142 +6,6 @@ Cessna 310 ---------- -Created on Sun Jan 3 18:44:39 2016 - -@author:olrosales@gmail.com - -@AeroPython -""" - -# Aircraft = Cessna 310 - - -import numpy as np - -from pyfme.utils.anemometry import calculate_dynamic_pressure -from pyfme.utils.coordinates import hor2body - - -def geometric_data(): - - """ Provides the value of some geometric data. - - Returns - ---- - Sw : float - Wing surface (m2). - c : foat - Mean aerodynamic Chord (m). - span : float - Wing span (m). - - References - ---------- - AIRCRAFT DYNAMICS From modelling to simulation (Marcello R. Napolitano) - page 589 - """ - - Sw = 175 * 0.3048 * 0.3048 # m2 - c = 4.79 * 0.3048 # m - span = 36.9 * 0.3048 # m - - return Sw, c, span - - -def mass_and_inertial_data(): - - """ Provides the value of some mass and inertial data. - - Returns - ------ - mass : float - mass (lb * 0.453592 = kg) - inertia : array_like - Inertia tensor (Kg * m^2) - - References - ---------- - AIRCRAFT DYNAMICS From modelling to simulation (Marcello R. Napolitano) - page 589 - """ - - mass = 4600 * 0.453592 # kg - Ixx_b = 8884 * 1.3558179 # Kg * m2 - Iyy_b = 1939 * 1.3558179 # Kg * m2 - Izz_b = 11001 * 1.3558179 # Kg * m2 - Ixz_b = 0 * 1.3558179 # Kg * m2 - - inertia = np.diag([Ixx_b, Iyy_b, Izz_b]) - - return mass, inertia - - -def long_aero_coefficients(): - - """Assigns the value of the coefficients - of stability in cruise conditions and order them in a matrix. - - - CL_0 is the lift coefficient evaluated at the initial condition - CL_a is the lift stability derivative with respect to the angle of attack - CL_de is the lift stability derivative with respect to the elevator - deflection - CL_dih is the lift stability derivative with respect to the stabilator - deflection - - CD_0 is the drag coefficient evaluated at the initial condition - CD_a is the drag stability derivative with respect to the angle of attack - - Cm_0 is the pitching moment coefficient evaluated at the condition - (alpha0 = deltaE = deltaih = 0º) - Cm_a is the pitching moment stability derivative with respect to the angle - of attack - Cm_de is the pitching moment stability derivative with respect to the - elevator deflection - Cm_dih is the pitching moment stability derivative with respect to the - stabilator deflection - - Returns - ------- - Long_coef_matrix : array_like - [ - [CL_0, CL_a, CL_de, CL_dih], - [CD_0, CD_a, 0, 0], - [Cm_0, Cm_a, Cm_de, Cm_dih] - ] - - - References - ---------- - AIRCRAFT DYNAMICS From modelling to simulation (Marcello R. Napolitano) - page 589 - """ - CL_0 = 0.288 - CL_a = 4.58 - CL_de = 0.81 - CL_dih = 0 - - CD_0 = 0.029 - CD_a = 0.160 - - Cm_0 = 0.07 - Cm_a = -0.137 - Cm_de = -2.26 - Cm_dih = 0 - - long_coef_matrix = np.array([ - [CL_0, CL_a, CL_de, CL_dih], - [CD_0, CD_a, 0, 0], - [Cm_0, Cm_a, Cm_de, Cm_dih] - ]) - - return long_coef_matrix - - -def lat_aero_coefficients(): - - """Assigns the value of the coefficients - of stability in cruise conditions and order them in a matrix. CY_b is the side force stability derivative with respect to the angle of sideslip @@ -164,242 +28,165 @@ def lat_aero_coefficients(): Cn_dr is the yawing moment stability derivative with respect to the rudder deflection - returns - ------- - Long_coef_matrix : array_like - [ - [CY_b, CY_da, CY_dr], - [Cl_b, Cl_da, Cl_dr], - [Cn_b, Cn_da, Cn_dr] - ] - - - References - ---------- - AIRCRAFT DYNAMICS From modelling to simulation (Marcello R. Napolitano) - page 590 - """ - - CY_b = -0.698 - CY_da = 0 - CY_dr = 0.230 - - Cl_b = -0.1096 - Cl_da = 0.172 - Cl_dr = 0.0192 - - Cn_b = 0.1444 - Cn_da = -0.0168 - Cn_dr = -0.1152 - - lat_coef_matrix = np.array([ - [CY_b, CY_da, CY_dr], - [Cl_b, Cl_da, Cl_dr], - [Cn_b, Cn_da, Cn_dr] - ]) - - return lat_coef_matrix - - -def get_aerodynamic_forces(TAS, rho, alpha, beta, delta_e, ih, delta_ail, - delta_r): + CL_0 is the lift coefficient evaluated at the initial condition + CL_a is the lift stability derivative with respect to the angle of attack + CL_de is the lift stability derivative with respect to the elevator + deflection + CL_dih is the lift stability derivative with respect to the stabilator + deflection + CD_0 is the drag coefficient evaluated at the initial condition + CD_a is the drag stability derivative with respect to the angle of attack + Cm_0 is the pitching moment coefficient evaluated at the condition + (alpha0 = deltaE = deltaih = 0º) + Cm_a is the pitching moment stability derivative with respect to the angle + of attack + Cm_de is the pitching moment stability derivative with respect to the + elevator deflection + Cm_dih is the pitching moment stability derivative with respect to the + stabilator deflection - """ Calculates forces +""" +import numpy as np - Parameters - ---------- +from pyfme.aircrafts.aircraft import Aircraft +from pyfme.models.constants import ft2m, slugft2_2_kgm2, lbs2kg +from pyfme.utils.coordinates import wind2body - rho : float - density (kg/(m3). - TAS : float - velocity (m/s). - 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). - - Returns - ------- - forces : array_like - 3 dimensional vector with (F_x_s, F_y_s, F_z_s) forces in stability - axes. - References - ---------- - AIRCRAFT DYNAMICS From modelling to simulation (Marcello R. Napolitano) - chapter 3 and 4 +class Cessna310(Aircraft): """ + Cessna 310 - long_coef_matrix = long_aero_coefficients() - lat_coef_matrix = lat_aero_coefficients() - - CL_0, CL_a, CL_de, CL_dih = long_coef_matrix[0, :] - CD_0, CD_a, CD_de, CD_dih = long_coef_matrix[1, :] - CY_b, CY_da, CY_dr = lat_coef_matrix[0, :] - - CL_full = CL_0 + CL_a * alpha + CL_de * delta_e + CL_dih * ih - CD_full = CD_0 + CD_a * alpha + CD_de * delta_e + CD_dih * ih - CY_full = CY_b * beta + CY_da * delta_ail + CY_dr * delta_r - - Sw = geometric_data()[0] - - aerodynamic_forces = calculate_dynamic_pressure(rho, TAS) *\ - Sw * np.array([-CD_full, CY_full, -CL_full]) # N - - return aerodynamic_forces - - -def get_aerodynamic_moments(TAS, rho, alpha, beta, delta_e, ih, delta_ail, - delta_r): - - """ Calculates forces - - Parameters - ---------- - rho : float - density (kg/m3). - TAS : float - velocity (m/s). - 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). - - Returns - ------- - moments : array_like - 3 dimensional vector with (Mxs, Mys, Mzs) forces - in stability axes. + The Cessna 310 is an American six-seat, low-wing, twin-engined monoplane + that was produced by Cessna between 1954 and 1980. It was the first + twin-engined aircraft that Cessna put into production after World War II. References ---------- AIRCRAFT DYNAMICS From modelling to simulation (Marcello R. Napolitano) - chapter 3 and 4 - """ - - long_coef_matrix = long_aero_coefficients() - lat_coef_matrix = lat_aero_coefficients() - - Cm_0, Cm_a, Cm_de, Cm_dih = long_coef_matrix[2, :] - Cl_b, Cl_da, Cl_dr = lat_coef_matrix[1, :] - Cn_b, Cn_da, Cn_dr = lat_coef_matrix[2, :] - - Cm_full = Cm_0 + Cm_a * alpha + Cm_de * delta_e + Cm_dih * ih - Cl_full = Cl_b * beta + Cl_da * delta_ail + Cl_dr * delta_r - Cn_full = Cn_b * beta + Cn_da * delta_ail + Cn_dr * delta_r - - span = geometric_data()[2] - c = geometric_data()[1] - Sw = geometric_data()[0] - - aerodynamic_moments = calculate_dynamic_pressure(rho, TAS) * Sw\ - * np.array([Cl_full * span, Cm_full * c, Cn_full * span]) - - return aerodynamic_moments - - -def get_engine_force(delta_t): - """ Calculates forces - - Parameters - ---------- - delta_t : float - trust_lever (0 = 0 Newton, 1 = CT). - - returns - ------- - engine_force : float - Thrust (N). - - References - ---------- - Airplane Flight Dyanamics and Automatic Flight Controls part I - Jan Roskam + page 589 """ - if 0 <= delta_t <= 1: - - Ct = 0.031 * delta_t - - q_cruise = 91.2 * 47.880172 # Pa - Sw = geometric_data()[0] - - engine_force = Ct * Sw * q_cruise # N - - else: - raise ValueError('delta_t must be between 0 and 1') - - return engine_force - - -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 __init__(self): + + # Mass & Inertia + self.mass = 4600 * lbs2kg # kg + self.inertia = np.diag([8884, 1939, 11001]) * slugft2_2_kgm2 # kg·m² + # Ixz_b = 0 * slugft2_2_kgm2 # Kg * m2 + + # Geometry + self.Sw = 175 * ft2m**2 # m2 + self.chord = 4.79 * ft2m # m + self.span = 36.9 * ft2m # m + + # Aerodynamic Data (Linearized) + # |CL| |CL_0 CL_a CL_de CL_dih| | 1 | + # |CD| = |CD_0 CD_a 0 0 | | alpha | + # |Cm| |Cm_0 Cm_a Cm_de Cm_dih| |delta_e | + # |delta_ih| + # + # |CY| |CY_b CY_da CY_dr| |beta | + # |Cl| = |Cl_b Cl_da Cl_dr| |delta_a| + # |Cn| |Cn_b Cn_da Cn_dr| |delta_r| + self._long_coef_matrix = np.array([[0.288, 4.58, 0.81, 0], + [0.029, 0.160, 0, 0], + [ 0.07, -0.137, -2.26, 0]]) + + self._lat_coef_matrix = np.array([[-0.698 , 0, 0.230], + [-0.1096, 0.172, 0.0192], + [ 0.1444, -0.0168, -0.1152]]) + self._long_inputs = np.zeros(4) + self._long_inputs[0] = 1.0 + + self._lat_inputs = np.zeros(3) + + # CONTROLS + self.controls = {'delta_elevator': 0, + 'hor_tail_incidence': 0, + 'delta_aileron': 0, + 'delta_rudder': 0, + 'delta_t': 0} + + # FIXME: these limits are not real + self.control_limits = {'delta_elevator': (-1, 1), # rad + 'hor_tail_incidence': (-1, 1), # rad + 'delta_aileron': (-1, 1), # rad + 'delta_rudder': (-1, 1), # rad + 'delta_t': (0, 1)} # non dimensional + + # Coefficients + # Aero + self.CL, self.CD, self.Cm = 0, 0, 0 + self.CY, self.Cl, self.Cn = 0, 0, 0 + # Thrust + self.Ct = 0 + + self.gravity_force = np.zeros(3) + self.total_forces = np.zeros(3) + self.total_moments = np.zeros(3) + self.load_factor = 0 + + # Velocities + self.TAS = 0 # True Air Speed. + self.CAS = 0 # Calibrated Air Speed. + self.EAS = 0 # Equivalent Air Speed. + self.Mach = 0 # Mach number + + self.q_inf = 0 # Dynamic pressure at infty (Pa) + + # Angles + self.alpha = 0 # Angle of attack (AOA). + self.beta = 0 # Angle of sideslip (AOS). + # Not present in this model: + self.Dalpha_Dt = 0 # Rate of change of AOA. + self.Dbeta_Dt = 0 # Rate of change of AOS. + + def _calculate_aero_lon_forces_moments_coeffs(self): + + self._long_inputs[1] = self.alpha + self._long_inputs[2] = self.controls['delta_elevator'] + self._long_inputs[3] = self.controls['hor_tail_incidence'] + + self.CL, self.CD, self.Cm = self._long_coef_matrix @ self._long_inputs + + def _calculate_aero_lat_forces_moments_coeffs(self): + self._lat_inputs[0] = self.beta + self._lat_inputs[1] = self.controls['delta_aileron'] + self._lat_inputs[2] = self.controls['delta_rudder'] + + self.CY, self.Cl, self.Cn = self._lat_coef_matrix @ self._lat_inputs + + def _calculate_aero_forces_moments(self): + q = self.q_inf + Sw = self.Sw + c = self.chord + b = self.span + self._calculate_aero_lon_forces_moments_coeffs() + self._calculate_aero_lat_forces_moments_coeffs() + L = q * Sw * self.CL + D = q * Sw * self.CD + Y = q * Sw * self.CY + l = q * Sw * b * self.Cl + m = q * Sw * c * self.Cm + n = q * Sw * b * self.Cn + return L, D, Y, l, m , n + + def _calculate_thrust_forces_moments(self): + q = self.q_inf + Sw = self.Sw + self.Ct = 0.031 * self.controls['delta_t'] + Ft = np.array([q * Sw * self.Ct, 0, 0]) + return Ft + + + def calculate_forces_and_moments(self): + + Ft = self._calculate_thrust_forces_moments() + L, D, Y, l, m, n = self._calculate_aero_forces_moments() + Fg = self.gravity_force + # FIXME: is it necessary to use wind2body conversion? + Fa = np.array([-D, Y, -L]) + + self.total_forces = 10 * Ft + Fg + Fa + self.total_moments = np.array([l, m, n]) + return self.total_forces, self.total_moments diff --git a/src/pyfme/aircrafts/tests/test_cessna_310.py b/src/pyfme/aircrafts/tests/test_cessna_310.py index e80ab50..c61b3ba 100644 --- a/src/pyfme/aircrafts/tests/test_cessna_310.py +++ b/src/pyfme/aircrafts/tests/test_cessna_310.py @@ -11,59 +11,34 @@ @AeroPython """ -import pytest - import numpy as np - from numpy.testing import (assert_array_almost_equal, assert_almost_equal) -from pyfme.aircrafts.cessna_310 import (get_aerodynamic_forces, - get_aerodynamic_moments, - get_engine_force) - +from pyfme.aircrafts.cessna_310 import Cessna310 -def test_get_aerodynamic_forces(): - TAS = 100 - rho = 1.225 - alpha = 0 - beta = 0 - delta_e = 0 - ih = 0 - delta_ail = 0 - delta_r = 0 - aerodynamic_forces_expected = np.array([-2887.832934, 0, -28679.16845]) +def test_calculate_aero_forces_moments_alpha_beta_zero(): + aircraft = Cessna310() + aircraft.q_inf = 0.5 * 1.225 * 100 ** 2 + aircraft.alpha = 0 + aircraft.beta = 0 - aerodynamic_forces = get_aerodynamic_forces(TAS, rho, alpha, - beta, delta_e, ih, delta_ail, - delta_r) + aircraft.controls = {'delta_elevator': 0, + 'hor_tail_incidence': 0, + 'delta_aileron': 0, + 'delta_rudder': 0, + 'delta_t': 0} - assert_array_almost_equal(aerodynamic_forces, aerodynamic_forces_expected, + L, D, Y, l, m, n = aircraft._calculate_aero_forces_moments() + assert_array_almost_equal([L, D, Y], + [28679.16845, 2887.832934, 0.], + decimal=4) + assert_array_almost_equal([l, m, n], + [0, 10177.065816, 0], decimal=4) - - -def test_get_aerodynamic_moments(): - - TAS = 100 - rho = 1.225 - alpha = 0 - beta = 0 - delta_e = 0 - ih = 0 - delta_ail = 0 - delta_r = 0 - aerodynamic_moments_expected = np.array([0, 10177.06582, 0]) - - aerodynamic_moments = get_aerodynamic_moments(TAS, rho, alpha, beta, - delta_e, ih, delta_ail, - delta_r) - - assert_array_almost_equal(aerodynamic_moments, - aerodynamic_moments_expected, decimal=4) def test_get_engine_force(): - delta_t = 0.5 trust_expected = 1100.399064 @@ -76,6 +51,5 @@ def test_get_engine_force(): wrong_value_1 = 2 with pytest.raises(ValueError): - get_engine_force(wrong_value_0) get_engine_force(wrong_value_1) diff --git a/src/pyfme/environment/atmosphere.py b/src/pyfme/environment/atmosphere.py new file mode 100644 index 0000000..768db26 --- /dev/null +++ b/src/pyfme/environment/atmosphere.py @@ -0,0 +1,240 @@ +# -*- coding: utf-8 -*- +""" +Python Flight Mechanics Engine (PyFME). +Copyright (c) AeroPython Development Team. +Distributed under the terms of the MIT License. + +Atmosphere +---------- + +""" + +from math import exp, sqrt +from abc import abstractmethod + +from pyfme.models.constants import GAMMA_AIR, R_AIR, GRAVITY +from pyfme.utils.altimetry import geometric2geopotential + + +class Atmosphere(object): + + def __init__(self): + # TODO: doc + self._gamma = None # Adiabatic index or ratio of specific heats + self._R_g = None # Gas constant J/(Kg·K) + self._g0 = None # Gravity m/s^2 + + self.geopotential_alt = None # Current geopotential height (m). + self.pressure_altitude = None # Current pressure altitude (m). + self.T = None # Temperature (K). + self.p = None # Pressure (atm). + self.rho = None # Density (kg/m³). + self.a = None # Speed of sound (m/s). + + def update(self, system): + """Update atmosphere state for the given system state. + + Parameters + ---------- + system : System object + System object with attribute alt_geop (geopotential + altitude. + + Returns + ------- + T : float + Temperature (K). + p : float + Pressure (Pa). + rho : float + Density (kg/m³) + a : float + Sound speed at flight level (m/s) + + Raises + ------ + ValueError + If the value of the altitude is outside the defined layers. + + Notes + ----- + Check layers and reference values in [2]. + + References + ---------- + .. [1] U.S. Standard Atmosphere, 1976, U.S. Government Printing Office, + Washington, D.C., 1976 + .. [2] https://en.wikipedia.org/wiki/U.S._Standard_Atmosphere + + """ + # Geopotential altitude + self.geopotential_alt = geometric2geopotential(system.height) + self.T, self.p, self.rho, self.a = self.__call__(self.geopotential_alt) + + @abstractmethod + def __call__(self, h): + pass + + +class ISA1976(Atmosphere): + """ + International Standard Atmosphere 1976 + -------------------------------------- + Implementation based on: + .. [1] U.S. Standard Atmosphere, 1976, U.S. Government Printing Office, + Washington, D.C., 1976 + + From: https://en.wikipedia.org/wiki/U.S._Standard_Atmosphere + + ========= ============ ========= =========== ============= + Subscript Geopotential Static Standard Temperature + altitude Pressure Temperature Lapse Rate + above MSL (pascals) (K) (K/m) + (m) + ========= ============ ========= =========== ============= + 0 0 101325 288.15 -0.0065 + 1 11000 22632.1 216.65 0 + 2 20000 5474.89 216.65 0.001 + 3 32000 868.019 228.65 0.0028 + 4 47000 110.906 270.65 0 + 5 51000 66.9389 270.65 -0.0028 + 6 71000 3.95642 214.65 -0.002 + ========= ============ ========= =========== ============= + """ + + def __init__(self): + super().__init__() + self._gamma = GAMMA_AIR # Adiabatic index or ratio of specific heats + self._R_g = R_AIR # Gas constant J/(Kg·K) + self._g0 = GRAVITY # Gravity m/s^2 + # Layer constants + self._h0 = (0, 11000, 20000, 32000, 47000, 51000, 71000, 84500) # m + self._T0_layers = (288.15, 216.65, 216.65, 228.65, 270.65, 270.65, + 214.65) # K + self._p0_layers = (101325.0, 22632.1, 5474.89, 868.019, 110.906, + 66.9389, 3.95642) # Pa + self._alpha_layers = (-0.0065, 0, 0.001, 0.0028, 0, -0.0028, + -0.002) # K / m + + # Initialize at h=0 + self.h = 0 # Current height (m). + self.T = self._T0_layers[0] # Temperature (K). + self.p = self._p0_layers[0] # Pressure (atm). + self.rho = self.p / (self._R_g * self.T) + self.a = sqrt(self._gamma * self._R_g * self.T) + + def __call__(self, h): + """ISA 1976 Standard atmosphere temperature, pressure and density. + + Parameters + ---------- + h : float + Geopotential altitude (m). h values must range from 0 to 84500 m. + + Returns + ------- + T : float + Temperature (K). + p : float + Pressure (Pa). + rho : float + Density (kg/m³) + a : float + Sound speed at flight level (m/s) + + Raises + ------ + ValueError + If the value of the altitude is outside the defined layers. + + Notes + ----- + Note that this method will calculate the atmosphere `T, p, rho, + a` values corresponding to the given geopotential altitude, but the + atmosphere object will not be updated. Use update instead to + update the atmosphere. + + Check layers and reference values in [2]. + + References + ---------- + .. [1] U.S. Standard Atmosphere, 1976, U.S. Government Printing Office, + Washington, D.C., 1976 + .. [2] https://en.wikipedia.org/wiki/U.S._Standard_Atmosphere + + """ + g0 = self._g0 + R_a = self._R_g + gamma = self._gamma + + if h < 0.0: + raise ValueError("Altitude cannot be less than 0 m.") + + elif self._h0[0] <= h < self._h0[1]: # Troposphere + T0 = self._T0_layers[0] + p0 = self._p0_layers[0] + alpha = self._alpha_layers[0] + + T = T0 + alpha * h + p = p0 * (T0 / (T0 + alpha * h)) ** (g0 / (R_a * alpha)) + + elif self._h0[1] <= h < self._h0[2]: # Tropopause + T0 = self._T0_layers[1] + p0 = self._p0_layers[1] + # alpha = self._alpha_layers[1] + h_diff = h - self._h0[1] + + T = T0 + p = p0 * exp(-g0 * h_diff / (R_a * T0)) + + elif self._h0[2] <= h < self._h0[3]: # Stratosphere 1 + T0 = self._T0_layers[2] + p0 = self._p0_layers[2] + alpha = self._alpha_layers[2] + h_diff = h - self._h0[2] + + T = T0 + alpha * h_diff + p = p0 * (T0 / (T0 + alpha * h_diff)) ** (g0 / (R_a * alpha)) + + elif self._h0[3] <= h < self._h0[4]: # Stratosphere 2 + T0 = self._T0_layers[3] + p0 = self._p0_layers[3] + alpha = self._alpha_layers[3] + h_diff = h - self._h0[3] + + T = T0 + alpha * h_diff + p = p0 * (T0 / (T0 + alpha * h_diff)) ** (g0 / (R_a * alpha)) + + elif self._h0[4] <= h < self._h0[5]: # Stratopause + T0 = self._T0_layers[4] + p0 = self._p0_layers[4] + # alpha = self._alpha_layers[4] + h_diff = h - self._h0[4] + + T = T0 + p = p0 * exp(-g0 * h_diff / (R_a * T0)) + + elif self._h0[5] <= h < self._h0[6]: # Mesosphere 1 + T0 = self._T0_layers[5] + p0 = self._p0_layers[5] + alpha = self._alpha_layers[5] + h_diff = h - self._h0[5] + + T = T0 + alpha * h_diff + p = p0 * (T0 / (T0 + alpha * h_diff)) ** (g0 / (R_a * alpha)) + + elif self._h0[6] <= h <= self._h0[7]: # Mesosphere 2 + T0 = self._T0_layers[6] + p0 = self._p0_layers[6] + alpha = self._alpha_layers[6] + h_diff = h - self._h0[6] + + T = T0 + alpha * h_diff + p = p0 * (T0 / (T0 + alpha * h_diff)) ** (g0 / (R_a * alpha)) + + else: + raise ValueError( + "Altitude cannot be greater than {} m.".format(self._h0[7])) + rho = p / (R_a * T) + a = sqrt(gamma * R_a * T) + return T, p, rho, a diff --git a/src/pyfme/environment/environment.py b/src/pyfme/environment/environment.py new file mode 100644 index 0000000..6a804a4 --- /dev/null +++ b/src/pyfme/environment/environment.py @@ -0,0 +1,49 @@ +""" +Python Flight Mechanics Engine (PyFME). +Copyright (c) AeroPython Development Team. +Distributed under the terms of the MIT License. +""" + +class Environment(object): + + def __init__(self, atmosphere, gravity, wind): + self.atmosphere = atmosphere + self.gravity = gravity + self.wind = wind + + @property + def T(self): + return self.atmosphere.T + + @property + def p(self): + return self.atmosphere.p + + @property + def rho(self): + return self.atmosphere.rho + + @property + def a(self): + return self.atmosphere.a + + @property + def gravity_magnitude(self): + return self.gravity.magnitude + + @property + def gravity_vector(self): + return self.gravity.vector + + @property + def body_wind(self): + return self.wind.body_wind + + @property + def horizon_wind(self): + return self.wind.horizon_wind + + def update(self, system): + self.atmosphere.update(system) + self.gravity.update(system) + self.wind.update(system) diff --git a/src/pyfme/environment/gravity.py b/src/pyfme/environment/gravity.py new file mode 100644 index 0000000..c77993a --- /dev/null +++ b/src/pyfme/environment/gravity.py @@ -0,0 +1,64 @@ +""" +Python Flight Mechanics Engine (PyFME). +Copyright (c) AeroPython Development Team. +Distributed under the terms of the MIT License. + +Gravity Models +-------------- + +""" +from abc import abstractmethod + +import numpy as np + +from pyfme.models.constants import GRAVITY, STD_GRAVITATIONAL_PARAMETER +from pyfme.utils.coordinates import hor2body + +class Gravity(object): + + def __init__(self): + self.magnitude = None + self.unitary_vector = np.zeros([3]) # Body axis + self.vector = np.zeros([3]) # Body axis + + @abstractmethod + def update(self, system): + pass + + +class VerticalConstant(Gravity): + + def __init__(self): + Gravity.__init__(self) + self.magnitude = GRAVITY + self._z_horizon = np.array([0, 0, 1], dtype=float) + + 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 + + +class VerticalNewton(Gravity): + + def __init__(self): + Gravity.__init__(self) + self._z_horizon = np.array([0, 0, 1], dtype=float) + + 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 + + +class LatitudeModel(Gravity): + # TODO: https://en.wikipedia.org/wiki/Gravity_of_Earth#Latitude_model + + def __init__(self): + raise NotImplementedError diff --git a/src/pyfme/environment/isa.py b/src/pyfme/environment/isa.py deleted file mode 100644 index a755dc5..0000000 --- a/src/pyfme/environment/isa.py +++ /dev/null @@ -1,156 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Python Flight Mechanics Engine (PyFME). -Copyright (c) AeroPython Development Team. -Distributed under the terms of the MIT License. - -ISA functions -------------- - -Implementation based on: - -.. [1] U.S. Standard Atmosphere, 1976, U.S. Government Printing Office, - Washington, D.C., 1976 - -From: https://en.wikipedia.org/wiki/U.S._Standard_Atmosphere - -========= ============ ========= =========== ============= -Subscript Geopotential Static Standard Temperature - altitude Pressure Temperature Lapse Rate - above MSL (pascals) (K) (K/m) - (m) -========= ============ ========= =========== ============= -0 0 101325 288.15 -0.0065 -1 11000 22632.1 216.65 0 -2 20000 5474.89 216.65 0.001 -3 32000 868.019 228.65 0.0028 -4 47000 110.906 270.65 0 -5 51000 66.9389 270.65 -0.0028 -6 71000 3.95642 214.65 -0.002 -========= ============ ========= =========== ============= - -""" - -from math import exp, sqrt - -# Constants -gamma = 1.401 # Adiabatic index or ratio of specific heats (dry air at 20º C) -R_a = 287.05287 # J/(Kg·K) -g0 = 9.80665 # m/s^2 - -# Layer constants -h0 = (0, 11000, 20000, 32000, 47000, 51000, 71000, 84500) # m -T0_layers = (288.15, 216.65, 216.65, 228.65, 270.65, 270.65, 214.65) # K -p0_layers = (101325.0, 22632.1, 5474.89, 868.019, 110.906, 66.9389, 3.95642) # Pa -alpha_layers = (-0.0065, 0, 0.001, 0.0028, 0, -0.0028, -0.002) # K / m - - -def atm(h): - """ISA 1976 Standard atmosphere temperature, pressure and density. - - Parameters - ---------- - h : float - Geopotential altitude (m). h values must range from 0 to 84500 m. - - Returns - ------- - T : float - Temperature (K). - p : float - Pressure (Pa). - rho : float - Density (kg/m³) - a : float - Sound speed at flight level (m/s) - - Raises - ------ - ValueError - If the value of the altitude is outside the defined layers. - - Notes - ----- - Check layers and reference values in [2]. - - References - ---------- - .. [1] U.S. Standard Atmosphere, 1976, U.S. Government Printing Office, - Washington, D.C., 1976 - .. [2] https://en.wikipedia.org/wiki/U.S._Standard_Atmosphere - - """ - - if h < 0.0: - raise ValueError("Altitude cannot be less than 0 m.") - - elif h0[0] <= h < h0[1]: # Troposphere - T0 = T0_layers[0] - p0 = p0_layers[0] - alpha = alpha_layers[0] - - T = T0 + alpha * h - p = p0 * (T0 / (T0 + alpha * h)) ** (g0 / (R_a * alpha)) - - elif h0[1] <= h < h0[2]: # Tropopause - T0 = T0_layers[1] - p0 = p0_layers[1] - # alpha = alpha_layers[1] - h_diff = h - h0[1] - - T = T0 - p = p0 * exp(-g0 * h_diff / (R_a * T0)) - - elif h0[2] <= h < h0[3]: # Stratosphere 1 - T0 = T0_layers[2] - p0 = p0_layers[2] - alpha = alpha_layers[2] - h_diff = h - h0[2] - - T = T0 + alpha * h_diff - p = p0 * (T0 / (T0 + alpha * h_diff)) ** (g0 / (R_a * alpha)) - - elif h0[3] <= h < h0[4]: # Stratosphere 2 - T0 = T0_layers[3] - p0 = p0_layers[3] - alpha = alpha_layers[3] - h_diff = h - h0[3] - - T = T0 + alpha * h_diff - p = p0 * (T0 / (T0 + alpha * h_diff)) ** (g0 / (R_a * alpha)) - - elif h0[4] <= h < h0[5]: # Stratopause - T0 = T0_layers[4] - p0 = p0_layers[4] - # alpha = alpha_layers[4] - h_diff = h - h0[4] - - T = T0 - p = p0 * exp(-g0 * h_diff / (R_a * T0)) - - elif h0[5] <= h < h0[6]: # Mesosphere 1 - T0 = T0_layers[5] - p0 = p0_layers[5] - alpha = alpha_layers[5] - h_diff = h - h0[5] - - T = T0 + alpha * h_diff - p = p0 * (T0 / (T0 + alpha * h_diff)) ** (g0 / (R_a * alpha)) - - elif h0[6] <= h <= h0[7]: # Mesosphere 2 - T0 = T0_layers[6] - p0 = p0_layers[6] - alpha = alpha_layers[6] - h_diff = h - h0[6] - - T = T0 + alpha * h_diff - p = p0 * (T0 / (T0 + alpha * h_diff)) ** (g0 / (R_a * alpha)) - - else: - raise ValueError("Altitude cannot be greater than {} m.".format(h0[7])) - - rho = p / (R_a * T) - - a = sqrt(gamma * R_a * T) - - return T, p, rho, a diff --git a/src/pyfme/environment/tests/test_isa.py b/src/pyfme/environment/tests/test_isa.py index 29f5543..a3ea0a9 100644 --- a/src/pyfme/environment/tests/test_isa.py +++ b/src/pyfme/environment/tests/test_isa.py @@ -17,7 +17,7 @@ import pytest -from pyfme.environment.isa import atm +from pyfme.environment.atmosphere import ISA1976 def test_sea_level(): @@ -27,7 +27,7 @@ def test_sea_level(): expected_rho = 1.2250 # kg / m3 expected_a = 340.4155 # m / s - T, p, rho, a = atm(h) + T, p, rho, a = ISA1976(h) # Reads: "Assert if T equals expected_T" assert_equal(T, expected_T) @@ -41,7 +41,7 @@ def test_altitude_is_out_of_range(): for h in wrong_h: with pytest.raises(ValueError): - atm(h) + ISA1976(h) def test_results_under_11km(): @@ -77,7 +77,7 @@ def test_results_under_11km(): ]) # m / s for ii, h_ in enumerate(h): - T, p, rho, a = atm(h_) + T, p, rho, a = ISA1976(h_) assert_almost_equal(T, expected_T[ii], decimal=3) assert_almost_equal(rho, expected_rho[ii], decimal=4) assert_almost_equal(a, expected_a[ii], decimal=2) @@ -108,7 +108,7 @@ def test_results_under_20km(): ]) for ii, h_ in enumerate(h): - T, p, rho, a = atm(h_) + T, p, rho, a = ISA1976(h_) assert_almost_equal(T, expected_T[ii], decimal=3) assert_almost_equal(rho, expected_rho[ii], decimal=4) assert_almost_equal(a, expected_a[ii], decimal=2) @@ -139,7 +139,7 @@ def test_results_under_32km(): ]) # m/s for ii, h_ in enumerate(h): - T, p, rho, a = atm(h_) + T, p, rho, a = ISA1976(h_) assert_almost_equal(T, expected_T[ii], decimal=3) assert_almost_equal(rho, expected_rho[ii], decimal=4) assert_almost_equal(a, expected_a[ii], decimal=2) @@ -170,7 +170,7 @@ def test_results_under_47km(): ]) # m / s for ii, h_ in enumerate(h): - T, p, rho, a = atm(h_) + T, p, rho, a = ISA1976(h_) assert_almost_equal(T, expected_T[ii], decimal=3) assert_almost_equal(rho, expected_rho[ii], decimal=4) assert_almost_equal(a, expected_a[ii], decimal=2) @@ -197,7 +197,7 @@ def test_results_under_51km(): ]) # m / s for ii, h_ in enumerate(h): - T, p, rho, a = atm(h_) + T, p, rho, a = ISA1976(h_) assert_almost_equal(T, expected_T[ii], decimal=3) assert_almost_equal(rho, expected_rho[ii], decimal=4) assert_almost_equal(a, expected_a[ii], decimal=2) @@ -224,7 +224,7 @@ def test_results_under_71km(): ]) # m / s for ii, h_ in enumerate(h): - T, p, rho, a = atm(h_) + T, p, rho, a = ISA1976(h_) assert_almost_equal(T, expected_T[ii], decimal=3) assert_almost_equal(rho, expected_rho[ii], decimal=4) assert_almost_equal(a, expected_a[ii], decimal=2) @@ -251,7 +251,7 @@ def test_results_under_84km(): ]) # m/s for ii, h_ in enumerate(h): - T, p, rho, a = atm(h_) + T, p, rho, a = ISA1976(h_) assert_almost_equal(T, expected_T[ii], decimal=3) assert_almost_equal(rho, expected_rho[ii], decimal=4) assert_almost_equal(a, expected_a[ii], decimal=2) diff --git a/src/pyfme/environment/wind.py b/src/pyfme/environment/wind.py new file mode 100644 index 0000000..1ae12fc --- /dev/null +++ b/src/pyfme/environment/wind.py @@ -0,0 +1,21 @@ +""" +Python Flight Mechanics Engine (PyFME). +Copyright (c) AeroPython Development Team. +Distributed under the terms of the MIT License. + +Wind Models +----------- + +""" +import numpy as np + +class NoWind(object): + + def __init__(self): + # Wind velocity: FROM North to South, FROM East to West, + # Wind velocity in the UPSIDE direction + self.horizon_wind = np.zeros([3], dtype=float) + self.body_wind = np.zeros([3], dtype=float) + + def update(self, system): + pass diff --git a/src/pyfme/models/__init__.py b/src/pyfme/models/__init__.py index e69de29..f06ce65 100644 --- a/src/pyfme/models/__init__.py +++ b/src/pyfme/models/__init__.py @@ -0,0 +1 @@ +from .systems import EulerFlatEarth \ No newline at end of file diff --git a/src/pyfme/models/constants.py b/src/pyfme/models/constants.py new file mode 100644 index 0000000..d940510 --- /dev/null +++ b/src/pyfme/models/constants.py @@ -0,0 +1,31 @@ +""" +Python Flight Mechanics Engine (PyFME). +Copyright (c) AeroPython Development Team. +Distributed under the terms of the MIT License. + +Constant variables +------------------ + +""" +# AIR +GAMMA_AIR = 1.401 # Adiabatic index or ratio of specific heats (dry air at + # 20º C) +R_AIR = 287.05287 # J/(Kg·K) +# Sea level conditions +RHO_0 = 1.225 # density at sea level (kg/m3) +P_0 = 101325 # pressure at sea level (Pa) +SOUND_VEL_0 = 340.293990543 # sound speed at sea level (m/s) + +# GRAVITY of EARTH +GRAVITY = 9.80665 # m/s^2 +# Standard Gravitational Parameter is the product of the gravitational +# constant G and the mass M of the body. +STD_GRAVITATIONAL_PARAMETER = 3.986004418e14 # m³/s² +EARTH_MASS = 5.9722e24 # kg +GRAVITATIONAL_CONSTANT = 6.67384e11 # N·m²/kg² + +# Conversions +lbs2kg = 0.453592 +ft2m = 0.3048 +slug2kg = 14.5939 +slugft2_2_kgm2 = 1.35581795 \ No newline at end of file diff --git a/src/pyfme/models/euler_flat_earth.py b/src/pyfme/models/euler_flat_earth.py index cd5dec9..363112a 100644 --- a/src/pyfme/models/euler_flat_earth.py +++ b/src/pyfme/models/euler_flat_earth.py @@ -18,8 +18,8 @@ import numpy as np -def linear_and_angular_momentum_eqs(time, vel, mass, inertia, forces, moments): - """Linear and angular momentum equations +def lamceq(time, vel, mass, inertia, forces, moments): + """Linear and angular momentum conservation equations Parameters ---------- @@ -35,10 +35,10 @@ def linear_and_angular_momentum_eqs(time, vel, mass, inertia, forces, moments): Current equations assume that the aircraft has a symmetry plane (x_b - z_b), thus J_xy and J_yz must be null. forces : array_like - 3 dimensional vector containing the total forces (including gravity) in + 3 dimensional vector containing the total total_forces (including gravity) in x_b, y_b, z_b axes (N). moments : array_like - 3 dimensional vector containing the total moments in x_b, y_b, z_b axes + 3 dimensional vector containing the total total_moments in x_b, y_b, z_b axes (N·m). Returns @@ -50,7 +50,7 @@ def linear_and_angular_momentum_eqs(time, vel, mass, inertia, forces, moments): See Also -------- - navigation_eqs, kinematic_angular_eqs + kleq, kaeq References ---------- @@ -67,9 +67,9 @@ def linear_and_angular_momentum_eqs(time, vel, mass, inertia, forces, moments): Iy = inertia[1, 1] Iz = inertia[2, 2] - # Note definition of moments of inertia p.21 Gomez Tierno, et al Mecánica + # Note definition of total_moments of inertia p.21 Gomez Tierno, et al Mecánica # de vuelo - # TODO: define moments of inertia like this for all the code. + # TODO: define total_moments of inertia like this for all the code. Jxz = - inertia[0, 2] Fx, Fy, Fz = forces @@ -96,7 +96,7 @@ def linear_and_angular_momentum_eqs(time, vel, mass, inertia, forces, moments): return np.array([du_dt, dv_dt, dw_dt, dp_dt, dq_dt, dr_dt]) -def jac_linear_and_angular_momentum_eqs(time, vel, mass, inertia): +def lamceq_jac(time, vel, mass, inertia): """ Jacobian of linear and angular momentum equations Parameters @@ -121,7 +121,7 @@ def jac_linear_and_angular_momentum_eqs(time, vel, mass, inertia): See Also -------- - linear_and_angular_momentum_eqs + lamceq References ---------- @@ -136,9 +136,9 @@ def jac_linear_and_angular_momentum_eqs(time, vel, mass, inertia): Iy = inertia[1, 1] Iz = inertia[2, 2] - # Note definition of moments of inertia p.21 Gomez Tierno, et al Mecánica + # Note definition of total_moments of inertia p.21 Gomez Tierno, et al Mecánica # de vuelo - # TODO: define moments of inertia like this for all the code. + # TODO: define total_moments of inertia like this for all the code. Jxz = - inertia[0, 2] @@ -178,7 +178,7 @@ def jac_linear_and_angular_momentum_eqs(time, vel, mass, inertia): return jac -def kinematic_angular_eqs(time, euler_angles, ang_vel): +def kaeq(time, euler_angles, ang_vel): """ Kinematic angular equations Parameters @@ -200,7 +200,7 @@ def kinematic_angular_eqs(time, euler_angles, ang_vel): See Also -------- - linear_and_angular_momentum_eqs, navigation_eqs + lamceq, kleq References ---------- @@ -225,7 +225,7 @@ def kinematic_angular_eqs(time, euler_angles, ang_vel): return np.array([dtheta_dt, dphi_dt, dpsi_dt]) -def jac_kinematic_angular_eqs(time, euler_angles, ang_vel): +def kaeq_jac(time, euler_angles, ang_vel): """Jacobian of kinematic angular equations Parameters @@ -247,7 +247,7 @@ def jac_kinematic_angular_eqs(time, euler_angles, ang_vel): See Also -------- - kinematic_angular_eqs + kaeq References ---------- @@ -275,7 +275,7 @@ def jac_kinematic_angular_eqs(time, euler_angles, ang_vel): return jac -def navigation_eqs(time, lin_vel, euler_angles): +def kleq(time, lin_vel, euler_angles): """Kinematic linear equations Parameters @@ -297,7 +297,7 @@ def navigation_eqs(time, lin_vel, euler_angles): See Also -------- - kinematic_angular_eqs, linear_and_angular_momentum_eqs + kaeq, lamceq References ---------- diff --git a/src/pyfme/models/system.py b/src/pyfme/models/system.py deleted file mode 100644 index 970a9a1..0000000 --- a/src/pyfme/models/system.py +++ /dev/null @@ -1,113 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Python Flight Mechanics Engine (PyFME). -Copyright (c) AeroPython Development Team. -Distributed under the terms of the MIT License. - -System Class ------------- -The long-term objective is to a generic class System which can handle all the -models (ie. euler_flat_earth, quaternions_rotating_earth, -quaternions_rotating_earth_variable_mass...) - -Meanwhile, this class can perform the propagation for the euler_flat_earth eqs -""" - -import numpy as np -from scipy.integrate import ode - - -from pyfme.models import euler_flat_earth - - -models = ('euler_flat_earth',) - - -class System(object): - """ - Class managing the integration for the set of equations defining the model. - """ - - def __init__(self, integrator='dopri5', model='euler_flat_earth', - jac=False, **integrator_params): - """ - Initialize the equations of the chosen model and selects the - integrator. Check `scipy.integrate.ode` to see available integrators. - - If jac = True the jacobian of the equations is used for the - integration. - """ - - if model not in models: - raise ValueError('The specified model is not available, please \ - check available models in ...') - - if jac: - jac_LM_and_AM = euler_flat_earth.jac_linear_and_angular_momentum_eqs - jac_att = euler_flat_earth.jac_linear_and_angular_momentum_eqs - jac_nav = None # not implemented - else: - jac_LM_and_AM = None - jac_att = None - jac_nav = None - - self._LM_and_AM_eqs = ode(euler_flat_earth.linear_and_angular_momentum_eqs, - jac=jac_LM_and_AM) - self._attitude_eqs = ode(euler_flat_earth.kinematic_angular_eqs, - jac=jac_att) - self._navigation_eqs = ode(euler_flat_earth.navigation_eqs, - jac=jac_nav) - - self._LM_and_AM_eqs.set_integrator(integrator, **integrator_params) - self._attitude_eqs.set_integrator(integrator, **integrator_params) - self._navigation_eqs.set_integrator(integrator, **integrator_params) - - # State vector must be initialized with set_initial_values() method - self.state_vector = None - - def set_initial_values(self, u, v, w, p, q, r, theta, phi, psi, - x, y, z, t0=0.0): - """ - Set the initial values of the required variables - """ - - self._LM_and_AM_eqs.set_initial_value(y=(u, v, w, p, q, r), t=t0) - self._attitude_eqs.set_initial_value(y=(theta, phi, psi), t=t0) - self._navigation_eqs.set_initial_value(y=(x, y, z), t=t0) - - self.state_vector = np.array([u, v, w, p, q, r, theta, phi, psi, x, y, z]) - - def propagate(self, mass, inertia, forces, moments, dt=0.1): - """ - Performs integration step for actual_time + dt and returns the state - vector - """ - actual_time = self._LM_and_AM_eqs.t - t = actual_time + dt - - self._LM_and_AM_eqs.set_f_params(mass, inertia, forces, moments) - velocities = self._LM_and_AM_eqs.integrate(t) - - if self._LM_and_AM_eqs.successful(): - self._attitude_eqs.set_f_params(velocities[3:]) - attitude_angles = self._attitude_eqs.integrate(t) - else: - raise RuntimeError('Integration of Linear and angular momentum \ - equations was not succesfull') - - if self._attitude_eqs.successful(): - self._navigation_eqs.set_f_params(velocities[0:3], attitude_angles) - position = self._navigation_eqs.integrate(t) - else: - raise RuntimeError('Integration of attitude equations was not \ - succesfull') - - if self._navigation_eqs.successful(): - self.state_vector = np.concatenate((velocities, - attitude_angles, - position)) - else: - raise RuntimeError('Integration of navigation equations was not \ - succesfull') - - return self.state_vector diff --git a/src/pyfme/models/systems.py b/src/pyfme/models/systems.py new file mode 100644 index 0000000..960f4cb --- /dev/null +++ b/src/pyfme/models/systems.py @@ -0,0 +1,274 @@ +""" +Python Flight Mechanics Engine (PyFME). +Copyright (c) AeroPython Development Team. +Distributed under the terms of the MIT License. + +Dynamic Systems +--------------- + +""" +from abc import abstractmethod + +import numpy as np +from scipy.integrate import ode + +from pyfme.utils.coordinates import body2hor + + +class System(object): + + def __init__(self, lat, lon, h, psi=0, x_earth=0, y_earth=0): + + # ABOUT UNITS: assume international system units unless otherwise + # indicated. (meters, kilograms, seconds, kelvin, Newtons, Pascal, + # radians...) + # POSITION + # Geographic coordinates: (geodetic lat, lon, height above ellipsoid) + self.coord_geographic = np.array([lat, lon, h], dtype=float) + # Geocentric coordinates (rotating with Earth): (x_geo, y_geo, z_geo) + # TODO: implement geographic2geocentric conversion: + # self.coord_geocentric = np.zeros_like([3], dtype=float) + + # Earth coordinates (Axis parallel to local horizon NED at h=0 at + # the initial position of the airplane): (x_earth, y_earth, z_earth) + self.coord_earth = np.array([x_earth, y_earth, -h], dtype=float) + + # ATTITUDE (psi, theta, phi). + self.euler_angles = np.zeros([3], dtype=float) + self.euler_angles[0] = psi + # TODO: convert to quaternions + # self.quaternions = np.zeros([4], dtype=float) + + # ABSOLUTE VELOCITY. + # Body: + self.vel_body = np.zeros_like([3], dtype=float) + # Local horizon (NED): + self.vel_NED = np.zeros_like([3], dtype=float) + + # ANGULAR VELOCITY: (p, q, r) + self.vel_ang = np.zeros_like([3], dtype=float) + + # Last time step dt + self.dt = None + + # TODO: guarantee that if euler angles change <--> quaternions change + # TODO: guarantee that if geographic change <--> geocentric change + # TODO: guarantee that if body vels change <--> horizon vels change + @property + def lat(self): + return self.coord_geographic[0] + + @property + def lon(self): + return self.coord_geographic[1] + + @property + def height(self): + return self.coord_geographic[2] + + @property + def x_geo(self): + return self.coord_geocentric[0] + + @property + def y_geo(self): + return self.coord_geocentric[1] + + @property + def z_geo(self): + return self.coord_geocentric[2] + + @property + def x_earth(self): + return self.coord_earth[0] + + @property + def y_earth(self): + return self.coord_earth[1] + + @property + def z_earth(self): + return self.coord_earth[2] + + @property + def psi(self): + return self.euler_angles[0] + + @property + def theta(self): + return self.euler_angles[1] + + @property + def phi(self): + return self.euler_angles[2] + + @property + def u(self): + return self.vel_body[0] + + @property + def v(self): + return self.vel_body[1] + + @property + def w(self): + return self.vel_body[2] + + @property + def v_north(self): + return self.vel_NED[0] + + @property + def v_east(self): + return self.vel_NED[1] + + @property + def v_down(self): + return self.vel_NED[2] + + @property + def p(self): + return self.vel_ang[0] + + @property + def q(self): + return self.vel_ang[1] + + @property + def r(self): + return self.vel_ang[2] + + @abstractmethod + def _propagate_state_vector(self, aircraft, dt): + pass + + @abstractmethod + def set_initial_state_vector(self): + pass + + def propagate(self, aircraft, environment, dt=0.01): + pass + +class EulerFlatEarth(System): + + def __init__(self, lat, lon, h, psi=0, x_earth=0, y_earth=0, + integrator='dopri5', use_jac=False, **integrator_params): + """ + Initialize the equations of the chosen model and selects the + integrator. Check `scipy.integrate.ode` to see available integrators. + + If use_jac = True the jacobian of the equations is used for the + integration. + """ + super().__init__(lat, lon, h, psi, x_earth, y_earth) + # State vector must be initialized with set_initial_state_vector() method + self.state_vector = None + + from pyfme.models.euler_flat_earth import lamceq, kaeq, kleq + self.lamceq = lamceq + + if use_jac: + from pyfme.models.euler_flat_earth import lamceq_jac, kaeq_jac + jac_LM_and_AM = lamceq_jac + jac_att = kaeq_jac + jac_nav = None # not implemented + else: + jac_LM_and_AM = None + jac_att = None + jac_nav = None + + self._ode_lamceq = ode(lamceq, jac=jac_LM_and_AM) + self._ode_kaqeq = ode(kaeq, jac=jac_att) + self._ode_kleq = ode(kleq, jac=jac_nav) + + self._ode_lamceq.set_integrator(integrator, **integrator_params) + self._ode_kaqeq.set_integrator(integrator, **integrator_params) + self._ode_kleq.set_integrator(integrator, **integrator_params) + + @property + def height(self): + return -self.coord_earth[2] + + 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 _propagate_state_vector(self, aircraft, dt): + """ + Performs integration step for actual_time + dt and returns the state + vector + """ + mass = aircraft.mass + inertia = aircraft.inertia + forces = aircraft.total_forces + moments = aircraft.total_moments + + t = self._ode_lamceq.t + dt + + self._ode_lamceq.set_f_params(mass, inertia, forces, moments) + velocities = self._ode_lamceq.integrate(t) + + if self._ode_lamceq.successful(): + self._ode_kaqeq.set_f_params(velocities[3:]) + attitude_angles = self._ode_kaqeq.integrate(t) + else: + raise RuntimeError('Integration of Linear and angular momentum \ + equations was not successful') + + if self._ode_kaqeq.successful(): + self._ode_kleq.set_f_params(velocities[0:3], attitude_angles) + position = self._ode_kleq.integrate(t) + else: + raise RuntimeError('Integration of attitude equations was not \ + successful') + + if self._ode_kleq.successful(): + self.state_vector[0:6] = velocities[:] + self.state_vector[6:9] = attitude_angles[:] + self.state_vector[9:12] = position[:] + else: + raise RuntimeError('Integration of navigation equations was not \ + successful') + + return self.state_vector + + 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) diff --git a/src/pyfme/models/tests/test_euler_flat_earth.py b/src/pyfme/models/tests/test_euler_flat_earth.py index 5717c47..e136f82 100644 --- a/src/pyfme/models/tests/test_euler_flat_earth.py +++ b/src/pyfme/models/tests/test_euler_flat_earth.py @@ -5,11 +5,11 @@ import numpy as np -from pyfme.models.euler_flat_earth import (linear_and_angular_momentum_eqs, - jac_linear_and_angular_momentum_eqs, - kinematic_angular_eqs, - jac_kinematic_angular_eqs, - navigation_eqs) +from pyfme.models.euler_flat_earth import (lamceq, + lamceq_jac, + kaeq, + kaeq_jac, + kleq) def test1_linear_and_angular_momentum_eqs(): @@ -24,8 +24,8 @@ def test1_linear_and_angular_momentum_eqs(): moments = np.array([100., 1000., 100], dtype=float) expected_sol = np.array([10, 10, 10, 11./9, 1, 92./9], dtype=float) - sol = linear_and_angular_momentum_eqs(time, vel, mass, inertia, forces, - moments) + sol = lamceq(time, vel, mass, inertia, forces, + moments) assert(np.allclose(expected_sol, sol)) @@ -41,8 +41,8 @@ def test2_linear_and_angular_momentum_eqs(): moments = np.array([100, 100, 100], dtype=float) expected_sol = np.array([99, 1, 2, 10./9, 1, 10./9], dtype=float) - sol = linear_and_angular_momentum_eqs(time, vel, mass, inertia, forces, - moments) + sol = lamceq(time, vel, mass, inertia, forces, + moments) assert(np.allclose(expected_sol, sol)) @@ -83,7 +83,7 @@ def test1_jac_linear_and_angular_momentum_eqs(): expected_sol[5, 4] = 9 expected_sol[5, 5] = - 10./9 - sol = jac_linear_and_angular_momentum_eqs(time, vel, mass, inertia) + sol = lamceq_jac(time, vel, mass, inertia) assert(np.allclose(expected_sol, sol)) @@ -114,7 +114,7 @@ def test2_jac_linear_and_angular_momentum_eqs(): expected_sol[5, 3] = 1./99 expected_sol[5, 5] = - 10./99 - sol = jac_linear_and_angular_momentum_eqs(time, vel, mass, inertia) + sol = lamceq_jac(time, vel, mass, inertia) assert(np.allclose(expected_sol, sol)) @@ -126,7 +126,7 @@ def test1_kinematic_angular_eqs(): ang_vel = np.array([1, 1, 1], dtype=float) expected_sol = np.array([0, 1 + 2 ** 0.5, 2]) - sol = kinematic_angular_eqs(time, euler_angles, ang_vel) + sol = kaeq(time, euler_angles, ang_vel) assert(np.allclose(expected_sol, sol)) @@ -138,7 +138,7 @@ def test2_kinematic_angular_eqs(): ang_vel = np.array([0, 1, 0], dtype=float) expected_sol = np.array([0, 0, 1], dtype=float) - sol = kinematic_angular_eqs(time, euler_angles, ang_vel) + sol = kaeq(time, euler_angles, ang_vel) assert(np.allclose(expected_sol, sol)) @@ -154,7 +154,7 @@ def test1_jac_kinematic_angular_eqs(): expected_sol[1, 0] = 2 * 2 ** 0.5 expected_sol[2, 0] = 2 - sol = jac_kinematic_angular_eqs(time, euler_angles, ang_vel) + sol = kaeq_jac(time, euler_angles, ang_vel) assert(np.allclose(expected_sol, sol)) @@ -169,7 +169,7 @@ def test2_jac_kinematic_angular_eqs(): expected_sol[0, 1] = - 1 expected_sol[1, 0] = 1 - sol = jac_kinematic_angular_eqs(time, euler_angles, ang_vel) + sol = kaeq_jac(time, euler_angles, ang_vel) assert(np.allclose(expected_sol, sol)) @@ -181,7 +181,7 @@ def test1_navigation_eqs(): euler_angles = np.array([np.pi / 4, np.pi / 4, 0]) expected_sol = np.array([1 + (2 ** 0.5) / 2, 0, 1 - (2 ** 0.5) / 2]) - sol = navigation_eqs(time, lin_vel, euler_angles) + sol = kleq(time, lin_vel, euler_angles) assert(np.allclose(expected_sol, sol)) @@ -193,6 +193,6 @@ def test2_navigation_eqs(): euler_angles = np.array([0, np.pi / 2, 0]) expected_sol = np.array([1, - 1, 0], dtype=float) - sol = navigation_eqs(time, lin_vel, euler_angles) + sol = kleq(time, lin_vel, euler_angles) assert(np.allclose(expected_sol, sol)) diff --git a/src/pyfme/simulator.py b/src/pyfme/simulator.py new file mode 100644 index 0000000..68947b8 --- /dev/null +++ b/src/pyfme/simulator.py @@ -0,0 +1,232 @@ +""" +Python Flight Mechanics Engine (PyFME). +Copyright (c) AeroPython Development Team. +Distributed under the terms of the MIT License. + +Simulation class +---------------- + +""" +from abc import abstractmethod +import numpy as np + + +class Simulation(object): + + def __init__(self, aircraft, system, environment): + self.aircraft = aircraft + self.system = system + self.environment = environment + self._time_step = 0 + self.PAR_KEYS = {'T': self.environment.T, # env + 'pressure': self.environment.p, + 'rho': self.environment.rho, + 'a': self.environment.a, + 'TAS': self.aircraft.TAS, # aircraft + 'Mach': self.aircraft.Mach, + 'q_inf': self.aircraft.q_inf, + 'alpha': self.aircraft.alpha, + 'beta': self.aircraft.beta, + 'x_earth': self.system.x_earth, # system + 'y_earth': self.system.y_earth, + 'z_earth': self.system.z_earth, + 'psi': self.system.psi, + 'theta': self.system.theta, + 'phi': self.system.phi, + 'u': self.system.u, + 'v': self.system.v, + 'w': self.system.w, + 'v_north': self.system.v_north, + 'v_east': self.system.v_east, + 'v_down': self.system.v_down, + 'p': self.system.p, + 'q': self.system.q, + 'r': self.system.r, + 'height': self.system.height, + 'F_xb': self.aircraft.total_forces[0], + 'F_yb': self.aircraft.total_forces[1], + 'F_zb': self.aircraft.total_forces[2], + 'M_xb': self.aircraft.total_moments[0], + 'M_yb': self.aircraft.total_moments[1], + 'M_zb': self.aircraft.total_moments[2] + } + self.par_dict = {} + + def time_step(self, dt): + + self.save_current_par_dict() + self._time_step += 1 + self.system.propagate(self.aircraft, dt) + self.environment.update(self.system) + controls = self._get_current_controls(self._time_step) + self.aircraft.update(controls, self.system, self.environment) + self.aircraft.calculate_forces_and_moments() + + @abstractmethod + def _get_current_controls(self, ii): + return + + @abstractmethod + def set_par_dict(self, par_list): + return + + @abstractmethod + def save_current_par_dict(self): + return + + +class BatchSimulation(Simulation): + + def __init__(self, aircraft, system, environment): + super().__init__(aircraft, system, environment) + self.time = None + self.aircraft_controls = {} + + def set_controls(self, time, controls): + """Set the time history of controls and the corresponding times. + + Parameters + ---------- + time + controls + + Returns + ------- + + """ + + # check time dimensions + if time.ndim != 1: + raise ValueError('Time must be unidimensional') + tsize = time.size + # check dimensions + for c in controls: + if controls[c].size != tsize: + msg = 'Control {} size ({}) does not match time size ({' \ + '})'.fromat(c, c.size, tsize) + raise ValueError(msg) + self.time = time + self.aircraft_controls = controls + + def _get_current_controls(self, ii): + """Returns controls at time step ii. + + Parameters + ---------- + ii + + Returns + ------- + + """ + return {control: self.aircraft_controls[control][ii] for control in + self.aircraft_controls.keys()} + + def run_simulation(self): + + for ii, t in enumerate(self.time[1:]): + dt = t - self.time[ii] + self.time_step(dt) + # Save last time step + self.save_current_par_dict() + + def set_par_dict(self, par_list): + + if self.time is None: + msg = "Set controls with BatchSimulation.set_controls before " \ + "setting the par_dict" + raise RuntimeError(msg) + + for par_name in par_list: + if par_name in self.PAR_KEYS: + self.par_dict[par_name] = np.empty_like(self.time) + else: + msg = "{} not found in PAR_KEYS".format(par_name) + raise RuntimeWarning(msg) + + def save_current_par_dict(self): + self.PAR_KEYS = {'T': self.environment.T, # env + 'pressure': self.environment.p, + 'rho': self.environment.rho, + 'a': self.environment.a, + 'TAS': self.aircraft.TAS, # aircraft + 'Mach': self.aircraft.Mach, + 'q_inf': self.aircraft.q_inf, + 'alpha': self.aircraft.alpha, + 'beta': self.aircraft.beta, + 'x_earth': self.system.x_earth, # system + 'y_earth': self.system.y_earth, + 'z_earth': self.system.z_earth, + 'psi': self.system.psi, + 'theta': self.system.theta, + 'phi': self.system.phi, + 'u': self.system.u, + 'v': self.system.v, + 'w': self.system.w, + 'v_north': self.system.v_north, + 'v_east': self.system.v_east, + 'v_down': self.system.v_down, + 'p': self.system.p, + 'q': self.system.q, + 'r': self.system.r, + 'height': self.system.height, + 'F_xb': self.aircraft.total_forces[0], + 'F_yb': self.aircraft.total_forces[1], + 'F_zb': self.aircraft.total_forces[2], + 'M_xb': self.aircraft.total_moments[0], + 'M_yb': self.aircraft.total_moments[1], + 'M_zb': self.aircraft.total_moments[2] + } + for par_name, par_values in self.par_dict.items(): + par_values[self._time_step] = self.PAR_KEYS[par_name] + + + +class RealTimeSimulation(Simulation): + + def __init__(self, aircraft, system, environment): + super(RealTimeSimulation, self).__init__(aircraft, system, environment) + # TODO:... + + def _get_current_controls(self, ii): + # Joystick reading + raise NotImplementedError + + def set_par_dict(self, par_list): + + for par_name in par_list: + if par_name in self.PAR_KEYS: + self.par_dict[par_name] = [] + else: + msg = "{} not found in PAR_KEYS".format(par_name) + raise RuntimeWarning(msg) + + def save_current_par_dict(self): + self.PAR_KEYS = {'T': self.environment.T, # env + 'pressure': self.environment.p, + 'rho': self.environment.rho, + 'a': self.environment.a, + 'TAS': self.aircraft.TAS, # aircraft + 'Mach': self.aircraft.Mach, + 'q_inf': self.aircraft.q_inf, + 'alpha': self.aircraft.alpha, + 'beta': self.aircraft.beta, + 'x_earth': self.system.x_earth, # system + 'y_earth': self.system.y_earth, + 'z_earth': self.system.z_earth, + 'psi': self.system.psi, + 'theta': self.system.theta, + 'phi': self.system.phi, + 'u': self.system.u, + 'v': self.system.v, + 'w': self.system.w, + 'v_north': self.system.v_north, + 'v_east': self.system.v_east, + 'v_down': self.system.v_down, + 'p': self.system.p, + 'q': self.system.q, + 'r': self.system.r, + 'height': self.system.height + } + for par_name, par_values in self.par_dict: + par_values.append(self.PAR_KEYS[par_name]) diff --git a/src/pyfme/utils/anemometry.py b/src/pyfme/utils/anemometry.py index 29345d4..412825e 100644 --- a/src/pyfme/utils/anemometry.py +++ b/src/pyfme/utils/anemometry.py @@ -24,12 +24,13 @@ """ from math import asin, atan, sqrt +from pyfme.models.constants import RHO_0, P_0, SOUND_VEL_0, GAMMA_AIR -rho_0 = 1.225 # density at sea level (kg/m3) -p_0 = 101325 # pressure at sea level (Pa) -a_0 = 340.293990543 # sound speed at sea level (m/s) -gamma = 1.4 # heat capacity ratio +rho_0 = RHO_0 # density at sea level (kg/m3) +p_0 = P_0 # pressure at sea level (Pa) +a_0 = SOUND_VEL_0 # sound speed at sea level (m/s) +gamma = GAMMA_AIR # heat capacity ratio def calculate_alpha_beta_TAS(u, v, w): diff --git a/src/pyfme/utils/tests/test_anemometry_stagnationpressure.py b/src/pyfme/utils/tests/test_anemometry_stagnationpressure.py index 812121e..7e6b3b7 100644 --- a/src/pyfme/utils/tests/test_anemometry_stagnationpressure.py +++ b/src/pyfme/utils/tests/test_anemometry_stagnationpressure.py @@ -10,19 +10,14 @@ from numpy.testing import (assert_almost_equal) -from pyfme.utils.anemometry import (stagnation_pressure) -from pyfme.environment.isa import atm - -rho_0 = 1.225 # density at sea level (kg/m3) -p_0 = 101325 # pressure at sea level (Pa) -a_0 = 340.293990543 # sound speed at sea level (m/s) -gamma = 1.4 # heat capacity ratio +from pyfme.utils.anemometry import stagnation_pressure +from pyfme.environment.atmosphere import ISA1976 def test_stagnation_pressure(): # subsonic case - _, p, _, a = atm(11000) + _, p, _, a = ISA1976(11000) tas = 240 p_stagnation_expected = 34952.7493849545 @@ -30,7 +25,7 @@ def test_stagnation_pressure(): assert_almost_equal(p_stagnation, p_stagnation_expected) # supersonic case - _, p, _, a = atm(11000) + _, p, _, a = ISA1976(11000) tas = 400 p_stagnation_expected = 65521.299596290904 diff --git a/src/pyfme/utils/tests/test_anemometry_tascaseas.py b/src/pyfme/utils/tests/test_anemometry_tascaseas.py index 9859653..c3d9f90 100644 --- a/src/pyfme/utils/tests/test_anemometry_tascaseas.py +++ b/src/pyfme/utils/tests/test_anemometry_tascaseas.py @@ -12,12 +12,13 @@ from pyfme.utils.anemometry import (tas2eas, eas2tas, cas2eas, eas2cas, tas2cas, cas2tas) -from pyfme.environment.isa import atm +from pyfme.models.constants import RHO_0, P_0, SOUND_VEL_0, GAMMA_AIR +from pyfme.environment.atmosphere import ISA1976 -rho_0 = 1.225 # density at sea level (kg/m3) -p_0 = 101325 # pressure at sea level (Pa) -a_0 = 340.293990543 # sound speed at sea level (m/s) -gamma = 1.4 # heat capacity ratio +RHO_0 = 1.225 # density at sea level (kg/m3) +P_0 = 101325 # pressure at sea level (Pa) +SOUND_VEL_0 = 340.293990543 # sound speed at sea level (m/s) +GAMMA_AIR = 1.4 # heat capacity ratio def test_tas2eas(): @@ -26,12 +27,12 @@ def test_tas2eas(): tas = 275 eas_expected = 275 - eas = tas2eas(tas, rho_0) + eas = tas2eas(tas, RHO_0) assert_almost_equal(eas, eas_expected) # Test at 11000m - _, _, rho, _ = atm(11000) + _, _, rho, _ = ISA1976(11000) tas = 275 eas_expected = 149.88797172756003 @@ -46,12 +47,12 @@ def test_eas2tas(): eas = 149.88797172756003 tas_expected = 149.88797172756003 - tas = eas2tas(eas, rho_0) + tas = eas2tas(eas, RHO_0) assert_almost_equal(tas, tas_expected) # Test at 11000m - _, _, rho, _ = atm(11000) + _, _, rho, _ = ISA1976(11000) eas = 149.88797172756003 tas_expected = 275 @@ -66,12 +67,12 @@ def test_tas2cas(): tas = 275 cas_expected = 275 - cas = tas2cas(tas, p_0, rho_0) + cas = tas2cas(tas, P_0, RHO_0) assert_almost_equal(cas, cas_expected) # Test at 11000m - _, p, rho, _ = atm(11000) + _, p, rho, _ = ISA1976(11000) tas = 275 cas_expected = 162.03569680495048 @@ -86,12 +87,12 @@ def test_cas2tas(): cas = 275 tas_expected = 275 - tas = cas2tas(cas, p_0, rho_0) + tas = cas2tas(cas, P_0, RHO_0) assert_almost_equal(tas, tas_expected) # Test at 11000m - _, p, rho, _ = atm(11000) + _, p, rho, _ = ISA1976(11000) cas = 162.03569680495048 tas_expected = 275 @@ -106,12 +107,12 @@ def test_cas2eas(): cas = 275 eas_expected = 275 - eas = cas2eas(cas, p_0, rho_0) + eas = cas2eas(cas, P_0, RHO_0) assert_almost_equal(eas, eas_expected) # Test at 11000m - _, p, rho, _ = atm(11000) + _, p, rho, _ = ISA1976(11000) cas = 162.03569680495048 eas_expected = 149.88797172756003 @@ -126,12 +127,12 @@ def test_eas2cas(): eas = 275 cas_expected = 275 - cas = eas2cas(eas, p_0, rho_0) + cas = eas2cas(eas, P_0, RHO_0) assert_almost_equal(cas, cas_expected) # Test at 11000m - _, p, rho, _ = atm(11000) + _, p, rho, _ = ISA1976(11000) eas = 149.88797172756003 cas_expected = 162.03569680495048 diff --git a/src/pyfme/utils/trimmer.py b/src/pyfme/utils/trimmer.py index f5d4b37..bff5191 100644 --- a/src/pyfme/utils/trimmer.py +++ b/src/pyfme/utils/trimmer.py @@ -13,17 +13,21 @@ Instead, it must be done with a numerical algorithm which iteratively adjusts the independent variables until some solution criterion is met. """ +from copy import deepcopy from warnings import warn + import numpy as np -from math import sqrt, sin, cos, tan, atan from scipy.optimize import least_squares -from pyfme.utils.coordinates import wind2body -from pyfme.environment.isa import atm +from pyfme.models.constants import GRAVITY -def steady_state_flight_trim(aircraft, h, TAS, gamma=0, turn_rate=0, - dyn_eqs=None, verbose=0): +def steady_state_flight_trimmer(aircraft, system, env, + TAS, + controls_0, controls2trim=None, + gamma=0.0, turn_rate=0.0, + verbose=0): + # TODO: write docstring again """Finds a combination of values of the state and control variables that correspond to a steady-state flight condition. Steady-state aircraft flight can be defined as a condition in which all of the motion variables are @@ -70,34 +74,49 @@ def steady_state_flight_trim(aircraft, h, TAS, gamma=0, turn_rate=0, .. [1] Stevens, BL and Lewis, FL, "Aircraft Control and Simulation", Wiley-lnterscience. """ - - if dyn_eqs is None: - from pyfme.models.euler_flat_earth import linear_and_angular_momentum_eqs - dynamic_eqs = linear_and_angular_momentum_eqs - - # TODO: try to look for a good inizialization method - alpha_0 = 0.05 * np.sign(gamma) - betha_0 = 0.001 * np.sign(turn_rate) - delta_e_0 = 0.05 - delta_ail_0 = 0.01 * np.sign(turn_rate) - delta_r_0 = 0.01 * np.sign(turn_rate) - delta_t_0 = 0.5 - - initial_guess = (alpha_0, - betha_0, - delta_e_0, - delta_ail_0, - delta_r_0, - delta_t_0) - - args = (h, TAS, gamma, turn_rate, aircraft, dynamic_eqs) - - # TODO: pass max deflection of the controls inside aircraft. - lower_bounds = (-1, -0.5, -1, -1, -1, 0) - upper_bounds = (+1, +0.5, +1, +1, +1, 1) - - results = least_squares(func, x0=initial_guess, args=args, verbose=verbose, - bounds=(lower_bounds, upper_bounds)) + # Creating a copy of these objects in order to not modify any attribute + # inside this funciton. + trimmed_ac = deepcopy(aircraft) + trimmed_sys = deepcopy(system) + trimmed_env = deepcopy(env) + + trimmed_ac.TAS = TAS + trimmed_ac.Mach = aircraft.TAS / env.a + trimmed_ac.q_inf = 0.5 * trimmed_env.rho * aircraft.TAS ** 2 + + # Update environment + trimmed_env.update(trimmed_sys) + + # Check if every necessary control for the aircraft is given in controls_0. + for ac_control in trimmed_ac.controls: + if ac_control not in controls_0: + raise ValueError("Control {} not given in controls_0: {}".format( + ac_control, controls_0)) + trimmed_ac.controls = controls_0 + + # If controls2trim is not given, trim for every control. + if controls2trim is None: + controls2trim = list(controls_0.keys()) + + # TODO: try to look for a good inizialization method for alpha & beta + initial_guess = [0.05 * np.sign(turn_rate), # alpha + 0.001 * np.sign(turn_rate)] # beta + + for control in controls2trim: + initial_guess.append(controls_0[control]) + + args = (trimmed_sys, trimmed_ac, trimmed_env, + controls2trim, gamma, turn_rate) + + lower_bounds = [-0.5, -0.25] # Alpha and beta upper bounds. + upper_bounds = [+0.5, +0.25] # Alpha and beta lower bounds. + for ii in controls2trim: + lower_bounds.append(aircraft.control_limits[ii][0]) + upper_bounds.append(aircraft.control_limits[ii][1]) + bounds = (lower_bounds, upper_bounds) + + results = least_squares(trimming_cost_func, x0=initial_guess, args=args, + verbose=verbose, bounds=bounds) trimmed_params = results['x'] fun = results['fun'] @@ -105,63 +124,47 @@ def steady_state_flight_trim(aircraft, h, TAS, gamma=0, turn_rate=0, if cost > 1e-7 or any(abs(fun) > 1e-3): warn("Trim process did not converge", RuntimeWarning) - if trimmed_params[5] > 0.99: - warn("Probably not enough power for demanded conditions") - alpha = trimmed_params[0] - beta = trimmed_params[1] + trimmed_sys.set_initial_state_vector() - delta_e = trimmed_params[2] - delta_ail = trimmed_params[3] - delta_r = trimmed_params[4] - delta_t = trimmed_params[5] + results = {'alpha': trimmed_ac.alpha, 'beta': trimmed_ac.beta, + 'u': trimmed_sys.u, 'v': trimmed_sys.v, 'w': trimmed_sys.w, + 'p': trimmed_sys.p, 'q': trimmed_sys.q, 'r': trimmed_sys.r, + 'theta': trimmed_sys.theta, 'phi': trimmed_sys.phi, + 'ls_opt': results} - # What happens if we have two engines and all that kind of things...? - control_vector = delta_e, delta_ail, delta_r, delta_t + for control in controls2trim: + results[control] = trimmed_ac.controls[control] - if abs(turn_rate) < 1e-8: - phi = 0 - else: - phi = turn_coord_cons(turn_rate, alpha, beta, TAS, gamma) + return trimmed_ac, trimmed_sys, trimmed_env, results - theta = rate_of_climb_cons(gamma, alpha, beta, phi) - - # w = turn_rate * k_h - # k_h = sin(theta) i_b + sin(phi) * cos(theta) j_b + cos(theta) * sin(phi) - # w = p * i_b + q * j_b + r * k_b - p = - turn_rate * sin(theta) - q = turn_rate * sin(phi) * cos(theta) - r = turn_rate * cos(theta) * sin(phi) - ang_vel = np.array([p, q, r]) - lin_vel = wind2body((TAS, 0, 0), alpha, beta) +from math import sqrt, sin, cos, tan, atan - return lin_vel, ang_vel, theta, phi, alpha, beta, control_vector +from pyfme.utils.coordinates import wind2body def turn_coord_cons(turn_rate, alpha, beta, TAS, gamma=0): """Calculates phi for coordinated turn. """ - g0 = 9.81 + g0 = GRAVITY G = turn_rate * TAS / g0 if abs(gamma) < 1e-8: phi = G * cos(beta) / (cos(alpha) - G * sin(alpha) * sin(beta)) phi = atan(phi) - else: a = 1 - G * tan(alpha) * sin(beta) b = sin(gamma) / cos(beta) - c = 1 + G**2 * cos(beta)**2 + c = 1 + G ** 2 * cos(beta) ** 2 - sq = sqrt(c * (1 - b**2) + G**2 * sin(beta)**2) + sq = sqrt(c * (1 - b ** 2) + G ** 2 * sin(beta) ** 2) - num = (a-b**2) + b * tan(alpha) * sq - den = a**2 - b**2 * (1 + c * tan(alpha)**2) + num = (a - b ** 2) + b * tan(alpha) * sq + den = a ** 2 - b ** 2 * (1 + c * tan(alpha) ** 2) phi = atan(G * cos(beta) / cos(alpha) * num / den) - return phi @@ -170,12 +173,10 @@ def turn_coord_cons_horizontal_and_small_beta(turn_rate, alpha, TAS): and beta is small (beta << 1). """ - g0 = 9.81 + g0 = GRAVITY G = turn_rate * TAS / g0 - phi = G / cos(alpha) phi = atan(phi) - return phi @@ -184,32 +185,34 @@ def rate_of_climb_cons(gamma, alpha, beta, phi): """ a = cos(alpha) * cos(beta) b = sin(phi) * sin(beta) + cos(phi) * sin(alpha) * cos(beta) - - sq = sqrt(a**2 - sin(gamma)**2 + b**2) - - theta = (a * b + sin(gamma) * sq) / (a**2 - sin(gamma)**2) + sq = sqrt(a ** 2 - sin(gamma) ** 2 + b ** 2) + theta = (a * b + sin(gamma) * sq) / (a ** 2 - sin(gamma) ** 2) theta = atan(theta) - return theta -def func(trimmed_params, h, TAS, gamma, turn_rate, aircraft, dynamic_eqs): +def trimming_cost_func(trimmed_params, system, ac, env, controls2trim, + gamma, turn_rate): """Function to optimize """ alpha = trimmed_params[0] beta = trimmed_params[1] - delta_e = trimmed_params[2] - delta_ail = trimmed_params[3] - delta_r = trimmed_params[4] - delta_t = trimmed_params[5] + new_controls = {} + for ii, control in enumerate(controls2trim): + new_controls[control] = trimmed_params[ii + 2] + # Choose coordinated turn constrain equation: if abs(turn_rate) < 1e-8: phi = 0 else: - phi = turn_coord_cons(turn_rate, alpha, beta, TAS, gamma) + phi = turn_coord_cons(turn_rate, alpha, beta, ac.TAS, gamma) + + system.euler_angles[2] = phi + # Rate of climb constrain theta = rate_of_climb_cons(gamma, alpha, beta, phi) + system.euler_angles[1] = theta # w = turn_rate * k_h # k_h = sin(theta) i_b + sin(phi) * cos(theta) j_b + cos(theta) * sin(phi) @@ -217,24 +220,13 @@ def func(trimmed_params, h, TAS, gamma, turn_rate, aircraft, dynamic_eqs): p = - turn_rate * sin(theta) q = turn_rate * sin(phi) * cos(theta) r = turn_rate * cos(theta) * sin(phi) + system.vel_ang = np.array([p, q, r]) + system.vel_body = wind2body((ac.TAS, 0, 0), alpha=alpha, beta=beta) - ang_vel = np.array([p, q, r]) - - lin_vel = wind2body((TAS, 0, 0), alpha, beta) - - # FIXME: This implied some changes in the aircraft model. - # psi angle does not influence the attitude of the aircraft for gravity - # force projection. So it is set to 0. - attitude = np.array([theta, phi, 0]) - _, _, rho, _ = atm(h) - forces, moments = aircraft.get_forces_and_moments(TAS, rho, alpha, beta, - delta_e, 0, delta_ail, - delta_r, delta_t, - attitude) - mass, inertia = aircraft.mass_and_inertial_data() - - vel = np.concatenate((lin_vel[:], ang_vel[:])) - - output = dynamic_eqs(0, vel, mass, inertia, forces, moments) + env.update(system) + ac.update(new_controls, system, env) + forces, moments = ac.calculate_forces_and_moments() + vel = np.concatenate((system.vel_body[:], system.vel_ang[:])) + output = system.lamceq(0, vel, ac.mass, ac.inertia, forces, moments) return output