public StabilityDerivExportOutput CalculateStabilityDerivs(CelestialBody body, double alt, double machNumber, int flapSetting, bool spoilers, double beta, double phi)
{
double pressure = body.GetPressure(alt);
double temperature = body.GetTemperature(alt);
double density = body.GetDensity(pressure, temperature);
double sspeed = body.GetSpeedOfSound(pressure, density);
double u0 = sspeed * machNumber;
double q = u0 * u0 * density * 0.5f;
Vector3d CoM;
double mass, area, MAC, b;
_instantCondition.GetCoMAndSize(out CoM, out mass, out area, out MAC, out b);
double effectiveG = _instantCondition.CalculateEffectiveGravity(body, alt, u0);
double neededCl = effectiveG * mass / (q * area);
InstantConditionSimVars iterationSimVars =
new InstantConditionSimVars(_instantCondition, body, alt, machNumber, neededCl, beta, phi, flapSetting, spoilers);
InstantConditionSimInput nominalInput;
InstantConditionSimOutput nominalOutput;
InstantConditionSimIterationResult stableCondition =
iterationSimVars.IterateForAlphaAndPitch(out nominalInput, out nominalOutput);
InstantConditionSimInput input = nominalInput.Clone();
InstantConditionSimOutput pertOutput;
double[] derivatives = new double[27];
// update size (in practice MAC and b) to match stableCondition
_instantCondition.GetCoMAndSize(out CoM, out mass, out area, out MAC, out b);
double Ix, Iy, Iz;
double Ixy, Iyz, Ixz;
_instantCondition.GetInertia(CoM, out Ix, out Iy, out Iz, out Ixy, out Iyz, out Ixz);
input.alpha = stableCondition.stableAoA + 2;
iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);
// Rodhern: A change is made to the Xw formula. Theoretically doing " -= nominalOutput.Cl" is the most 'correct',
// it does however in some way mix the Cd value measured at 'stableAoA + 2' with a Cl value measured at 'StableAoA'.
// Because Cl and Cd are very AoA-dependent, the asymmetrical measurement (AoA+=[0;2]) is quite affected.
double pertOutCl = pertOutput.Cl;
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad); //vert vel derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad);
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad);
pertOutput.Cl += nominalOutput.Cd;
pertOutput.Cd -= pertOutCl; // Rodhern: Convergence is worse, but possibly the numerical value is more useful this way.
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (Iy * u0);
derivatives[3] = pertOutput.Cl; //Zw
derivatives[4] = pertOutput.Cd; //Xw
derivatives[5] = pertOutput.Cm; //Mw
input.alpha = stableCondition.stableAoA;
input.fltenv.MachNumber = machNumber + 0.05;
iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber; //fwd vel derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05 * machNumber;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05 * machNumber;
pertOutput.Cl += 2 * nominalOutput.Cl;
pertOutput.Cd += 2 * nominalOutput.Cd;
pertOutput.Cm += 2 * nominalOutput.Cm;
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (Iy * u0);
derivatives[6] = pertOutput.Cl; //Zu
derivatives[7] = pertOutput.Cd; //Xu
derivatives[8] = pertOutput.Cm; //Mu
input.fltenv.MachNumber = machNumber;
input.alphaDot = -3;
iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (3 * FARMathUtil.deg2rad); //pitch rate derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (3 * FARMathUtil.deg2rad);
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (3 * FARMathUtil.deg2rad);
pertOutput.Cl *= -q * area / mass;
pertOutput.Cd *= -q * area / mass;
pertOutput.Cm *= q * area * MAC / Iy;
derivatives[9] = pertOutput.Cl; //Zq
derivatives[10] = pertOutput.Cd; //Xq
derivatives[11] = pertOutput.Cm; //Mq
input.alphaDot = 0;
double pitchDelta = (stableCondition.stablePitchValue > 0) ? -0.1 : 0.1;
input.pitchValue = stableCondition.stablePitchValue + pitchDelta;
iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / pitchDelta; //elevator derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / pitchDelta;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / pitchDelta;
pertOutput.Cl *= -q * area / mass; // Rodhern: Replaced 'q' by '-q', so that formulas
pertOutput.Cd *= -q * area / mass; // for Ze and Xe match those for Zu and Xu.
pertOutput.Cm *= q * area * MAC / Iy;
derivatives[12] = pertOutput.Cl; //Ze
derivatives[13] = pertOutput.Cd; //Xe
derivatives[14] = pertOutput.Cm; //Me
input.pitchValue = stableCondition.stablePitchValue;
input.beta = (beta + 2);
iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad); //sideslip angle derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / (2 * FARMathUtil.deg2rad);
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (2 * FARMathUtil.deg2rad);
pertOutput.Cy *= q * area / mass;
pertOutput.Cn *= q * area * b / Iz;
pertOutput.C_roll *= q * area * b / Ix;
derivatives[15] = pertOutput.Cy; //Yb
derivatives[17] = pertOutput.Cn; //Nb
derivatives[16] = pertOutput.C_roll; //Lb
input.beta = beta;
input.phiDot = -3;
iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / (3 * FARMathUtil.deg2rad); //roll rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / (3 * FARMathUtil.deg2rad);
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (3 * FARMathUtil.deg2rad);
pertOutput.Cy *= q * area / mass;
pertOutput.Cn *= q * area * b / Iz;
pertOutput.C_roll *= q * area * b / Ix;
derivatives[18] = pertOutput.Cy; //Yp
derivatives[20] = pertOutput.Cn; //Np
derivatives[19] = pertOutput.C_roll; //Lp
input.phiDot = 0;
input.betaDot = -3;
iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / (3 * FARMathUtil.deg2rad); //yaw rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / (3 * FARMathUtil.deg2rad);
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (3 * FARMathUtil.deg2rad);
pertOutput.Cy *= q * area / mass;
pertOutput.Cn *= q * area * b / Iz;
pertOutput.C_roll *= q * area * b / Ix;
derivatives[21] = pertOutput.Cy; //Yr
derivatives[23] = pertOutput.Cn; //Nr
derivatives[22] = pertOutput.C_roll; //Lr
input = new InstantConditionSimInput(body); // Reset to (an artificial) default condition
_instantCondition.ResetClCdCmSteady(CoM, input);
// Assign values to output variables
StabilityDerivOutput stabDerivOutput = new StabilityDerivOutput(stableCondition, derivatives);
stabDerivOutput.nominalVelocity = u0;
stabDerivOutput.altitude = alt;
stabDerivOutput.body = body;
stabDerivOutput.b = b;
stabDerivOutput.MAC = MAC;
stabDerivOutput.area = area;
stabDerivOutput.stabDerivs[0] = Ix;
stabDerivOutput.stabDerivs[1] = Iy;
stabDerivOutput.stabDerivs[2] = Iz;
stabDerivOutput.stabDerivs[24] = Ixy;
stabDerivOutput.stabDerivs[25] = Iyz;
stabDerivOutput.stabDerivs[26] = Ixz;
// Assign values to export variables
StabilityDerivExportVariables stabDerivExport = new StabilityDerivExportVariables();
stabDerivExport.craftmass = mass;
stabDerivExport.envpressure = pressure;
stabDerivExport.envtemperature = temperature;
stabDerivExport.envdensity = density;
stabDerivExport.envsoundspeed = sspeed;
stabDerivExport.envg = _instantCondition.CalculateAccelerationDueToGravity(body, alt);
stabDerivExport.sitmach = machNumber;
stabDerivExport.sitdynpres = q;
stabDerivExport.siteffg = _instantCondition.CalculateEffectiveGravity(body, alt, u0);
return(new StabilityDerivExportOutput(stabDerivOutput, stabDerivExport));
}
public GraphData RunTransientSimLateral(StabilityDerivOutput vehicleData, double endTime, double initDt, double[] InitCond)
{
SimMatrix A = new SimMatrix(4, 4);
int i = 0;
int j = 0;
double[] Derivs = new double[27];
vehicleData.stabDerivs.CopyTo(Derivs, 0);
double u0 = vehicleData.nominalVelocity;
double b2u = vehicleData.b / (2 * u0);
double effg = _instantCondition.CalculateEffectiveGravity(vehicleData.body, vehicleData.altitude, u0) * Math.Cos(vehicleData.stableCondition.stableAoA * Math.PI / 180);
double factor_xz_x = Derivs[26] / Derivs[0];
double factor_xz_z = Derivs[26] / Derivs[2];
double factor_invxz = 1 / (1 - factor_xz_x * factor_xz_z);
FARLogger.Info("u0= " + u0);
FARLogger.Info("b/(2u)= " + b2u + " IGNORED!");
FARLogger.Info("effg= " + effg + ", after multiplication with cos(AoA).");
FARLogger.Info("Ixz/Ix= " + factor_xz_x + ", used to add yaw to roll-deriv.");
FARLogger.Info("Ixz/Iz= " + factor_xz_z + ", used to add roll to yaw-deriv.");
FARLogger.Info("(1 - Ixz^2/(IxIz))^-1= " + factor_invxz);
// Rodhern: For possible backward compability the rotation (moment) derivatives can be
// scaled by "b/(2u)" (for pitch rate "mac/(2u)").
//for (int h = 18; h <= 23; h++)
// Derivs[h] = Derivs[h] * b2u;
Derivs[15] = Derivs[15] / u0;
Derivs[18] = Derivs[18] / u0;
Derivs[21] = Derivs[21] / u0 - 1;
double Lb = Derivs[16] * factor_invxz;
double Nb = Derivs[17] * factor_invxz;
double Lp = Derivs[19] * factor_invxz;
double Np = Derivs[20] * factor_invxz;
double Lr = Derivs[22] * factor_invxz;
double Nr = Derivs[23] * factor_invxz;
Derivs[16] = Lb + factor_xz_x * Nb;
Derivs[17] = Nb + factor_xz_z * Lb;
Derivs[19] = Lp + factor_xz_x * Np;
Derivs[20] = Np + factor_xz_z * Lp;
Derivs[22] = Lr + factor_xz_x * Nr;
Derivs[23] = Nr + factor_xz_z * Lr;
for (int k = 15; k < Derivs.Length; k++)
{
double f = Derivs[k];
if (i <= 2)
{
FARLogger.Info("A[" + i + "," + j + "]= f_" + k + " = " + f + ", after manipulation.");
A.Add(f, i, j);
}
if (j < 2)
{
j++;
}
else
{
j = 0;
i++;
}
}
A.Add(effg / u0, 3, 0);
A.Add(1, 1, 3);
A.PrintToConsole(); //We should have an array that looks like this:
/* i --------------->
* j [ Yb / u0 , Yp / u0 , -(1 - Yr/ u0) , g Cos(θ0) / u0 ]
* | [ Lb , Lp , Lr , 0 ]
* | [ Nb , Np , Nr , 0 ]
* \ / [ 0 , 1 , 0 , 0 ]
* V
*/
RungeKutta4 transSolve = new RungeKutta4(endTime, initDt, A, InitCond);
transSolve.Solve();
GraphData lines = new GraphData();
lines.xValues = transSolve.time;
double[] yVal = transSolve.GetSolution(0);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, GUIColors.GetColor(3), "β", true);
yVal = transSolve.GetSolution(1);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, GUIColors.GetColor(2), "p", true);
yVal = transSolve.GetSolution(2);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, GUIColors.GetColor(1), "r", true);
yVal = transSolve.GetSolution(3);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, GUIColors.GetColor(0), "φ", true);
/*graph.SetBoundaries(0, endTime, -10, 10);
* graph.SetGridScaleUsingValues(1, 5);
* graph.horizontalLabel = "time";
* graph.verticalLabel = "value";
* graph.Update();*/
return(lines);
}