/// <summary>
/// Runs the Aircraft model; called by the Executive
/// </summary>
/// <returns>false if no error</returns>
public override bool Run(bool Holding)
{
#if TODO
if (InternalRun())
{
return(true);
}
if (FDMExec.Holding())
{
return(false); // if paused don't execute
}
// if false then execute this Run()
vForces = Vector3D.Zero;
vForces = FDMExec.Aerodynamics.Forces;
vForces += FDMExec.Propulsion.GetForces();
vForces += FDMExec.GroundReactions.GetForces();
vMoments = Vector3D.Zero;
vMoments = FDMExec.Aerodynamics.Moments;
vMoments += FDMExec.Propulsion.GetMoments();
vMoments += FDMExec.GroundReactions.GetMoments();
vBodyAccel = vForces / FDMExec.MassBalance.Mass;
vNcg = vBodyAccel / FDMExec.Inertial.Gravity;
vNwcg = FDMExec.State.GetTb2s() * vNcg;
vNwcg.Z = -1 * vNwcg.Z + 1;
return(false);
#endif
throw new NotImplementedException("Pending upgrade to lastest version of JSBSIM");
}
/// <summary>
/// This set of calculations results in the body and inertial frame accelerations
/// being computed.
/// Compute body and inertial frames accelerations based on the current body
/// forces including centripetal and Coriolis accelerations for the former.
/// in.vOmegaPlanet is the Earth angular rate - expressed in the inertial frame -
/// so it has to be transformed to the body frame. More completely,
/// in.vOmegaPlanet is the rate of the ECEF frame relative to the Inertial
/// frame (ECI), expressed in the Inertial frame.
/// in.Force is the total force on the vehicle in the body frame.
/// in.vPQR is the vehicle body rate relative to the ECEF frame, expressed
/// in the body frame.
/// in.vUVW is the vehicle velocity relative to the ECEF frame, expressed
/// in the body frame.
/// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
/// Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
/// </summary>
private void CalculateUVWdot()
{
if (FDMExec.GetHoldDown() && !FDMExec.GetTrimStatus())
{
vBodyAccel = Vector3D.Zero;
}
else
{
vBodyAccel = inputs.Force / inputs.Mass;
}
vUVWdot = vBodyAccel - (inputs.vPQR + 2.0 * (inputs.Ti2b * inputs.vOmegaPlanet)) * inputs.vUVW;
// Include Centripetal acceleration.
vUVWdot -= inputs.Ti2b * (inputs.vOmegaPlanet * (inputs.vOmegaPlanet * inputs.vInertialPosition));
if (FDMExec.GetHoldDown())
{
// The acceleration in ECI is calculated so that the acceleration is zero
// in the body frame.
vUVWidot = inputs.vOmegaPlanet * (inputs.vOmegaPlanet * inputs.vInertialPosition);
vUVWdot = Vector3D.Zero;
}
else
{
vUVWdot += inputs.Tec2b * inputs.vGravAccel;
vUVWidot = inputs.Tb2i * vBodyAccel + inputs.Tec2i * inputs.vGravAccel;
}
}
/// <summary>
// Compute body frame rotational accelerations based on the current body moments
///
/// vPQRdot is the derivative of the absolute angular velocity of the vehicle
/// (body rate with respect to the ECEF frame), expressed in the body frame,
/// where the derivative is taken in the body frame.
/// J is the inertia matrix
/// Jinv is the inverse inertia matrix
/// vMoments is the moment vector in the body frame
/// in.vPQRi is the total inertial angular velocity of the vehicle
/// expressed in the body frame.
/// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
/// Second edition (2004), eqn 1.5-16e (page 50)
/// </summary>
private void CalculatePQRdot()
{
if (gravTorque)
{
// Compute the gravitational torque
// Reference: See Harris and Lyle "Spacecraft Gravitational Torques",
// NASA SP-8024 (1969) eqn (2) (page 7)
Vector3D R = inputs.Ti2b * inputs.vInertialPosition;
double invRadius = 1.0 / R.Magnitude();
R *= invRadius;
inputs.Moment += (3.0 * inputs.vGravAccel.Magnitude() * invRadius) * (R * (inputs.J * R));
}
// Compute body frame rotational accelerations based on the current body
// moments and the total inertial angular velocity expressed in the body
// frame.
// if (HoldDown && !FDMExec->GetTrimStatus()) {
if (FDMExec.GetHoldDown())
{
// The rotational acceleration in ECI is calculated so that the rotational
// acceleration is zero in the body frame.
vPQRdot = Vector3D.Zero;
vPQRidot = inputs.vPQRi * (inputs.Ti2b * inputs.vOmegaPlanet);
}
else
{
vPQRidot = inputs.Jinv * (inputs.Moment - inputs.vPQRi * (inputs.J * inputs.vPQRi));
vPQRdot = vPQRidot - inputs.vPQRi * (inputs.Ti2b * inputs.vOmegaPlanet);
}
}
public override bool Run(bool Holding)
{
if (InternalRun())
{
return(true);
}
if (FDMExec.Holding())
{
return(false); // if paused don't execute
}
vForces = Vector3D.Zero;
vMoments = Vector3D.Zero;
// Only execute gear force code below 300 feet
if (FDMExec.Propagate.DistanceAGL < 300.0)
{
// Sum forces and moments for all gear, here.
// Some optimizations may be made here - or rather in the gear code itself.
// The gear ::Run() method is called several times - once for each gear.
// Perhaps there is some commonality for things which only need to be
// calculated once.
foreach (LGear gear in lGear)
{
vForces += gear.Force();
vMoments += gear.Moment();
}
}
else
{
// Crash Routine
}
return(false);
}
/// <summary>
/// Runs the Aircraft model; called by the Executive
/// </summary>
/// <returns>false if no error</returns>
public override bool Run()
{
if (InternalRun())
{
return(true);
}
if (FDMExec.Holding())
{
return(false); // if paused don't execute
}
// if false then execute this Run()
vForces = Vector3D.Zero;
vForces = FDMExec.Aerodynamics.Forces;
vForces += FDMExec.Propulsion.GetForces();
vForces += FDMExec.GroundReactions.GetForces();
vMoments = Vector3D.Zero;
vMoments = FDMExec.Aerodynamics.Moments;
vMoments += FDMExec.Propulsion.GetMoments();
vMoments += FDMExec.GroundReactions.GetMoments();
vBodyAccel = vForces / FDMExec.MassBalance.Mass;
vNcg = vBodyAccel / FDMExec.Inertial.Gravity;
vNwcg = FDMExec.State.GetTb2s() * vNcg;
vNwcg.Z = -1 * vNwcg.Z + 1;
return(false);
}
public override bool Run()
{
double denom, k1, k2, k3, k4, k5, k6;
double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
if (InternalRun())
{
return(true);
}
if (FDMExec.Holding())
{
return(false); // if paused don't execute
}
weight = emptyWeight + FDMExec.Propulsion.GetTanksWeight() + GetPointMassWeight();
mass = Constants.lbtoslug * weight;
// Calculate new CG
vXYZcg = (FDMExec.Propulsion.GetTanksMoment() + emptyWeight * vbaseXYZcg
+ GetPointMassMoment()) / weight;
// Calculate new total moments of inertia
// At first it is the base configuration inertia matrix ...
mJ = baseJ;
// ... with the additional term originating from the parallel axis theorem.
mJ += GetPointmassInertia(Constants.lbtoslug * emptyWeight, vbaseXYZcg);
// Then add the contributions from the additional pointmasses.
mJ += CalculatePMInertias();
mJ += FDMExec.Propulsion.CalculateTankInertias();
Ixx = mJ.M11;
Iyy = mJ.M22;
Izz = mJ.M33;
Ixy = -mJ.M12;
Ixz = -mJ.M13;
Iyz = -mJ.M23;
// Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
k1 = (Iyy * Izz - Iyz * Iyz);
k2 = (Iyz * Ixz + Ixy * Izz);
k3 = (Ixy * Iyz + Iyy * Ixz);
denom = 1.0 / (Ixx * k1 - Ixy * k2 - Ixz * k3);
k1 = k1 * denom;
k2 = k2 * denom;
k3 = k3 * denom;
k4 = (Izz * Ixx - Ixz * Ixz) * denom;
k5 = (Ixy * Ixz + Iyz * Ixx) * denom;
k6 = (Ixx * Iyy - Ixy * Ixy) * denom;
mJinv = new Matrix3D(k1, k2, k3, k2, k4, k5, k3, k5, k6);
return(false);
}
public override bool Run()
{
// Fast return if we have nothing to do ...
if (InternalRun())
{
return(true);
}
if (FDMExec.Holding())
{
return(false); // if paused don't execute
}
// Gravitation accel
double r = FDMExec.Propagate.Radius;
gAccel = GetGAccel(r);
return(false);
}
/// <summary>
/// Runs the Atmosphere model; called by the Executive
/// </summary>
/// <returns>false if no error</returns>
public override bool Run()
{
if (InternalRun())
{
return(true);
}
if (FDMExec.Holding())
{
return(false);
}
T_dev = 0.0;
h = FDMExec.Propagate.Altitude;
if (!useExternal)
{
Calculate(h);
CalculateDerived();
}
else
{
CalculateDerived();
}
// if false then execute this Run()
//do temp, pressure, and density first
if (!useExternal)
{
h = FDMExec.Propagate.Altitude;
Calculate(h);
}
if (turbType != TurbType.ttNone)
{
Turbulence();
vWindNED += vTurbulence;
}
return(false);
}
public override bool Run(bool Holding)
{
//if (log.IsInfoEnabled)
// log.Info("Entering Run() for model " + name + "rate =" + rate);
if (InternalRun())
{
return(true);
}
if (enabled && !FDMExec.State.IsIntegrationSuspended && !FDMExec.Holding())
{
if (outputType == OutputType.Socket)
{
// Not done yet
//if (log.IsWarnEnabled)
// log.Warn("Socket output is Not Implemented");
}
else if (outputType == OutputType.CSV || outputType == OutputType.Tab)
{
DelimitedOutput(filename); //TODO
}
else if (outputType == OutputType.Terminal)
{
// Not done yet
throw new NotImplementedException("Terminal output");
}
else if (outputType == OutputType.Log4Net)
{
Log4NetOutput();
}
else if (outputType == OutputType.None)
{
// Do nothing
}
else
{
// Not a valid type of output
}
}
return(false);
}
public override bool Run()
{
if (InternalRun())
{
return(true);
}
if (FDMExec.Holding())
{
return(false);
}
T_dev = 0.0;
// if false then execute this Run()
//do temp, pressure, and density first
if (!useExternal)
{
h = FDMExec.Propagate.Altitude;
Calculate(h);
}
if (turbType != TurbType.ttNone)
{
Turbulence();
vWindNED += vTurbulence;
}
if (vWindNED[0] != 0.0)
{
psiw = Math.Atan2(vWindNED[1], vWindNED[0]);
}
if (psiw < 0)
{
psiw += 2 * Math.PI;
}
soundspeed = Math.Sqrt(Constants.SHRatio * Constants.Reng * (useInfo.Temperature));
return(false);
}
/// <summary>
/// Runs the state propagation model; called by the Executive
/// Can pass in a value indicating if the executive is directing the simulation to Hold.
/// </summary>
/// <param name="Holding">if true, the executive has been directed to hold the sim from
/// advancing time.Some models may ignore this flag, such as the Input
/// model, which may need to be active to listen on a socket for the
/// "Resume" command to be given.</param>
/// <returns>false if no error</returns>
public override bool Run(bool Holding)
{
if (base.Run(Holding))
{
return(true); // Fast return if we have nothing to do ...
}
if (Holding)
{
return(false);
}
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
if (!FDMExec.GetHoldDown())
{
CalculateFrictionForces(inputs.DeltaT * rate); // Update rate derivatives with friction forces
}
Debug(2);
return(false);
}
/// <summary>
/// Executes the propulsion model.
/// The initial plan for the FGPropulsion class calls for Run() to be executed,
/// calculating the power available from the engine.
///
/// [Note: Should we be checking the Starved flag here?]
/// </summary>
/// <returns></returns>
public override bool Run(bool Holding)
{
if (InternalRun())
{
return(true);
}
if (FDMExec.Holding())
{
return(false); // if paused don't execute
}
double dt = FDMExec.State.DeltaTime;
vForces = Vector3D.Zero;
vMoments = Vector3D.Zero;
for (int i = 0; i < engines.Count; i++)
{
engines[i].Calculate();
vForces += engines[i].GetBodyForces(); // sum body frame forces
vMoments += engines[i].GetMoments(); // sum body frame moments
}
totalFuelQuantity = 0.0;
for (int i = 0; i < tanks.Count; i++)
{
tanks[i].Calculate(dt * rate);
if (tanks[i].TankType == Tank.EnumTankType.Fuel)
{
totalFuelQuantity += tanks[i].Contents;
}
}
if (refuel)
{
DoRefuel(dt * rate);
}
return(false);
}
public bool Load(XmlElement root)
{
if (log.IsDebugEnabled)
{
log.Debug("Reading and running from script file " + scriptName);
}
string name = root.GetAttribute("name");
if (name.Length != 0)
{
this.scriptName = name;
}
XmlNodeList childNodes = root.GetElementsByTagName("use");
if (childNodes.Count != 2 && log.IsErrorEnabled)
{
if (log.IsErrorEnabled)
{
log.Error("Two <use> tags must be specified in script file.");
}
return(false);
}
string aircraft = (childNodes[0] as XmlElement).GetAttribute("aircraft");
string initialize = (childNodes[1] as XmlElement).GetAttribute("initialize");
if (aircraft.Length != 0)
{
try
{
FDMExec.LoadModel(aircraft);
}
catch (Exception e)
{
if (log.IsErrorEnabled)
{
log.Error("Exception reading script file: " + e);
}
return(false);
}
}
else
{
if (log.IsErrorEnabled)
{
log.Error("Aircraft must be specified first in script file.");
}
return(false);
}
childNodes = root.GetElementsByTagName("run");
if (childNodes.Count != 1)
{
if (log.IsErrorEnabled)
{
log.Error("One \"<run>\" tag must be specified in script file");
}
return(false);
}
// Set sim timing
XmlElement run_element = childNodes[0] as XmlElement;
startTime = double.Parse(run_element.GetAttribute("start"), FormatHelper.numberFormatInfo);
FDMExec.State.SimTime = startTime;
endTime = double.Parse(run_element.GetAttribute("end"), FormatHelper.numberFormatInfo);
FDMExec.State.DeltaTime = double.Parse(run_element.GetAttribute("dt"), FormatHelper.numberFormatInfo);
foreach (XmlNode currentNode in root.GetElementsByTagName("when"))
{
ReadWhenClause(currentNode as XmlElement);
}
try
{
FDMExec.GetIC().Load(initialize, true);
}
catch (Exception e)
{
if (log.IsErrorEnabled)
{
log.Error("Initialization unsuccessful. Exception " + e);
}
}
return(true);
}
/// <summary>
/// Runs the Propagate model; called by the Executive
/// </summary>
/// <returns>false if no error</returns>
public override bool Run()
{
if (InternalRun())
{
return(true);
}
if (FDMExec.Holding())
{
return(false); // if paused don't execute
}
RecomputeRunwayRadius();
double dt = FDMExec.State.DeltaTime * rate; // The 'stepsize'
Vector3D omega = new Vector3D(0.0, 0.0, FDMExec.Inertial.Omega); // earth rotation
Vector3D vForces = FDMExec.Aircraft.Forces; // current forces
Vector3D vMoments = FDMExec.Aircraft.Moments; // current moments
double mass = FDMExec.MassBalance.Mass; // mass
Matrix3D J = FDMExec.MassBalance.GetJ(); // inertia matrix
Matrix3D Jinv = FDMExec.MassBalance.GetJinv(); // inertia matrix inverse
double r = this.Radius; // radius
if (r == 0.0)
{
if (log.IsErrorEnabled)
{
log.Error("radius = 0 !");
}
r = 1e-16;
} // radius check
double rInv = 1.0 / r;
Vector3D gAccel = new Vector3D(0.0, 0.0, FDMExec.Inertial.GetGAccel(r));
// The rotation matrices:
Matrix3D Tl2b = GetTl2b(); // local to body frame
Matrix3D Tb2l = GetTb2l(); // body to local frame
Matrix3D Tec2l = VState.vLocation.GetTec2l(); // earth centered to local frame
Matrix3D Tl2ec = VState.vLocation.GetTl2ec(); // local to earth centered frame
// Inertial angular velocity measured in the body frame.
Vector3D pqri = VState.vPQR + Tl2b * (Tec2l * omega);
// Compute vehicle velocity wrt EC frame, expressed in Local horizontal frame.
vVel = Tb2l * VState.vUVW;
// First compute the time derivatives of the vehicle state values:
// Compute body frame rotational accelerations based on the current body moments
vPQRdot = Jinv * (vMoments - Vector3D.Cross(pqri, (J * pqri)));
// Compute body frame accelerations based on the current body forces
vUVWdot = Vector3D.Cross(VState.vUVW, VState.vPQR) + vForces / mass;
// Coriolis acceleration.
Vector3D ecVel = Tl2ec * vVel;
Vector3D ace = 2.0 * Vector3D.Cross(omega, ecVel);
vUVWdot -= Tl2b * (Tec2l * ace);
// Centrifugal acceleration.
if (!FDMExec.GroundReactions.GetWOW())
{
Vector3D aeec = Vector3D.Cross(omega, Vector3D.Cross(omega, (Vector3D)VState.vLocation));
vUVWdot -= Tl2b * (Tec2l * aeec);
}
// Gravitation accel
vUVWdot += Tl2b * gAccel;
// Compute vehicle velocity wrt EC frame, expressed in EC frame
Vector3D vLocationDot = Tl2ec * vVel;
Vector3D omegaLocal = new Vector3D(rInv * vVel.East,
-rInv * vVel.North,
-rInv * vVel.East * VState.vLocation.TanLatitude);
// Compute quaternion orientation derivative on current body rates
Quaternion vQtrndot = VState.vQtrn.GetQDot(VState.vPQR - Tl2b * omegaLocal);
// Propagate velocities
VState.vPQR += dt * vPQRdot;
VState.vUVW += dt * vUVWdot;
// Propagate positions
VState.vQtrn += dt * vQtrndot;
VState.vLocation += (Location)(dt * vLocationDot);
return(false);
}
private void Turbulence(double h)
{
switch (turbType)
{
case tType.ttCulp:
vTurbPQR.P = wind_from_clockwise;
if (TurbGain == 0.0)
{
return;
}
// keep the inputs within allowable limts for this model
if (TurbGain < 0.0)
{
TurbGain = 0.0;
}
if (TurbGain > 1.0)
{
TurbGain = 1.0;
}
if (TurbRate < 0.0)
{
TurbRate = 0.0;
}
if (TurbRate > 30.0)
{
TurbRate = 30.0;
}
if (Rhythmicity < 0.0)
{
Rhythmicity = 0.0;
}
if (Rhythmicity > 1.0)
{
Rhythmicity = 1.0;
}
// generate a sine wave corresponding to turbulence rate in hertz
double time = FDMExec.GetSimTime();
double sinewave = Math.Sin(time * TurbRate * 6.283185307);
double random = 0.0;
if (target_time == 0.0)
{
strength = random = 1 - 2.0 * uniformNumber.Next();
target_time = time + 0.71 + (random * 0.5);
}
if (time > target_time)
{
spike = 1.0;
target_time = 0.0;
}
// max vertical wind speed in fps, corresponds to TurbGain = 1.0
double max_vs = 40;
vTurbulenceNED = Vector3D.Zero;
double delta = strength * max_vs * TurbGain * (1 - Rhythmicity) * spike;
// Vertical component of turbulence.
vTurbulenceNED.Down = sinewave * max_vs * TurbGain * Rhythmicity;
vTurbulenceNED.Down += delta;
if (inputs.DistanceAGL / inputs.wingspan < 3.0)
{
vTurbulenceNED.Down *= inputs.DistanceAGL / inputs.wingspan * 0.3333;
}
// Yaw component of turbulence.
vTurbulenceNED.North = Math.Sin(delta * 3.0);
vTurbulenceNED.East = Math.Cos(delta * 3.0);
// Roll component of turbulence. Clockwise vortex causes left roll.
vTurbPQR.P += delta * 0.04;
spike = spike * 0.9;
break;
case tType.ttMilspec:
case tType.ttTustin:
// an index of zero means turbulence is disabled
// airspeed occurs as divisor in the code below
if (probability_of_exceedence_index == 0 || inputs.V == 0)
{
vTurbulenceNED.North = vTurbulenceNED.East = vTurbulenceNED.Down = 0.0;
vTurbPQR.P = vTurbPQR.Q = vTurbPQR.R = 0.0;
return;
}
// Turbulence model according to MIL-F-8785C (Flying Qualities of Piloted Aircraft)
double b_w = inputs.wingspan, L_u, L_w, sig_u, sig_w;
if (b_w == 0.0)
{
b_w = 30.0;
}
// clip height functions at 10 ft
if (h <= 10.0)
{
h = 10;
}
// Scale lengths L and amplitudes sigma as function of height
if (h <= 1000)
{
L_u = h / Math.Pow(0.177 + 0.000823 * h, 1.2); // MIL-F-8785c, Fig. 10, p. 55
L_w = h;
sig_w = 0.1 * windspeed_at_20ft;
sig_u = sig_w / Math.Pow(0.177 + 0.000823 * h, 0.4); // MIL-F-8785c, Fig. 11, p. 56
}
else if (h <= 2000)
{
// linear interpolation between low altitude and high altitude models
L_u = L_w = 1000 + (h - 1000.0) / 1000.0 * 750.0;
sig_u = sig_w = 0.1 * windspeed_at_20ft
+ (h - 1000.0) / 1000.0 * (POE_Table.GetValue(probability_of_exceedence_index, h) - 0.1 * windspeed_at_20ft);
}
else
{
L_u = L_w = 1750.0; // MIL-F-8785c, Sec. 3.7.2.1, p. 48
sig_u = sig_w = POE_Table.GetValue(probability_of_exceedence_index, h);
}
// keep values from last timesteps
// TODO maybe use deque?
double
xi_u_km1 = 0, nu_u_km1 = 0,
xi_v_km1 = 0, xi_v_km2 = 0, nu_v_km1 = 0, nu_v_km2 = 0,
xi_w_km1 = 0, xi_w_km2 = 0, nu_w_km1 = 0, nu_w_km2 = 0,
xi_p_km1 = 0, nu_p_km1 = 0,
xi_q_km1 = 0, xi_r_km1 = 0;
double T_V = inputs.totalDeltaT, // for compatibility of nomenclature
sig_p = 1.9 / Math.Sqrt(L_w * b_w) * sig_w, // Yeager1998, eq. (8)
//sig_q = Math.Sqrt(M_PI/2/L_w/b_w), // eq. (14)
//sig_r = Math.Sqrt(2*M_PI/3/L_w/b_w), // eq. (17)
L_p = Math.Sqrt(L_w * b_w) / 2.6, // eq. (10)
tau_u = L_u / inputs.V, // eq. (6)
tau_w = L_w / inputs.V, // eq. (3)
tau_p = L_p / inputs.V, // eq. (9)
tau_q = 4 * b_w / Math.PI / inputs.V, // eq. (13)
tau_r = 3 * b_w / Math.PI / inputs.V, // eq. (17)
nu_u = gaussianNumber.Next(),
nu_v = gaussianNumber.Next(),
nu_w = gaussianNumber.Next(),
nu_p = gaussianNumber.Next(),
xi_u = 0, xi_v = 0, xi_w = 0, xi_p = 0, xi_q = 0, xi_r = 0;
// values of turbulence NED velocities
if (turbType == tType.ttTustin)
{
// the following is the Tustin formulation of Yeager's report
double omega_w = inputs.V / L_w, // hidden in nomenclature p. 3
omega_v = inputs.V / L_u, // this is defined nowhere
C_BL = 1 / tau_u / Math.Tan(T_V / 2 / tau_u), // eq. (19)
C_BLp = 1 / tau_p / Math.Tan(T_V / 2 / tau_p), // eq. (22)
C_BLq = 1 / tau_q / Math.Tan(T_V / 2 / tau_q), // eq. (24)
C_BLr = 1 / tau_r / Math.Tan(T_V / 2 / tau_r); // eq. (26)
// all values calculated so far are strictly positive, except for
// the random numbers nu_*. This means that in the code below, all
// divisors are strictly positive, too, and no floating point
// exception should occur.
xi_u = -(1 - C_BL * tau_u) / (1 + C_BL * tau_u) * xi_u_km1
+ sig_u * Math.Sqrt(2 * tau_u / T_V) / (1 + C_BL * tau_u) * (nu_u + nu_u_km1); // eq. (18)
xi_v = -2 * (sqr(omega_v) - sqr(C_BL)) / sqr(omega_v + C_BL) * xi_v_km1
- sqr(omega_v - C_BL) / sqr(omega_v + C_BL) * xi_v_km2
+ sig_u * Math.Sqrt(3 * omega_v / T_V) / sqr(omega_v + C_BL) * (
(C_BL + omega_v / Math.Sqrt(3.0)) * nu_v
+ 2 / Math.Sqrt(3.0) * omega_v * nu_v_km1
+ (omega_v / Math.Sqrt(3.0) - C_BL) * nu_v_km2); // eq. (20) for v
xi_w = -2 * (sqr(omega_w) - sqr(C_BL)) / sqr(omega_w + C_BL) * xi_w_km1
- sqr(omega_w - C_BL) / sqr(omega_w + C_BL) * xi_w_km2
+ sig_w * Math.Sqrt(3 * omega_w / T_V) / sqr(omega_w + C_BL) * (
(C_BL + omega_w / Math.Sqrt(3.0)) * nu_w
+ 2 / Math.Sqrt(3.0) * omega_w * nu_w_km1
+ (omega_w / Math.Sqrt(3.0) - C_BL) * nu_w_km2); // eq. (20) for w
xi_p = -(1 - C_BLp * tau_p) / (1 + C_BLp * tau_p) * xi_p_km1
+ sig_p * Math.Sqrt(2 * tau_p / T_V) / (1 + C_BLp * tau_p) * (nu_p + nu_p_km1); // eq. (21)
xi_q = -(1 - 4 * b_w * C_BLq / Math.PI / inputs.V) / (1 + 4 * b_w * C_BLq / Math.PI / inputs.V) * xi_q_km1
+ C_BLq / inputs.V / (1 + 4 * b_w * C_BLq / Math.PI / inputs.V) * (xi_w - xi_w_km1); // eq. (23)
xi_r = -(1 - 3 * b_w * C_BLr / Math.PI / inputs.V) / (1 + 3 * b_w * C_BLr / Math.PI / inputs.V) * xi_r_km1
+ C_BLr / inputs.V / (1 + 3 * b_w * C_BLr / Math.PI / inputs.V) * (xi_v - xi_v_km1); // eq. (25)
}
else if (turbType == tType.ttMilspec)
{
// the following is the MIL-STD-1797A formulation
// as cited in Yeager's report
xi_u = (1 - T_V / tau_u) * xi_u_km1 + sig_u * Math.Sqrt(2 * T_V / tau_u) * nu_u; // eq. (30)
xi_v = (1 - 2 * T_V / tau_u) * xi_v_km1 + sig_u * Math.Sqrt(4 * T_V / tau_u) * nu_v; // eq. (31)
xi_w = (1 - 2 * T_V / tau_w) * xi_w_km1 + sig_w * Math.Sqrt(4 * T_V / tau_w) * nu_w; // eq. (32)
xi_p = (1 - T_V / tau_p) * xi_p_km1 + sig_p * Math.Sqrt(2 * T_V / tau_p) * nu_p; // eq. (33)
xi_q = (1 - T_V / tau_q) * xi_q_km1 + Math.PI / 4 / b_w * (xi_w - xi_w_km1); // eq. (34)
xi_r = (1 - T_V / tau_r) * xi_r_km1 + Math.PI / 3 / b_w * (xi_v - xi_v_km1); // eq. (35)
}
// rotate by wind azimuth and assign the velocities
double cospsi = Math.Cos(psiw), sinpsi = Math.Sin(psiw);
vTurbulenceNED.North = cospsi * xi_u + sinpsi * xi_v;
vTurbulenceNED.East = -sinpsi * xi_u + cospsi * xi_v;
vTurbulenceNED.Down = xi_w;
vTurbPQR.P = cospsi * xi_p + sinpsi * xi_q;
vTurbPQR.Q = -sinpsi * xi_p + cospsi * xi_q;
vTurbPQR.R = xi_r;
// vTurbPQR is in the body fixed frame, not NED
vTurbPQR = inputs.Tl2b * vTurbPQR;
// hand on the values for the next timestep
xi_u_km1 = xi_u; nu_u_km1 = nu_u;
xi_v_km2 = xi_v_km1; xi_v_km1 = xi_v; nu_v_km2 = nu_v_km1; nu_v_km1 = nu_v;
xi_w_km2 = xi_w_km1; xi_w_km1 = xi_w; nu_w_km2 = nu_w_km1; nu_w_km1 = nu_w;
xi_p_km1 = xi_p; nu_p_km1 = nu_p;
xi_q_km1 = xi_q;
xi_r_km1 = xi_r;
break;
default:
break;
}
TurbDirection = Math.Atan2(vTurbulenceNED.East, vTurbulenceNED.North) * Constants.radtodeg;
}
/// <summary>
/// Runs the Aerodynamics model; called by the Executive
/// </summary>
/// <returns>false if no error</returns>
public override bool Run()
{
double alpha, twovel;
if (InternalRun())
{
return(true);
}
if (FDMExec.Holding())
{
return(false); // if paused don't execute
}
// calculate some oft-used quantities for speed
twovel = 2 * FDMExec.Auxiliary.Vt;
if (twovel != 0)
{
bi2vel = FDMExec.Aircraft.WingSpan / twovel;
ci2vel = FDMExec.Aircraft.WingChord / twovel;
}
alphaw = FDMExec.Auxiliary.Getalpha() + FDMExec.Aircraft.WingIncidence;
alpha = FDMExec.Auxiliary.Getalpha();
qbar_area = FDMExec.Aircraft.WingArea * FDMExec.Auxiliary.Qbar;
if (alphaclmax != 0)
{
if (alpha > 0.85 * alphaclmax)
{
impending_stall = 10 * (alpha / alphaclmax - 0.85);
}
else
{
impending_stall = 0;
}
}
if (alphahystmax != 0.0 && alphahystmin != 0.0)
{
if (alpha > alphahystmax)
{
stall_hyst = 1;
}
else if (alpha < alphahystmin)
{
stall_hyst = 0;
}
}
vLastFs = vFs;
vFs = Vector3D.Zero;
// Tell the variable functions to cache their values, so while the aerodynamic
// functions are being calculated for each axis, these functions do not get
// calculated each time, but instead use the values that have already
// been calculated for this frame.
for (int i = 0; i < variables.Count; i++)
{
variables[i].CacheValue(true);
}
for (int axis_ctr = 0; axis_ctr < 3; axis_ctr++)
{
if (Coeff[axis_ctr] != null)
{
foreach (Function func in Coeff[axis_ctr])
{
vFs[axis_ctr] += func.GetValue();
}
}
}
// calculate lift coefficient squared
if (FDMExec.Auxiliary.Qbar > 0)
{
clsq = vFs[(int)StabilityAxisForces.eLift] / (FDMExec.Aircraft.WingArea * FDMExec.Auxiliary.Qbar);
clsq *= clsq;
}
if (vFs[(int)StabilityAxisForces.eDrag] > 0)
{
lod = vFs[(int)StabilityAxisForces.eLift] / vFs[(int)StabilityAxisForces.eDrag];
}
//correct signs of drag and lift to wind axes convention
//positive forward, right, down
vFs[(int)StabilityAxisForces.eDrag] *= -1; vFs[(int)StabilityAxisForces.eLift] *= -1;
vForces = FDMExec.State.GetTs2b() * vFs;
vDXYZcg = FDMExec.MassBalance.StructuralToBody(FDMExec.Aircraft.AeroRefPointXYZ);
vMoments = Vector3D.Cross(vDXYZcg, vForces); // M = r X F
for (int axis_ctr = 0; axis_ctr < 3; axis_ctr++)
{
if (Coeff[axis_ctr + 3] != null)
{
foreach (Function func in Coeff[axis_ctr + 3])
{
vMoments[axis_ctr] += func.GetValue();
}
}
}
return(false);
}
/// <summary>
/// Runs the Auxiliary routines; called by the Executive
/// </summary>
/// <returns>false if no error</returns>
public override bool Run(bool Holding)
{
#if TODO
double A, B, D, hdot_Vt;
Vector3D vPQR = FDMExec.Propagate.GetPQR();
Vector3D vUVW = FDMExec.Propagate.GetUVW();
Vector3D vUVWdot = FDMExec.Propagate.GetUVWdot();
Vector3D vVel = FDMExec.Propagate.GetVel();
if (InternalRun())
{
return(true);
}
if (FDMExec.Holding())
{
return(false); // if paused don't execute
}
p = FDMExec.Atmosphere.Pressure;
rhosl = FDMExec.Atmosphere.DensitySeaLevel;
psl = FDMExec.Atmosphere.PressureSeaLevel;
sat = FDMExec.Atmosphere.Temperature;
// Rotation
double cTht = FDMExec.Propagate.GetCosEuler((int)EulerAngleType.eTht);
double cPhi = FDMExec.Propagate.GetCosEuler((int)EulerAngleType.ePhi);
double sPhi = FDMExec.Propagate.GetSinEuler((int)EulerAngleType.ePhi);
vEulerRates.Theta = vPQR.eQ * cPhi - vPQR.eR * sPhi;
if (cTht != 0.0)
{
vEulerRates.Psi = (vPQR.eQ * sPhi + vPQR.eR * cPhi) / cTht;
vEulerRates.Phi = vPQR.eP + vEulerRates.Psi * sPhi;
}
//TODO vAeroPQR = vPQR + FDMExec.Atmosphere.TurbPQR;
// Translation
//vAeroUVW = vUVW + FDMExec.Propagate.GetTl2b() * FDMExec.Atmosphere.GetWindNED();
vt = vAeroUVW.GetMagnitude();
if (vt > 0.05)
{
if (vAeroUVW.W != 0.0)
{
alpha = vAeroUVW.U * vAeroUVW.U > 0.0 ? Math.Atan2(vAeroUVW.W, vAeroUVW.U) : 0.0;
}
if (vAeroUVW.V != 0.0)
{
beta = vAeroUVW.U * vAeroUVW.U + vAeroUVW.W * vAeroUVW.W > 0.0 ? Math.Atan2(vAeroUVW.V,
Math.Sqrt(vAeroUVW.U * vAeroUVW.U + vAeroUVW.W * vAeroUVW.W)) : 0.0;
}
double mUW = (vAeroUVW.U * vAeroUVW.U + vAeroUVW.W * vAeroUVW.W);
double signU = 1;
if (vAeroUVW.U != 0.0)
{
signU = vAeroUVW.U / Math.Abs(vAeroUVW.U);
}
if ((mUW == 0.0) || (vt == 0.0))
{
adot = 0.0;
bdot = 0.0;
}
else
{
adot = (vAeroUVW.U * vUVWdot.W - vAeroUVW.W * vUVWdot.U) / mUW;
bdot = (signU * mUW * vUVWdot.V - vAeroUVW.V * (vAeroUVW.U * vUVWdot.U
+ vAeroUVW.W * vUVWdot.W)) / (vt * vt * Math.Sqrt(mUW));
}
}
else
{
alpha = beta = adot = bdot = 0;
}
qbar = 0.5 * FDMExec.Atmosphere.Density * vt * vt;
qbarUW = 0.5 * FDMExec.Atmosphere.Density * (vAeroUVW.U * vAeroUVW.U + vAeroUVW.W * vAeroUVW.W);
qbarUV = 0.5 * FDMExec.Atmosphere.Density * (vAeroUVW.U * vAeroUVW.U + vAeroUVW.V * vAeroUVW.V);
mach = vt / FDMExec.Atmosphere.SoundSpeed;
machU = vMachUVW.U = vAeroUVW.U / FDMExec.Atmosphere.SoundSpeed;
vMachUVW.V = vAeroUVW.V / FDMExec.Atmosphere.SoundSpeed;
vMachUVW.W = vAeroUVW.W / FDMExec.Atmosphere.SoundSpeed;
// Position
Vground = Math.Sqrt(vVel.North * vVel.North + vVel.East * vVel.East);
if (vVel.North == 0)
{
psigt = 0;
}
else
{
psigt = Math.Atan2(vVel.East, vVel.North);
}
if (psigt < 0.0)
{
psigt += 2 * Math.PI;
}
if (vt != 0)
{
hdot_Vt = -vVel.Down / vt;
if (Math.Abs(hdot_Vt) <= 1)
{
gamma = Math.Asin(hdot_Vt);
}
}
else
{
gamma = 0.0;
}
tat = sat * (1 + 0.2 * mach * mach); // Total Temperature, isentropic flow
tatc = Conversion.RankineToCelsius(tat);
if (machU < 1)
{ // Calculate total pressure assuming isentropic flow
pt = p * Math.Pow((1 + 0.2 * machU * machU), 3.5);
}
else
{
// Use Rayleigh pitot tube formula for normal shock in front of pitot tube
B = 5.76 * machU * machU / (5.6 * machU * machU - 0.8);
D = (2.8 * machU * machU - 0.4) * 0.4167;
pt = p * Math.Pow(B, 3.5) * D;
}
A = Math.Pow(((pt - p) / psl + 1), 0.28571);
if (machU > 0.0)
{
vcas = Math.Sqrt(7 * psl / rhosl * (A - 1));
veas = Math.Sqrt(2 * qbar / rhosl);
}
else
{
vcas = veas = 0.0;
}
///TODO vPilotAccel.InitMatrix();
if (vt > 1.0)
{
vPilotAccel = FDMExec.Aerodynamics.Forces
+ FDMExec.Propulsion.GetForces()
+ FDMExec.GroundReactions.GetForces();
vPilotAccel /= FDMExec.MassBalance.Mass;
vToEyePt = FDMExec.MassBalance.StructuralToBody(FDMExec.Aircraft.EyepointXYZ);
vPilotAccel += Vector3D.Cross(FDMExec.Propagate.GetPQRdot(), vToEyePt);
vPilotAccel += Vector3D.Cross(vPQR, Vector3D.Cross(vPQR, vToEyePt));
}
else
{
Vector3D aux = new Vector3D(0.0, 0.0, FDMExec.Inertial.Gravity);
vPilotAccel = FDMExec.Propagate.GetTl2b() * aux;
}
vPilotAccelN = vPilotAccel / FDMExec.Inertial.Gravity;
earthPosAngle += FDMExec.State.DeltaTime * FDMExec.Inertial.Omega;
// VRP computation
Location vLocation = FDMExec.Propagate.GetLocation();
Vector3D vrpStructural = FDMExec.Aircraft.VisualRefPointXYZ;
Vector3D vrpBody = FDMExec.MassBalance.StructuralToBody(vrpStructural);
Vector3D vrpLocal = FDMExec.Propagate.GetTb2l() * vrpBody;
vLocationVRP = vLocation.LocalToLocation(vrpLocal);
// Recompute some derived values now that we know the dependent parameters values ...
hoverbcg = FDMExec.Propagate.DistanceAGL / FDMExec.Aircraft.WingSpan;
Vector3D vMac = FDMExec.Propagate.GetTb2l() * FDMExec.MassBalance.StructuralToBody(FDMExec.Aircraft.AeroRefPointXYZ);
hoverbmac = (FDMExec.Propagate.DistanceAGL + vMac.Z) / FDMExec.Aircraft.WingSpan;
return(false);
#endif
throw new NotImplementedException("Pending upgrade to lastest version of JSBSIM");
}
public virtual string FindFullPathName(string path)
{
return(ModelLoader.CheckPathName(FDMExec.GetFullAircraftPath(), path));
}
