diff --git a/.gitignore b/.gitignore index 1b8d2db..d8e5de6 100644 --- a/.gitignore +++ b/.gitignore @@ -71,6 +71,7 @@ ENV/ # IDEs .vscode/ .idea/ +.vs/ # OS generated files .DS_Store diff --git a/pyproject.toml b/pyproject.toml index 7b5d8c8..4b67553 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -29,6 +29,7 @@ dependencies = [ "pathsim", "numpy>=1.15", "scipy>=1.2", + "jsbsim>=1.2.4" ] [project.optional-dependencies] diff --git a/src/pathsim_flight/__init__.py b/src/pathsim_flight/__init__.py index f5eb4bd..38f709c 100644 --- a/src/pathsim_flight/__init__.py +++ b/src/pathsim_flight/__init__.py @@ -11,3 +11,8 @@ __version__ = "unknown" __all__ = ["__version__"] + +# For direct block import from main package +from .atmosphere import * +from .jsbsim import * +from .utils import * diff --git a/src/pathsim_flight/atmosphere/__init__.py b/src/pathsim_flight/atmosphere/__init__.py new file mode 100644 index 0000000..33c6723 --- /dev/null +++ b/src/pathsim_flight/atmosphere/__init__.py @@ -0,0 +1 @@ +from .international_standard_atmosphere import * \ No newline at end of file diff --git a/src/pathsim_flight/atmosphere/international_standard_atmosphere.py b/src/pathsim_flight/atmosphere/international_standard_atmosphere.py new file mode 100644 index 0000000..1085f27 --- /dev/null +++ b/src/pathsim_flight/atmosphere/international_standard_atmosphere.py @@ -0,0 +1,99 @@ +######################################################################################### +## +## International Standard Atmosphere Block +## +######################################################################################### + +# IMPORTS =============================================================================== + +from pathsim.blocks import Function +import math +from collections import namedtuple + +# BLOCKS ================================================================================ + +class ISAtmosphere(Function): + """International Standard Atmosphere. + + For a given geometric altitude and temperature deviation from standard day, + compute the pressure, density, temperature, and speed of sound. + + See - https://seanmcleod70.github.io/FlightDynamicsCalcs/InternationalStandardAtmosphere.html + """ + + input_port_labels = { + "altitude": 0, + "temp_deviation": 1 + } + + output_port_labels = { + "pressure": 0, + "density": 1, + "temperature": 2, + "speed_of_sound": 3 + } + + def __init__(self): + super().__init__(func=self._eval) + + # Constants + R = 287.0528 # Specific gas constant + g0 = 9.80665 # Gravitational acceleration + gamma = 1.4 # Air specific heat ratio + r0 = 6356766 # Earth radius + + StdSL_pressure = 101325 # Pa + StdSL_speed_of_sound = 340.294 # m/s + + # Atmosphere bands + AtmosphereBand = namedtuple('AtmosphereBand', ['start_alt', 'end_alt', + 'base_temperature', 'base_pressure', + 'lapse_rate']) + + atmosphere_bands = [ + AtmosphereBand(0, 11000, 288.15, 101325, -0.0065), + AtmosphereBand(11000, 20000, 216.65, 22632, 0.0), + AtmosphereBand(20000, 32000, 216.65, 5474.9, 0.001), + AtmosphereBand(32000, 47000, 228.65, 868.02, 0.0028), + AtmosphereBand(47000, 51000, 270.65, 110.91, 0.0), + AtmosphereBand(51000, 71000, 270.65, 66.939, -0.0028), + AtmosphereBand(71000, 84852, 214.65, 3.9564, -0.002), + ] + + def _eval(self, geometric_altitude, delta_temp=0): + geopot_altitude = self.geopotential_altitude(geometric_altitude) + band_data = self.get_atmosphere_band(geopot_altitude) + + dh = geopot_altitude - band_data.start_alt + lapse_rate = band_data.lapse_rate + + temp = 0 + pressure = 0 + density = 0 + speed_of_sound = 0 + + if lapse_rate != 0.0: + temp = band_data.base_temperature + lapse_rate * dh + pressure = band_data.base_pressure * math.pow(temp/band_data.base_temperature, -self.g0/(lapse_rate * self.R)) + else: + temp = band_data.base_temperature + pressure = band_data.base_pressure * math.exp((-self.g0 * dh)/(self.R * temp)) + + density = pressure/(self.R * (temp + delta_temp)) + speed_of_sound = math.sqrt(self.gamma * self.R * (temp + delta_temp)) + + return (pressure, density, temp + delta_temp, speed_of_sound) + + def geopotential_altitude(self, geometric_altitude): + return (geometric_altitude * self.r0)/(self.r0 + geometric_altitude) + + def geometric_altitude(self, geopotential_altitude): + return (self.r0 * geopotential_altitude)/(self.r0 - geopotential_altitude) + + def get_atmosphere_band(self, geopot_altitude): + for band in self.atmosphere_bands: + if geopot_altitude >= band.start_alt and geopot_altitude <= band.end_alt: + return band + raise IndexError('Altitude out of range') + + diff --git a/src/pathsim_flight/jsbsim/__init__.py b/src/pathsim_flight/jsbsim/__init__.py new file mode 100644 index 0000000..2169b8a --- /dev/null +++ b/src/pathsim_flight/jsbsim/__init__.py @@ -0,0 +1 @@ +from .jsbsim_wrapper import * \ No newline at end of file diff --git a/src/pathsim_flight/jsbsim/jsbsim_wrapper.py b/src/pathsim_flight/jsbsim/jsbsim_wrapper.py new file mode 100644 index 0000000..7890be0 --- /dev/null +++ b/src/pathsim_flight/jsbsim/jsbsim_wrapper.py @@ -0,0 +1,106 @@ +######################################################################################### +## +## JSBSim Wrapper Block +## +######################################################################################### + +# IMPORTS =============================================================================== + +from pathsim.blocks import Wrapper +import jsbsim + +# BLOCKS ================================================================================ + +class JSBSimWrapper(Wrapper): + """A wrapper for the JSBSim flight dynamics model, which allows it to be + used as a block in PathSim. + + Parameters + ---------- + T : float + The time step for the JSBSim model, in seconds. Default is 1/120, which corresponds + to 120 Hz. + input_properties : list of str + A list of JSBSim property names that correspond to the input ports of the block. + output_properties : list of str + A list of JSBSim property names that correspond to the output ports of the block. + JSBSim_path : str + The file path to the JSBSim installation. If None, it will look for JSBSim files + in the pip install location. + aircraft_model : str + The name of the aircraft model to load in JSBSim. Default is '737'. + trim_airspeed : float + The airspeed to use for trimming the aircraft, as KCAS. Default is 200 KCAS. + trim_altitude : float + The altitude ASL to use for trimming the aircraft, in feet. Default is 1000 ft ASL. + trim_gamma : float + The flight path angle to use for trimming the aircraft, in degrees. Default is + 0 degrees (level flight). + """ + + input_port_labels = None + + output_port_labels = None + + # TODO: Probably need to add some additional parameters, e.g. ground trim versus + # full air trim versus no trim, gear position, flap position. + + def __init__(self, T=1/120, input_properties=None, output_properties=None, JSBSim_path=None, + aircraft_model='737', trim_airspeed=200, trim_altitude=1000, trim_gamma=0): + super().__init__(func=self._func, T=T) + + self.input_properties = input_properties if input_properties is not None else [] + self.output_properties = output_properties if output_properties is not None else [] + + # Init JSBSim, load aircraft, trim with initial conditions + self.init_fdm(JSBSim_path, aircraft_model, T) + self.trim(trim_airspeed, trim_altitude, trim_gamma) + + def init_fdm(self, JSBSim_path, aircraft_model, T): + # Avoid flooding the console with log messages + jsbsim.FGJSBBase().debug_lvl = 0 + + # Create a flight dynamics model (FDM) instance. + # None for JSBSim_path means it will look for JSBSim files in pip install location. + self.fdm = jsbsim.FGFDMExec(JSBSim_path) + + # Load the aircraft model + self.fdm.load_model(aircraft_model) + + # Set the time step + self.fdm.set_dt(T) + + def trim(self, trim_airspeed, trim_alitude, trim_gamma): + # Set engines running + self.fdm['propulsion/set-running'] = -1 + + # Set initial conditions for trim + self.fdm['ic/h-sl-ft'] = trim_alitude + self.fdm['ic/vc-kts'] = trim_airspeed + self.fdm['ic/gamma-deg'] = trim_gamma + self.fdm.run_ic() + + # Calculate trim solution + self.fdm['simulation/do_simple_trim'] = 1 + + def _func(self, *u): + # PathSim's Wrapper base class passes the input vector as separate arguments, + # so we need to collect them into a list + + # Confirm that the input vector u has the expected length + if len(u) != len(self.input_properties): + raise ValueError(f"Expected {len(self.input_properties)} inputs, but got {len(u)}") + + # Pass input vector u to JSBSim by setting the corresponding properties + for i in range(len(u)): + self.fdm[self.input_properties[i]] = u[i] + + # Run the JSBSim model for one time step + self.fdm.run() + + # Extract output properties from JSBSim and return as vector y + y = [] + for i in range(len(self.output_properties)): + y.append(self.fdm[self.output_properties[i]]) + + return y diff --git a/src/pathsim_flight/utils/__init__.py b/src/pathsim_flight/utils/__init__.py new file mode 100644 index 0000000..788356d --- /dev/null +++ b/src/pathsim_flight/utils/__init__.py @@ -0,0 +1 @@ +from .airspeed_conversions import * diff --git a/src/pathsim_flight/utils/airspeed_conversions.py b/src/pathsim_flight/utils/airspeed_conversions.py new file mode 100644 index 0000000..70d70ca --- /dev/null +++ b/src/pathsim_flight/utils/airspeed_conversions.py @@ -0,0 +1,205 @@ +######################################################################################### +## +## Airspeed conversion Blocks +## +######################################################################################### + +# IMPORTS =============================================================================== + +from pathsim.blocks import Function +from ..atmosphere import ISAtmosphere +import math + +# BLOCKS ================================================================================ + +class CAStoMach(Function): + """Convert calibrated airspeed (CAS) to Mach value. + CAS in m/s altitude in m. + """ + + input_port_labels = { + "cas": 0, + "altitude": 1 + } + + output_port_labels = { + "mach": 0 + } + + def __init__(self): + super().__init__(func=self._eval) + + def _eval(self, cas, altitude): + """Convert Calibrated airspeed to Mach value. + + Assume m/s for cas and m for altitude. + + Based on the formulas in the US Air Force Aircraft Performance Flight + Testing Manual (AFFTC-TIH-99-01), in particular sections 4.6 to 4.8. + + The subsonic and supersonic Mach number equations are used with the simple + substitutions of (Vc/asl) for M and Psl for P. However, the condition for + which the equations are used is no longer subsonic (M < 1) or supersonic + (M > 1) but rather calibrated airspeed being less or greater than the + speed of sound ( asl ), standard day, sea level (661.48 knots). + """ + ISA = ISAtmosphere() + + pressure, _, _, _ = ISA._eval(altitude) + + if cas < ISAtmosphere.StdSL_speed_of_sound: + # Bernoulli's compressible equation (4.11) + qc = ISAtmosphere.StdSL_pressure * ( + math.pow(1 + 0.2 * math.pow(cas / ISAtmosphere.StdSL_speed_of_sound, 2), 3.5) - 1 + ) + else: + # Rayleigh's supersonic pitot equation (4.16) + qc = ISAtmosphere.StdSL_pressure * ( + ( + (166.9215801 * math.pow(cas / ISAtmosphere.StdSL_speed_of_sound, 7)) + / math.pow(7 * math.pow(cas / ISAtmosphere.StdSL_speed_of_sound, 2) - 1, 2.5) + ) + - 1 + ) + + # Solving for M in equation (4.11), also used as initial condition for supersonic case + mach = math.sqrt(5 * (math.pow(qc / pressure + 1, 2 / 7) - 1)) + + if mach > 1: + # Iterate equation (4.22) since M appears on both sides of the equation + for i in range(7): + mach = 0.88128485 * math.sqrt((qc / pressure + 1) * math.pow(1 - 1 / (7 * mach * mach), 2.5)) + + return mach + + +class CAStoTAS(Function): + """Convert calibrated airspeed (CAS) to true airspeed (TAS). + CAS and TAS in m/s altitude in m. + """ + + input_port_labels = { + "cas": 0, + "altitude": 1 + } + + output_port_labels = { + "tas": 0 + } + + def __init__(self): + super().__init__(func=self._eval) + + def _eval(self, cas, altitude): + """Assume m/s for input and output velocities and m for altitude.""" + + mach = CAStoMach()._eval(cas, altitude) + ISA = ISAtmosphere() + _, _, _, speed_of_sound = ISA.state(altitude) + return mach * speed_of_sound + + +class TAStoCAS(Function): + """Convert true airspeed (TAS) to calibrated airspeed (CAS). + TAS and CAS in m/s altitude in m. + """ + + input_port_labels = { + "tas": 0, + "altitude": 1 + } + + output_port_labels = { + "cas": 0 + } + + def __init__(self): + super().__init__(func=self._eval) + + def _eval(self, tas, altitude): + """Assume m/s for input and output velocities and m for altitude.""" + + ISA = ISAtmosphere() + pressure, _, _, speed_of_sound = ISA.state(altitude) + + mach = tas / speed_of_sound + qc = pressure * ( math.pow(1 + 0.2*mach**2, 7/2) - 1) + cas = ISA.StdSL_speed_of_sound * math.sqrt( 5 * ( math.pow(qc/ISA.StdSL_pressure + 1, 2/7) - 1) ) + return cas + + +class CAStoEAS(Function): + """Convert calibrated airspeed (CAS) to equivalent airspeed (EAS). + CAS and EAS in m/s altitude in m. + """ + + input_port_labels = { + "cas": 0, + "altitude": 1 + } + + output_port_labels = { + "eas": 0 + } + + def __init__(self): + super().__init__(func=self._eval) + + def _eval(self, cas, altitude): + """Assume m/s for input and output velocities and m for altitude.""" + ISA = ISAtmosphere() + _, density, _, _ = ISA.state(altitude) + _, rho0, _, _ = ISA.state(0) # Standard sea level density + eas = CAStoTAS()._eval(cas, altitude) * math.sqrt(density / rho0) + return eas + + +class EAStoTAS(Function): + """Convert equivalent airspeed (EAS) to true airspeed (TAS). + EAS and TAS in m/s altitude in m. + """ + + input_port_labels = { + "eas": 0, + "altitude": 1 + } + + output_port_labels = { + "tas": 0 + } + + def __init__(self): + super().__init__(func=self._eval) + + def _eval(self, eas, altitude): + """Assume m/s for input and output velocities and m for altitude.""" + ISA = ISAtmosphere() + _, density, _, _ = ISA.state(altitude) + _, rho0, _, _ = ISA.state(0) # Standard sea level density + tas = eas * math.sqrt(rho0 / density) + return tas + + +class MachtoCAS(Function): + """Convert Mach value to calibrated airspeed (CAS). + CAS in m/s altitude in m. + """ + + input_port_labels = { + "mach": 0, + "altitude": 1 + } + + output_port_labels = { + "cas": 0 + } + + def __init__(self): + super().__init__(func=self._eval) + + def _eval(mach, altitude): + """Assume m for altitude.""" + ISA = ISAtmosphere() + _, _, _, speed_of_sound = ISA.state(altitude) + return TAStoCAS()._eval(mach * speed_of_sound) +