Initial JSBSimWrapper, ISA and airspeed blocks

.gitignore

@@ -71,6 +71,7 @@ ENV/
 # IDEs
 .vscode/
 .idea/
+.vs/
 
 # OS generated files
 .DS_Store

pyproject.toml

@@ -29,6 +29,7 @@ dependencies = [
     "pathsim",
     "numpy>=1.15",
     "scipy>=1.2",
+    "jsbsim>=1.2.4"
 ]
 
 [project.optional-dependencies]

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 *

src/pathsim_flight/atmosphere/__init__.py

@@ -0,0 +1 @@
+from .international_standard_atmosphere import *

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')
+
+

src/pathsim_flight/jsbsim/__init__.py

@@ -0,0 +1 @@
+from .jsbsim_wrapper import *

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

src/pathsim_flight/utils/__init__.py

@@ -0,0 +1 @@
+from .airspeed_conversions import *