public EditorSimManager(InstantConditionSim _instantSim)
{
_instantCondition = _instantSim;
StabDerivCalculator = new StabilityDerivCalculator(_instantCondition);
SweepSim = new SweepSim(_instantCondition);
_aeroCenter = new EditorAeroCenter();
vehicleData = new StabilityDerivOutput();
}
public StabilityDerivGUI(EditorSimManager simManager, GUIDropDown<int> flapSettingDropDown, GUIDropDown<CelestialBody> bodySettingDropdown)
{
this.simManager = simManager;
_flapSettingDropdown = flapSettingDropDown;
_bodySettingDropdown = bodySettingDropdown;
stabDerivOutput = new StabilityDerivOutput();
}
public EditorSimManager()
{
_instantCondition = new InstantConditionSim();
_stabDerivCalculator = new StabilityDerivCalculator(_instantCondition);
_stabDerivLinearSim = new StabilityDerivLinearSim(_instantCondition);
_sweepSim = new SweepSim(_instantCondition);
_aeroCenter = new EditorAeroCenter();
vehicleData = new StabilityDerivOutput();
}
public void Display()
{
//stabDerivHelp = GUILayout.Toggle(stabDerivHelp, "?", ButtonStyle, GUILayout.Width(200));
GUILayout.Label("Flight Condition:");
GUILayout.BeginHorizontal();
GUILayout.Label("Planet:");
_bodySettingDropdown.GUIDropDownDisplay();
GUILayout.Label("Altitude (km):");
altitude = GUILayout.TextField(altitude, GUILayout.ExpandWidth(true));
GUILayout.Label("Mach Number: ");
machNumber = GUILayout.TextField(machNumber, GUILayout.ExpandWidth(true));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Flap Setting: ");
_flapSettingDropdown.GUIDropDownDisplay();
GUILayout.Label("Spoilers:");
spoilersDeployed = GUILayout.Toggle(spoilersDeployed, spoilersDeployed ? "Deployed" : "Retracted", GUILayout.Width(100));
GUILayout.EndHorizontal();
if (GUILayout.Button("Calculate Stability Derivatives", GUILayout.Width(250.0F), GUILayout.Height(25.0F)))
{
CelestialBody body = _bodySettingDropdown.ActiveSelection;
FARAeroUtil.UpdateCurrentActiveBody(body);
//atm_temp_str = Regex.Replace(atm_temp_str, @"[^-?[0-9]*(\.[0-9]*)?]", "");
//rho_str = Regex.Replace(rho_str, @"[^-?[0-9]*(\.[0-9]*)?]", "");
machNumber = Regex.Replace(machNumber, @"[^-?[0-9]*(\.[0-9]*)?]", "");
altitude = Regex.Replace(altitude, @"[^-?[0-9]*(\.[0-9]*)?]", "");
double altitudeDouble = Convert.ToDouble(altitude);
altitudeDouble *= 1000;
double temp = body.GetTemperature(altitudeDouble);
double pressure = body.GetPressure(altitudeDouble);
if (pressure > 0)
{
//double temp = Convert.ToSingle(atm_temp_str);
double machDouble = Convert.ToSingle(machNumber);
machDouble = FARMathUtil.Clamp(machDouble, 0.001, float.PositiveInfinity);
double density = body.GetDensity(pressure, temp);
double sspeed = body.GetSpeedOfSound(pressure, density);
double vel = sspeed * machDouble;
//UpdateControlSettings();
double q = vel * vel * density * 0.5f;
stabDerivOutput = simManager.StabDerivCalculator.CalculateStabilityDerivs(vel, q, machDouble, 0, 0, 0, _flapSettingDropdown.ActiveSelection, spoilersDeployed, body, altitudeDouble);
simManager.vehicleData = stabDerivOutput;
SetAngleVectors(stabDerivOutput.stableAoA);
}
else
PopupDialog.SpawnPopupDialog("Error!", "Altitude was above atmosphere", "OK", false, HighLogic.Skin);
}
GUILayout.BeginHorizontal();
GUILayout.Label("Aircraft Properties", GUILayout.Width(180));
GUILayout.Label("Moments of Inertia", GUILayout.Width(160));
GUILayout.Label("Products of Inertia", GUILayout.Width(160));
GUILayout.Label("Level Flight", GUILayout.Width(140));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.BeginVertical(GUILayout.Width(180));
GUILayout.Label("Ref Area: " + stabDerivOutput.area.ToString("G3") + " m²");
GUILayout.Label("Scaled Chord: " + stabDerivOutput.MAC.ToString("G3") + " m");
GUILayout.Label("Scaled Span: " + stabDerivOutput.b.ToString("G3") + " m");
GUILayout.EndVertical();
GUILayout.BeginVertical(GUILayout.Width(160));
GUILayout.Label(new GUIContent("Ixx: " + stabDerivOutput.stabDerivs[0].ToString("G6") + " kg * m²", "Inertia about X-axis due to rotation about X-axis"));
GUILayout.Label(new GUIContent("Iyy: " + stabDerivOutput.stabDerivs[1].ToString("G6") + " kg * m²", "Inertia about Y-axis due to rotation about Y-axis"));
GUILayout.Label(new GUIContent("Izz: " + stabDerivOutput.stabDerivs[2].ToString("G6") + " kg * m²", "Inertia about Z-axis due to rotation about Z-axis"));
GUILayout.EndVertical();
GUILayout.BeginVertical(GUILayout.Width(160));
GUILayout.Label(new GUIContent("Ixy: " + stabDerivOutput.stabDerivs[24].ToString("G6") + " kg * m²", "Inertia about X-axis due to rotation about Y-axis; is equal to inertia about Y-axis due to rotation about X-axis"));
GUILayout.Label(new GUIContent("Iyz: " + stabDerivOutput.stabDerivs[25].ToString("G6") + " kg * m²", "Inertia about Y-axis due to rotation about Z-axis; is equal to inertia about Z-axis due to rotation about Y-axis"));
GUILayout.Label(new GUIContent("Ixz: " + stabDerivOutput.stabDerivs[26].ToString("G6") + " kg * m²", "Inertia about X-axis due to rotation about Z-axis; is equal to inertia about Z-axis due to rotation about X-axis"));
GUILayout.EndVertical();
GUILayout.BeginVertical(GUILayout.Width(140));
GUILayout.Label(new GUIContent("u0: " + stabDerivOutput.nominalVelocity.ToString("G6") + " m/s", "Air speed based on this mach number and temperature."));
GUILayout.BeginHorizontal();
GUILayout.Label(new GUIContent("Cl: " + stabDerivOutput.stableCl.ToString("G3"), "Required lift coefficient at this mass, speed and air density."));
GUILayout.Label(new GUIContent("Cd: " + stabDerivOutput.stableCd.ToString("G3"), "Resulting drag coefficient at this mass, speed and air density."));
GUILayout.EndHorizontal();
GUILayout.Label(new GUIContent("AoA: " + stabDerivOutput.stableAoAState + stabDerivOutput.stableAoA.ToString("G6") + " deg", "Angle of attack required to achieve the necessary lift force."));
GUILayout.EndVertical();
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Longitudinal Derivatives", GUILayout.Width(160));
GUILayout.EndHorizontal();
GUIStyle BackgroundStyle = new GUIStyle(GUI.skin.box);
BackgroundStyle.hover = BackgroundStyle.active = BackgroundStyle.normal;
GUILayout.BeginVertical(BackgroundStyle);
GUILayout.BeginHorizontal();
GUILayout.Label("Down Vel Derivatives", GUILayout.Width(160));
GUILayout.Label("Fwd Vel Derivatives", GUILayout.Width(160));
GUILayout.Label("Pitch Rate Derivatives", GUILayout.Width(160));
GUILayout.Label("Pitch Ctrl Derivatives", GUILayout.Width(160));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel("Zw: ", stabDerivOutput.stabDerivs[3], " s⁻¹", "Change in Z-direction acceleration with respect to Z-direction velocity; should be negative", 160, -1);
StabilityLabel("Zu: ", stabDerivOutput.stabDerivs[6], " s⁻¹", "Change in Z-direction acceleration with respect to X-direction velocity; should be negative", 160, -1);
StabilityLabel("Zq: ", stabDerivOutput.stabDerivs[9], " m/s", "Change in Z-direction acceleration with respect to pitch-up rate; sign unimportant", 160, 0);
StabilityLabel("Zδe: ", stabDerivOutput.stabDerivs[12], " m/s²", "Change in Z-direction acceleration with respect to pitch control input; should be negative", 160, 0);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel("Xw: ", stabDerivOutput.stabDerivs[4], " s⁻¹", "Change in X-direction acceleration with respect to Z-direction velocity; sign unimportant", 160, 0);
StabilityLabel("Xu: ", stabDerivOutput.stabDerivs[7], " s⁻¹", "Change in X-direction acceleration with respect to X-direction velocity; should be negative", 160, -1);
StabilityLabel("Xq: ", stabDerivOutput.stabDerivs[10], " m/s", "Change in X-direction acceleration with respect to pitch-up rate; sign unimportant", 160, 0);
StabilityLabel("Xδe: ", stabDerivOutput.stabDerivs[13], " m/s²", "Change in X-direction acceleration with respect to pitch control input; sign unimportant", 160, 0);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel("Mw: ", stabDerivOutput.stabDerivs[5], " (m * s)⁻¹", "Change in pitch-up angular acceleration with respect to Z-direction velocity; should be negative", 160, -1);
StabilityLabel("Mu: ", stabDerivOutput.stabDerivs[8], " (m * s)⁻¹", "Change in pitch-up angular acceleration acceleration with respect to X-direction velocity; sign unimportant", 160, 0);
StabilityLabel("Mq: ", stabDerivOutput.stabDerivs[11], " s⁻¹", "Change in pitch-up angular acceleration acceleration with respect to pitch-up rate; should be negative", 160, -1);
StabilityLabel("Mδe: ", stabDerivOutput.stabDerivs[14], " s⁻²", "Change in pitch-up angular acceleration acceleration with respect to pitch control input; should be positive", 160, 1);
GUILayout.EndHorizontal();
GUILayout.EndVertical();
GUILayout.BeginHorizontal();
GUILayout.Label("Lateral Derivatives", GUILayout.Width(160));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Sideslip Derivatives", GUILayout.Width(160));
GUILayout.Label("Roll Rate Derivatives", GUILayout.Width(160));
GUILayout.Label("Yaw Rate Derivatives", GUILayout.Width(160));
GUILayout.EndHorizontal();
GUILayout.BeginVertical(BackgroundStyle);
GUILayout.BeginHorizontal();
StabilityLabel("Yβ: ", stabDerivOutput.stabDerivs[15], " m/s²", "Change in Y-direction acceleration with respect to sideslip angle β; should be negative", 160, -1);
StabilityLabel("Yp: ", stabDerivOutput.stabDerivs[18], " m/s", "Change in Y-direction acceleration with respect to roll-right rate; sign unimportant", 160, 0);
StabilityLabel("Yr: ", stabDerivOutput.stabDerivs[21], " m/s", "Change in Y-direction acceleration with respect to yaw-right rate; should be positive", 160, 1);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel("Lβ: ", stabDerivOutput.stabDerivs[16], " s⁻²", "Change in roll-right angular acceleration with respect to sideslip angle β; should be negative", 160, -1);
StabilityLabel("Lp: ", stabDerivOutput.stabDerivs[19], " s⁻¹", "Change in roll-right angular acceleration with respect to roll-right rate; should be negative", 160, -1);
StabilityLabel("Lr: ", stabDerivOutput.stabDerivs[22], " s⁻¹", "Change in roll-right angular acceleration with respect to yaw-right rate; should be positive", 160, 1);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel("Nβ: ", stabDerivOutput.stabDerivs[17], " s⁻²", "Change in yaw-right angular acceleration with respect to sideslip angle β; should be positive", 160, 1);
StabilityLabel("Np: ", stabDerivOutput.stabDerivs[20], " s⁻¹", "Change in yaw-right angular acceleration with respect to roll-right rate; sign unimportant", 160, 0);
StabilityLabel("Nr: ", stabDerivOutput.stabDerivs[23], " s⁻¹", "Change in yaw-right angular acceleration with respect to yaw-right rate; should be negative", 160, -1);
GUILayout.EndHorizontal();
GUILayout.EndVertical();
DrawTooltip();
}
public StabilityDerivOutput CalculateStabilityDerivs(double u0, double q, double machNumber, double alpha, double beta, double phi, int flapSetting, bool spoilers, CelestialBody body, double alt)
{
StabilityDerivOutput stabDerivOutput = new StabilityDerivOutput();
stabDerivOutput.nominalVelocity = u0;
stabDerivOutput.altitude = alt;
stabDerivOutput.body = body;
Vector3d CoM = Vector3d.zero;
double mass = 0;
double MAC = 0;
double b = 0;
double area = 0;
double Ix = 0;
double Iy = 0;
double Iz = 0;
double Ixy = 0;
double Iyz = 0;
double Ixz = 0;
InstantConditionSimInput input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
InstantConditionSimOutput nominalOutput;
InstantConditionSimOutput pertOutput = new InstantConditionSimOutput();
_instantCondition.GetClCdCmSteady(input, out nominalOutput, true);
List <Part> partsList = EditorLogic.SortedShipList;
for (int i = 0; i < partsList.Count; i++)
{
Part p = partsList[i];
if (FARAeroUtil.IsNonphysical(p))
{
continue;
}
double partMass = p.mass;
if (p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
//partMass += p.GetModuleMass(p.mass);
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
CoM += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
mass += partMass;
FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>();
if (w != null)
{
if (w.isShielded)
{
continue;
}
area += w.S;
MAC += w.GetMAC() * w.S;
b += w.Getb_2() * w.S;
if (w is FARControllableSurface)
{
(w as FARControllableSurface).SetControlStateEditor(CoM, p.transform.up, 0, 0, 0, input.flaps, input.spoilers);
}
}
}
if (area == 0)
{
area = _instantCondition._maxCrossSectionFromBody;
MAC = _instantCondition._bodyLength;
b = 1;
}
MAC /= area;
b /= area;
CoM /= mass;
mass *= 1000;
stabDerivOutput.b = b;
stabDerivOutput.MAC = MAC;
stabDerivOutput.area = area;
for (int i = 0; i < partsList.Count; i++)
{
Part p = partsList[i];
if (p == null || FARAeroUtil.IsNonphysical(p))
{
continue;
}
//This section handles the parallel axis theorem
Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
double x2, y2, z2, x, y, z;
x2 = relPos.z * relPos.z;
y2 = relPos.x * relPos.x;
z2 = relPos.y * relPos.y;
x = relPos.z;
y = relPos.x;
z = relPos.y;
double partMass = p.mass;
if (p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
//partMass += p.GetModuleMass(p.mass);
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
Ix += (y2 + z2) * partMass;
Iy += (x2 + z2) * partMass;
Iz += (x2 + y2) * partMass;
Ixy += -x * y * partMass;
Iyz += -z * y * partMass;
Ixz += -x * z * partMass;
//And this handles the part's own moment of inertia
Vector3 principalInertia = p.Rigidbody.inertiaTensor;
Quaternion prncInertRot = p.Rigidbody.inertiaTensorRotation;
//The rows of the direction cosine matrix for a quaternion
Vector3 Row1 = new Vector3(prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));
Vector3 Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
-prncInertRot.x * prncInertRot.x + prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));
Vector3 Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
-prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y + prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w);
//And converting the principal moments of inertia into the coordinate system used by the system
Ix += principalInertia.x * Row1.x * Row1.x + principalInertia.y * Row1.y * Row1.y + principalInertia.z * Row1.z * Row1.z;
Iy += principalInertia.x * Row2.x * Row2.x + principalInertia.y * Row2.y * Row2.y + principalInertia.z * Row2.z * Row2.z;
Iz += principalInertia.x * Row3.x * Row3.x + principalInertia.y * Row3.y * Row3.y + principalInertia.z * Row3.z * Row3.z;
Ixy += principalInertia.x * Row1.x * Row2.x + principalInertia.y * Row1.y * Row2.y + principalInertia.z * Row1.z * Row2.z;
Ixz += principalInertia.x * Row1.x * Row3.x + principalInertia.y * Row1.y * Row3.y + principalInertia.z * Row1.z * Row3.z;
Iyz += principalInertia.x * Row2.x * Row3.x + principalInertia.y * Row2.y * Row3.y + principalInertia.z * Row2.z * Row3.z;
}
Ix *= 1000;
Iy *= 1000;
Iz *= 1000;
stabDerivOutput.stabDerivs[0] = Ix;
stabDerivOutput.stabDerivs[1] = Iy;
stabDerivOutput.stabDerivs[2] = Iz;
stabDerivOutput.stabDerivs[24] = Ixy;
stabDerivOutput.stabDerivs[25] = Iyz;
stabDerivOutput.stabDerivs[26] = Ixz;
double effectiveG = _instantCondition.CalculateAccelerationDueToGravity(body, alt); //This is the effect of gravity
effectiveG -= u0 * u0 / (alt + body.Radius); //This is the effective reduction of gravity due to high velocity
double neededCl = mass * effectiveG / (q * area);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
//Longitudinal Mess
_instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);
alpha = FARMathUtil.BrentsMethod(_instantCondition.FunctionIterateForAlpha, -30d, 30d, 0.001, 500);
input.alpha = alpha;
nominalOutput = _instantCondition.iterationOutput;
//alpha_str = (alpha * Mathf.PI / 180).ToString();
input.alpha = (alpha + 2);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
stabDerivOutput.stableCl = neededCl;
stabDerivOutput.stableCd = nominalOutput.Cd;
stabDerivOutput.stableAoA = alpha;
stabDerivOutput.stableAoAState = "";
if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
{
stabDerivOutput.stableAoAState = ((nominalOutput.Cl > neededCl) ? "<" : ">");
}
Debug.Log("Cl needed: " + neededCl + ", AoA: " + alpha + ", Cl: " + nominalOutput.Cl + ", Cd: " + nominalOutput.Cd);
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 -= nominalOutput.Cl;
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (Iy * u0);
stabDerivOutput.stabDerivs[3] = pertOutput.Cl; //Zw
stabDerivOutput.stabDerivs[4] = pertOutput.Cd; //Xw
stabDerivOutput.stabDerivs[5] = pertOutput.Cm; //Mw
input.alpha = alpha;
input.machNumber = machNumber + 0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
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.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (u0 * Iy);
stabDerivOutput.stabDerivs[6] = pertOutput.Cl; //Zu
stabDerivOutput.stabDerivs[7] = pertOutput.Cd; //Xu
stabDerivOutput.stabDerivs[8] = pertOutput.Cm; //Mu
input.machNumber = machNumber;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.alphaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05; //pitch rate derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;
pertOutput.Cl *= q * area * MAC / (2 * u0 * mass);
pertOutput.Cd *= q * area * MAC / (2 * u0 * mass);
pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);
stabDerivOutput.stabDerivs[9] = pertOutput.Cl; //Zq
stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq
input.alphaDot = 0;
input.pitchValue = 0.1;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1; //elevator derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;
pertOutput.Cl *= q * area / mass;
pertOutput.Cd *= q * area / mass;
pertOutput.Cm *= q * area * MAC / Iy;
stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me
//Lateral Mess
input.pitchValue = 0;
input.beta = (beta + 2);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
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;
stabDerivOutput.stabDerivs[15] = pertOutput.Cy; //Yb
stabDerivOutput.stabDerivs[17] = pertOutput.Cn; //Nb
stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb
input.beta = beta;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.phiDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05; //roll rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[18] = pertOutput.Cy; //Yp
stabDerivOutput.stabDerivs[20] = pertOutput.Cn; //Np
stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp
input.phiDot = 0;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.betaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f; //yaw rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[21] = pertOutput.Cy; //Yr
stabDerivOutput.stabDerivs[23] = pertOutput.Cn; //Nr
stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr
return(stabDerivOutput);
}
public StabilityDerivExportOutput(StabilityDerivOutput outputvalues, StabilityDerivExportVariables exportvalues)
{
this.outputvals = outputvalues;
this.exportvals = exportvalues;
}
public static GraphData RunTransientSimLateral(
StabilityDerivOutput vehicleData,
double endTime,
double initDt,
double[] InitCond
)
{
var A = new SimMatrix(4, 4);
A.PrintToConsole();
int i = 0;
int j = 0;
int num = 0;
var Derivs = new double[27];
vehicleData.stabDerivs.CopyTo(Derivs, 0);
Derivs[15] = Derivs[15] / vehicleData.nominalVelocity;
Derivs[18] = Derivs[18] / vehicleData.nominalVelocity;
Derivs[21] = Derivs[21] / vehicleData.nominalVelocity - 1;
double Lb = Derivs[16] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Nb = Derivs[17] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Lp = Derivs[19] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Np = Derivs[20] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Lr = Derivs[22] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Nr = Derivs[23] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
Derivs[16] = Lb + Derivs[26] / Derivs[0] * Nb;
Derivs[17] = Nb + Derivs[26] / Derivs[2] * Lb;
Derivs[19] = Lp + Derivs[26] / Derivs[0] * Np;
Derivs[20] = Np + Derivs[26] / Derivs[2] * Lp;
Derivs[22] = Lr + Derivs[26] / Derivs[0] * Nr;
Derivs[23] = Nr + Derivs[26] / Derivs[2] * Lr;
foreach (double f in Derivs)
{
if (num < 15)
{
num++; //Avoid Ix, Iy, Iz and long derivs
continue;
}
num++;
FARLogger.Info("" + i + "," + j);
if (i <= 2)
{
A.Add(f, i, j);
}
if (j < 2)
{
j++;
}
else
{
j = 0;
i++;
}
}
A.Add(InstantConditionSim.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude) *
Math.Cos(vehicleData.stableAoA * Math.PI / 180) /
vehicleData.nominalVelocity,
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 //And one that looks like this:
*
* [ Z e ]
* [ X e ]
* [ M e ]
* [ 0 ]
*
*
*/
var transSolve = new RungeKutta4(endTime, initDt, A, InitCond);
transSolve.Solve();
var lines = new GraphData {
xValues = transSolve.time
};
double[] yVal = transSolve.GetSolution(0);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, FARConfig.GUIColors.LdColor, "β", true);
yVal = transSolve.GetSolution(1);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, FARConfig.GUIColors.CmColor, "p", true);
yVal = transSolve.GetSolution(2);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, FARConfig.GUIColors.CdColor, "r", true);
yVal = transSolve.GetSolution(3);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, FARConfig.GUIColors.ClColor, "φ", true);
return(lines);
}
public StabilityDerivExportOutput CalculateStabilityDerivs(CelestialBody body, double alt, double machNumber, int flapSetting, bool spoilers, double alpha, 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;
StabilityDerivOutput stabDerivOutput = new StabilityDerivOutput();
StabilityDerivExportVariables stabDerivExport = new StabilityDerivExportVariables();
stabDerivOutput.nominalVelocity = u0;
stabDerivOutput.altitude = alt;
stabDerivOutput.body = body;
Vector3d CoM = Vector3d.zero;
double mass = 0;
double MAC = 0;
double b = 0;
double area = 0;
double Ix = 0;
double Iy = 0;
double Iz = 0;
double Ixy = 0;
double Iyz = 0;
double Ixz = 0;
InstantConditionSimInput input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
InstantConditionSimOutput nominalOutput;
InstantConditionSimOutput pertOutput = new InstantConditionSimOutput();
_instantCondition.GetClCdCmSteady(input, out nominalOutput, true);
List <Part> partsList = EditorLogic.SortedShipList;
for (int i = 0; i < partsList.Count; i++)
{
Part p = partsList[i];
if (FARAeroUtil.IsNonphysical(p))
{
continue;
}
double partMass = p.mass;
if (p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
//partMass += p.GetModuleMass(p.mass);
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
CoM += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
mass += partMass;
FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>();
if (w != null)
{
if (w.isShielded)
{
continue;
}
area += w.S;
MAC += w.GetMAC() * w.S;
b += w.Getb_2() * w.S;
if (w is FARControllableSurface)
{
(w as FARControllableSurface).SetControlStateEditor(CoM, p.transform.up, 0, 0, 0, input.flaps, input.spoilers);
}
}
}
if (area.NearlyEqual(0))
{
area = _instantCondition._maxCrossSectionFromBody;
MAC = _instantCondition._bodyLength;
b = 1;
}
MAC /= area;
b /= area;
CoM /= mass;
mass *= 1000;
stabDerivOutput.b = b;
stabDerivOutput.MAC = MAC;
stabDerivOutput.area = area;
for (int i = 0; i < partsList.Count; i++)
{
Part p = partsList[i];
if (p == null || FARAeroUtil.IsNonphysical(p))
{
continue;
}
//This section handles the parallel axis theorem
Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
double x2, y2, z2, x, y, z;
x2 = relPos.z * relPos.z;
y2 = relPos.x * relPos.x;
z2 = relPos.y * relPos.y;
x = relPos.z;
y = relPos.x;
z = relPos.y;
double partMass = p.mass;
if (p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
//partMass += p.GetModuleMass(p.mass);
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
Ix += (y2 + z2) * partMass;
Iy += (x2 + z2) * partMass;
Iz += (x2 + y2) * partMass;
Ixy += -x * y * partMass;
Iyz += -z * y * partMass;
Ixz += -x * z * partMass;
//And this handles the part's own moment of inertia
Vector3 principalInertia = p.Rigidbody.inertiaTensor;
Quaternion prncInertRot = p.Rigidbody.inertiaTensorRotation;
//The rows of the direction cosine matrix for a quaternion
Vector3 Row1 = new Vector3(prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));
Vector3 Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
-prncInertRot.x * prncInertRot.x + prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));
Vector3 Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
-prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y + prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w);
//And converting the principal moments of inertia into the coordinate system used by the system
Ix += principalInertia.x * Row1.x * Row1.x + principalInertia.y * Row1.y * Row1.y + principalInertia.z * Row1.z * Row1.z;
Iy += principalInertia.x * Row2.x * Row2.x + principalInertia.y * Row2.y * Row2.y + principalInertia.z * Row2.z * Row2.z;
Iz += principalInertia.x * Row3.x * Row3.x + principalInertia.y * Row3.y * Row3.y + principalInertia.z * Row3.z * Row3.z;
Ixy += principalInertia.x * Row1.x * Row2.x + principalInertia.y * Row1.y * Row2.y + principalInertia.z * Row1.z * Row2.z;
Ixz += principalInertia.x * Row1.x * Row3.x + principalInertia.y * Row1.y * Row3.y + principalInertia.z * Row1.z * Row3.z;
Iyz += principalInertia.x * Row2.x * Row3.x + principalInertia.y * Row2.y * Row3.y + principalInertia.z * Row2.z * Row3.z;
}
Ix *= 1000;
Iy *= 1000;
Iz *= 1000;
stabDerivOutput.stabDerivs[0] = Ix;
stabDerivOutput.stabDerivs[1] = Iy;
stabDerivOutput.stabDerivs[2] = Iz;
stabDerivOutput.stabDerivs[24] = Ixy;
stabDerivOutput.stabDerivs[25] = Iyz;
stabDerivOutput.stabDerivs[26] = Ixz;
double effectiveG = _instantCondition.CalculateAccelerationDueToGravity(body, alt); //This is the effect of gravity
effectiveG -= u0 * u0 / (alt + body.Radius); //This is the effective reduction of gravity due to high velocity
double neededCl = mass * effectiveG / (q * area);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
//Longitudinal Mess
_instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);
alpha = FARMathUtil.SelectedSearchMethod(machNumber, _instantCondition.FunctionIterateForAlpha);
input.alpha = alpha;
nominalOutput = _instantCondition.iterationOutput;
//alpha_str = (alpha * Mathf.PI / 180).ToString();
input.alpha = (alpha + 2);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
stabDerivOutput.stableCl = neededCl;
stabDerivOutput.stableCd = nominalOutput.Cd;
stabDerivOutput.stableAoA = alpha;
stabDerivOutput.stableAoAState = "";
if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
{
stabDerivOutput.stableAoAState = ((nominalOutput.Cl > neededCl) ? "<" : ">");
}
FARLogger.Info("Cl needed: " + neededCl + ", AoA: " + alpha + ", Cl: " + nominalOutput.Cl + ", Cd: " + nominalOutput.Cd);
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 -= nominalOutput.Cl;
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (Iy * u0);
stabDerivOutput.stabDerivs[3] = pertOutput.Cl; //Zw
stabDerivOutput.stabDerivs[4] = pertOutput.Cd; //Xw
stabDerivOutput.stabDerivs[5] = pertOutput.Cm; //Mw
// Rodhern: The motivation for the revised stability derivatives sign interpretations of Zq, Xq, Ze and Xe
// is to align the sign conventions used for Zu, Zq, Ze, Xu, Xq and Xe. Further explanation can be found
// here: https://forum.kerbalspaceprogram.com/index.php?/topic/109098-official-far-craft-repository/&do=findComment&comment=2425057
input.alpha = alpha;
input.machNumber = machNumber + 0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
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.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (u0 * Iy);
stabDerivOutput.stabDerivs[6] = pertOutput.Cl; //Zu
stabDerivOutput.stabDerivs[7] = pertOutput.Cd; //Xu
stabDerivOutput.stabDerivs[8] = pertOutput.Cm; //Mu
input.machNumber = machNumber;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.alphaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05; //pitch rate derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;
pertOutput.Cl *= -q * area * MAC / (2 * u0 * mass); // Rodhern: Replaced 'q' by '-q', so that formulas
pertOutput.Cd *= -q * area * MAC / (2 * u0 * mass); // for Zq and Xq match those for Zu and Xu.
pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);
stabDerivOutput.stabDerivs[9] = pertOutput.Cl; //Zq
stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq
input.alphaDot = 0;
input.pitchValue = 0.1;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1; //elevator derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;
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;
stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me
//Lateral Mess
input.pitchValue = 0;
input.beta = (beta + 2);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
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;
stabDerivOutput.stabDerivs[15] = pertOutput.Cy; //Yb
stabDerivOutput.stabDerivs[17] = pertOutput.Cn; //Nb
stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb
input.beta = beta;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.phiDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05; //roll rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[18] = pertOutput.Cy; //Yp
stabDerivOutput.stabDerivs[20] = pertOutput.Cn; //Np
stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp
input.phiDot = 0;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.betaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f; //yaw rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[21] = pertOutput.Cy; //Yr
stabDerivOutput.stabDerivs[23] = pertOutput.Cn; //Nr
stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr
// Assign values to export variables
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 = effectiveG;
return(new StabilityDerivExportOutput(stabDerivOutput, stabDerivExport));
}
public GraphData RunTransientSimLateral(StabilityDerivOutput vehicleData, double endTime, double initDt, double[] InitCond)
{
SimMatrix A = new SimMatrix(4, 4);
A.PrintToConsole();
int i = 0;
int j = 0;
int num = 0;
double[] Derivs = new double[27];
vehicleData.stabDerivs.CopyTo(Derivs, 0);
Derivs[15] = Derivs[15] / vehicleData.nominalVelocity;
Derivs[18] = Derivs[18] / vehicleData.nominalVelocity;
Derivs[21] = Derivs[21] / vehicleData.nominalVelocity - 1;
double Lb = Derivs[16] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Nb = Derivs[17] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Lp = Derivs[19] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Np = Derivs[20] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Lr = Derivs[22] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Nr = Derivs[23] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
Derivs[16] = Lb + Derivs[26] / Derivs[0] * Nb;
Derivs[17] = Nb + Derivs[26] / Derivs[2] * Lb;
Derivs[19] = Lp + Derivs[26] / Derivs[0] * Np;
Derivs[20] = Np + Derivs[26] / Derivs[2] * Lp;
Derivs[22] = Lr + Derivs[26] / Derivs[0] * Nr;
Derivs[23] = Nr + Derivs[26] / Derivs[2] * Lr;
for (int k = 0; k < Derivs.Length; k++)
{
double f = Derivs[k];
if (num < 15)
{
num++; //Avoid Ix, Iy, Iz and long derivs
continue;
}
else
num++;
Debug.Log(i + "," + j);
if (i <= 2)
A.Add(f, i, j);
if (j < 2)
j++;
else
{
j = 0;
i++;
}
}
A.Add(_instantCondition.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude) * Math.Cos(vehicleData.stableAoA * Math.PI / 180) / vehicleData.nominalVelocity, 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 //And one that looks like this:
*
* [ Z e ]
* [ X e ]
* [ M e ]
* [ 0 ]
*
*
*/
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;
}
public static GraphData RunTransientSimLongitudinal(
StabilityDerivOutput vehicleData,
double endTime,
double initDt,
double[] InitCond
)
{
var A = new SimMatrix(4, 4);
var B = new SimMatrix(1, 4);
A.PrintToConsole();
int i = 0;
int j = 0;
int num = 0;
foreach (double f in vehicleData.stabDerivs)
{
if (num < 3 || num >= 15)
{
num++; //Avoid Ix, Iy, Iz
continue;
}
num++;
FARLogger.Info(i + "," + j);
if (i <= 2)
{
if (num == 10)
{
A.Add(f + vehicleData.nominalVelocity, i, j);
}
else
{
A.Add(f, i, j);
}
}
else
{
B.Add(f, 0, j);
}
if (j < 2)
{
j++;
}
else
{
j = 0;
i++;
}
}
A.Add(-InstantConditionSim.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude), 3, 1);
A.Add(1, 2, 3);
A.PrintToConsole(); //We should have an array that looks like this:
/* i --------------->
* j [ Z w , Z u , Z q + u, 0 ]
* | [ X w , X u , X q , -g ]
* | [ M w , M u , M q , 0 ]
* \ / [ 0 , 0 , 1 , 0 ]
* V //And one that looks like this:
*
* [ Z e ]
* [ X e ]
* [ M e ]
* [ 0 ]
*
*
*/
var transSolve = new RungeKutta4(endTime, initDt, A, InitCond);
transSolve.Solve();
var lines = new GraphData {
xValues = transSolve.time
};
double[] yVal = transSolve.GetSolution(0);
ScaleAndClampValues(yVal, 1, 50);
lines.AddData(yVal, FARConfig.GUIColors.LdColor, "w", true);
yVal = transSolve.GetSolution(1);
ScaleAndClampValues(yVal, 1, 50);
lines.AddData(yVal, FARConfig.GUIColors.CmColor, "u", true);
yVal = transSolve.GetSolution(2);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, FARConfig.GUIColors.CdColor, "q", true);
yVal = transSolve.GetSolution(3);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, FARConfig.GUIColors.ClColor, "θ", true);
return(lines);
}
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);
}
public GraphData RunTransientSimLongitudinal(StabilityDerivOutput vehicleData, double endTime, double initDt, double[] InitCond)
{
SimMatrix A = new SimMatrix(4, 4);
SimMatrix B = new SimMatrix(1, 4);
A.PrintToConsole();
int i = 0;
int j = 0;
int num = 0;
double[] Derivs = new double[27];
for (int k = 0; k < vehicleData.stabDerivs.Length; k++)
{
double f = vehicleData.stabDerivs[k];
if (num < 3 || num >= 15)
{
num++; //Avoid Ix, Iy, Iz
continue;
}
else
num++;
MonoBehaviour.print(i + "," + j);
if (i <= 2)
if (num == 10)
A.Add(f + vehicleData.nominalVelocity, i, j);
else
A.Add(f, i, j);
else
B.Add(f, 0, j);
if (j < 2)
j++;
else
{
j = 0;
i++;
}
}
A.Add(-_instantCondition.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude), 3, 1);
A.Add(1, 2, 3);
A.PrintToConsole(); //We should have an array that looks like this:
/* i --------------->
* j [ Z w , Z u , Z q + u, 0 ]
* | [ X w , X u , X q , -g ]
* | [ M w , M u , M q , 0 ]
* \ / [ 0 , 0 , 1 , 0 ]
* V //And one that looks like this:
*
* [ Z e ]
* [ X e ]
* [ M e ]
* [ 0 ]
*
*
*/
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, 1, 50);
lines.AddData(yVal, GUIColors.GetColor(3), "w", true);
yVal = transSolve.GetSolution(1);
ScaleAndClampValues(yVal, 1, 50);
lines.AddData(yVal, GUIColors.GetColor(2), "u", true);
yVal = transSolve.GetSolution(2);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, GUIColors.GetColor(1), "q", 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;
}
public StabilityDerivOutput CalculateStabilityDerivs(double u0, double q, double machNumber, double alpha, double beta, double phi, int flapSetting, bool spoilers, CelestialBody body, double alt)
{
StabilityDerivOutput stabDerivOutput = new StabilityDerivOutput();
stabDerivOutput.nominalVelocity = u0;
stabDerivOutput.altitude = alt;
stabDerivOutput.body = body;
Vector3d CoM = Vector3d.zero;
double mass = 0;
double MAC = 0;
double b = 0;
double area = 0;
double Ix = 0;
double Iy = 0;
double Iz = 0;
double Ixy = 0;
double Iyz = 0;
double Ixz = 0;
InstantConditionSimInput input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
InstantConditionSimOutput nominalOutput;
InstantConditionSimOutput pertOutput = new InstantConditionSimOutput();
_instantCondition.GetClCdCmSteady(input, out nominalOutput, true);
List<Part> partsList = EditorLogic.SortedShipList;
for (int i = 0; i < partsList.Count; i++)
{
Part p = partsList[i];
if (FARAeroUtil.IsNonphysical(p))
continue;
double partMass = p.mass;
if (p.Resources.Count > 0)
partMass += p.GetResourceMass();
//partMass += p.GetModuleMass(p.mass);
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
CoM += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
mass += partMass;
FARWingAerodynamicModel w = p.GetComponent<FARWingAerodynamicModel>();
if (w != null)
{
if (w.isShielded)
continue;
area += w.S;
MAC += w.GetMAC() * w.S;
b += w.Getb_2() * w.S;
if (w is FARControllableSurface)
{
(w as FARControllableSurface).SetControlStateEditor(CoM, p.transform.up, 0, 0, 0, input.flaps, input.spoilers);
}
}
}
if (area == 0)
{
area = _instantCondition._maxCrossSectionFromBody;
MAC = _instantCondition._bodyLength;
b = 1;
}
MAC /= area;
b /= area;
CoM /= mass;
mass *= 1000;
stabDerivOutput.b = b;
stabDerivOutput.MAC = MAC;
stabDerivOutput.area = area;
for (int i = 0; i < partsList.Count; i++)
{
Part p = partsList[i];
if (p == null || FARAeroUtil.IsNonphysical(p))
continue;
//This section handles the parallel axis theorem
Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
double x2, y2, z2, x, y, z;
x2 = relPos.z * relPos.z;
y2 = relPos.x * relPos.x;
z2 = relPos.y * relPos.y;
x = relPos.z;
y = relPos.x;
z = relPos.y;
double partMass = p.mass;
if (p.Resources.Count > 0)
partMass += p.GetResourceMass();
//partMass += p.GetModuleMass(p.mass);
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
Ix += (y2 + z2) * partMass;
Iy += (x2 + z2) * partMass;
Iz += (x2 + y2) * partMass;
Ixy += -x * y * partMass;
Iyz += -z * y * partMass;
Ixz += -x * z * partMass;
//And this handles the part's own moment of inertia
Vector3 principalInertia = p.Rigidbody.inertiaTensor;
Quaternion prncInertRot = p.Rigidbody.inertiaTensorRotation;
//The rows of the direction cosine matrix for a quaternion
Vector3 Row1 = new Vector3(prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));
Vector3 Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
-prncInertRot.x * prncInertRot.x + prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));
Vector3 Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
-prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y + prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w);
//And converting the principal moments of inertia into the coordinate system used by the system
Ix += principalInertia.x * Row1.x * Row1.x + principalInertia.y * Row1.y * Row1.y + principalInertia.z * Row1.z * Row1.z;
Iy += principalInertia.x * Row2.x * Row2.x + principalInertia.y * Row2.y * Row2.y + principalInertia.z * Row2.z * Row2.z;
Iz += principalInertia.x * Row3.x * Row3.x + principalInertia.y * Row3.y * Row3.y + principalInertia.z * Row3.z * Row3.z;
Ixy += principalInertia.x * Row1.x * Row2.x + principalInertia.y * Row1.y * Row2.y + principalInertia.z * Row1.z * Row2.z;
Ixz += principalInertia.x * Row1.x * Row3.x + principalInertia.y * Row1.y * Row3.y + principalInertia.z * Row1.z * Row3.z;
Iyz += principalInertia.x * Row2.x * Row3.x + principalInertia.y * Row2.y * Row3.y + principalInertia.z * Row2.z * Row3.z;
}
Ix *= 1000;
Iy *= 1000;
Iz *= 1000;
stabDerivOutput.stabDerivs[0] = Ix;
stabDerivOutput.stabDerivs[1] = Iy;
stabDerivOutput.stabDerivs[2] = Iz;
stabDerivOutput.stabDerivs[24] = Ixy;
stabDerivOutput.stabDerivs[25] = Iyz;
stabDerivOutput.stabDerivs[26] = Ixz;
double effectiveG = _instantCondition.CalculateAccelerationDueToGravity(body, alt); //This is the effect of gravity
effectiveG -= u0 * u0 / (alt + body.Radius); //This is the effective reduction of gravity due to high velocity
double neededCl = mass * effectiveG / (q * area);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
//Longitudinal Mess
_instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);
alpha = FARMathUtil.BrentsMethod(_instantCondition.FunctionIterateForAlpha, -30d, 30d, 0.001, 500);
input.alpha = alpha;
nominalOutput = _instantCondition.iterationOutput;
//alpha_str = (alpha * Mathf.PI / 180).ToString();
input.alpha = (alpha + 2);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
stabDerivOutput.stableCl = neededCl;
stabDerivOutput.stableCd = nominalOutput.Cd;
stabDerivOutput.stableAoA = alpha;
stabDerivOutput.stableAoAState = "";
if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
stabDerivOutput.stableAoAState = ((nominalOutput.Cl > neededCl) ? "<" : ">");
Debug.Log("Cl needed: " + neededCl + ", AoA: " + alpha + ", Cl: " + nominalOutput.Cl + ", Cd: " + nominalOutput.Cd);
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 -= nominalOutput.Cl;
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (Iy * u0);
stabDerivOutput.stabDerivs[3] = pertOutput.Cl; //Zw
stabDerivOutput.stabDerivs[4] = pertOutput.Cd; //Xw
stabDerivOutput.stabDerivs[5] = pertOutput.Cm; //Mw
input.alpha = alpha;
input.machNumber = machNumber + 0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
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.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (u0 * Iy);
stabDerivOutput.stabDerivs[6] = pertOutput.Cl; //Zu
stabDerivOutput.stabDerivs[7] = pertOutput.Cd; //Xu
stabDerivOutput.stabDerivs[8] = pertOutput.Cm; //Mu
input.machNumber = machNumber;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.alphaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05; //pitch rate derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;
pertOutput.Cl *= q * area * MAC / (2 * u0 * mass);
pertOutput.Cd *= q * area * MAC / (2 * u0 * mass);
pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);
stabDerivOutput.stabDerivs[9] = pertOutput.Cl; //Zq
stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq
input.alphaDot = 0;
input.pitchValue = 0.1;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1; //elevator derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;
pertOutput.Cl *= q * area / mass;
pertOutput.Cd *= q * area / mass;
pertOutput.Cm *= q * area * MAC / Iy;
stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me
//Lateral Mess
input.pitchValue = 0;
input.beta = (beta + 2);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
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;
stabDerivOutput.stabDerivs[15] = pertOutput.Cy; //Yb
stabDerivOutput.stabDerivs[17] = pertOutput.Cn; //Nb
stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb
input.beta = beta;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.phiDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05; //roll rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[18] = pertOutput.Cy; //Yp
stabDerivOutput.stabDerivs[20] = pertOutput.Cn; //Np
stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp
input.phiDot = 0;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.betaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f; //yaw rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[21] = pertOutput.Cy; //Yr
stabDerivOutput.stabDerivs[23] = pertOutput.Cn; //Nr
stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr
return stabDerivOutput;
}
public GraphData RunTransientSimLongitudinal(StabilityDerivOutput vehicleData, double endTime, double initDt, double[] InitCond)
{
SimMatrix A = new SimMatrix(4, 4);
SimMatrix B = new SimMatrix(1, 4);
A.PrintToConsole();
int i = 0;
int j = 0;
int num = 0;
double[] Derivs = new double[27];
for (int k = 0; k < vehicleData.stabDerivs.Length; k++)
{
double f = vehicleData.stabDerivs[k];
if (num < 3 || num >= 15)
{
num++; //Avoid Ix, Iy, Iz
continue;
}
else
{
num++;
}
FARLogger.Info(i + "," + j);
if (i <= 2)
{
if (num == 10)
{
A.Add(f + vehicleData.nominalVelocity, i, j);
}
else
{
A.Add(f, i, j);
}
}
else
{
B.Add(f, 0, j);
}
if (j < 2)
{
j++;
}
else
{
j = 0;
i++;
}
}
A.Add(-_instantCondition.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude), 3, 1);
A.Add(1, 2, 3);
A.PrintToConsole(); //We should have an array that looks like this:
/* i --------------->
* j [ Z w , Z u , Z q + u, 0 ]
* | [ X w , X u , X q , -g ]
* | [ M w , M u , M q , 0 ]
* \ / [ 0 , 0 , 1 , 0 ]
* V //And one that looks like this:
*
* [ Z e ]
* [ X e ]
* [ M e ]
* [ 0 ]
*
*
*/
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, 1, 50);
lines.AddData(yVal, GUIColors.GetColor(3), "w", true);
yVal = transSolve.GetSolution(1);
ScaleAndClampValues(yVal, 1, 50);
lines.AddData(yVal, GUIColors.GetColor(2), "u", true);
yVal = transSolve.GetSolution(2);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, GUIColors.GetColor(1), "q", 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);
}
private void DataInput(InitialConditions inits, StabilityDerivOutput vehicleData, bool longitudinal)
{
GUILayout.BeginHorizontal();
for (int i = 0; i < inits.inits.Length; i++)
{
GUILayout.Label("Init " + inits.names[i] +": ");
inits.inits[i] = GUILayout.TextField(inits.inits[i], GUILayout.ExpandWidth(true));
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("End Time: ");
inits.maxTime = GUILayout.TextField(inits.maxTime, GUILayout.ExpandWidth(true));
GUILayout.Label("dt: ");
inits.dt = GUILayout.TextField(inits.dt, GUILayout.ExpandWidth(true));
if (GUILayout.Button("Run Simulation", GUILayout.Width(150.0F), GUILayout.Height(25.0F)))
{
for (int i = 0; i < inits.inits.Length; i++)
{
inits.inits[i] = Regex.Replace(inits.inits[i], @"[^-?[0-9]*(\.[0-9]*)?]", "");
}
inits.maxTime = Regex.Replace(inits.maxTime, @"[^-?[0-9]*(\.[0-9]*)?]", "");
inits.dt = Regex.Replace(inits.dt, @"[^-?[0-9]*(\.[0-9]*)?]", "");
double[] initCond = new double[inits.inits.Length];
for (int i = 0; i < initCond.Length; i++)
{
initCond[i] = Convert.ToDouble(inits.inits[i]) * inits.scaling[i];
}
GraphData data;
if(longitudinal)
data = simManager.StabDerivLinearSim.RunTransientSimLongitudinal(vehicleData, Convert.ToDouble(inits.maxTime), Convert.ToDouble(inits.dt), initCond);
else
data = simManager.StabDerivLinearSim.RunTransientSimLateral(vehicleData, Convert.ToDouble(inits.maxTime), Convert.ToDouble(inits.dt), initCond);
UpdateGraph(data, "time", "params", 0, Convert.ToDouble(inits.maxTime), 50);
}
GUILayout.EndHorizontal();
}
private void LateralGUI(StabilityDerivOutput vehicleData)
{
GUIStyle BackgroundStyle = new GUIStyle(GUI.skin.box);
BackgroundStyle.hover = BackgroundStyle.active = BackgroundStyle.normal;
GUILayout.BeginHorizontal();
GUILayout.Label("Lateral Derivatives", GUILayout.Width(160));
GUILayout.EndHorizontal();
GUILayout.BeginVertical();
GUILayout.BeginHorizontal();
GUILayout.Label("Sideslip Derivatives", GUILayout.Width(160));
GUILayout.Label("Roll Rate Derivatives", GUILayout.Width(160));
GUILayout.Label("Yaw Rate Derivatives", GUILayout.Width(160));
GUILayout.EndHorizontal();
GUILayout.BeginVertical(BackgroundStyle);
GUILayout.BeginHorizontal();
StabilityLabel("Yβ: ", vehicleData.stabDerivs[15], " m/s²", "Change in Y-direction acceleration with respect to sideslip angle β; should be negative", 160, -1);
StabilityLabel("Yp: ", vehicleData.stabDerivs[18], " m/s", "Change in Y-direction acceleration with respect to roll-right rate; sign unimportant", 160, 0);
StabilityLabel("Yr: ", vehicleData.stabDerivs[21], " m/s", "Change in Y-direction acceleration with respect to yaw-right rate; should be positive", 160, 1);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel("Lβ: ", vehicleData.stabDerivs[16], " s⁻²", "Change in roll-right angular acceleration with respect to sideslip angle β; should be negative", 160, -1);
StabilityLabel("Lp: ", vehicleData.stabDerivs[19], " s⁻¹", "Change in roll-right angular acceleration with respect to roll-right rate; should be negative", 160, -1);
StabilityLabel("Lr: ", vehicleData.stabDerivs[22], " s⁻¹", "Change in roll-right angular acceleration with respect to yaw-right rate; should be positive", 160, 1);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel("Nβ: ", vehicleData.stabDerivs[17], " s⁻²", "Change in yaw-right angular acceleration with respect to sideslip angle β; should be positive", 160, 1);
StabilityLabel("Np: ", vehicleData.stabDerivs[20], " s⁻¹", "Change in yaw-right angular acceleration with respect to roll-right rate; sign unimportant", 160, 0);
StabilityLabel("Nr: ", vehicleData.stabDerivs[23], " s⁻¹", "Change in yaw-right angular acceleration with respect to yaw-right rate; should be negative", 160, -1);
GUILayout.EndHorizontal();
GUILayout.EndVertical();
GUILayout.EndVertical();
}
private void LongitudinalGUI(StabilityDerivOutput vehicleData)
{
GUIStyle BackgroundStyle = new GUIStyle(GUI.skin.box);
BackgroundStyle.hover = BackgroundStyle.active = BackgroundStyle.normal;
GUILayout.BeginHorizontal();
GUILayout.Label("Longitudinal Derivatives", GUILayout.Width(160));
GUILayout.EndHorizontal();
GUILayout.BeginVertical();
GUILayout.BeginHorizontal();
GUILayout.Label("Down Vel Derivatives", GUILayout.Width(160));
GUILayout.Label("Fwd Vel Derivatives", GUILayout.Width(160));
GUILayout.Label("Pitch Rate Derivatives", GUILayout.Width(160));
GUILayout.Label("Pitch Ctrl Derivatives", GUILayout.Width(160));
GUILayout.EndHorizontal();
GUILayout.BeginVertical(BackgroundStyle);
GUILayout.BeginHorizontal();
StabilityLabel("Zw: ", vehicleData.stabDerivs[3], " s⁻¹", "Change in Z-direction acceleration with respect to Z-direction velocity; should be negative", 160, -1);
StabilityLabel("Zu: ", vehicleData.stabDerivs[6], " s⁻¹", "Change in Z-direction acceleration with respect to X-direction velocity; should be negative", 160, -1);
StabilityLabel("Zq: ", vehicleData.stabDerivs[9], " m/s", "Change in Z-direction acceleration with respect to pitch-up rate; sign unimportant", 160, 0);
StabilityLabel("Zδe: ", vehicleData.stabDerivs[12], " m/s²", "Change in Z-direction acceleration with respect to pitch control input; should be negative", 160, 0);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel("Xw: ", vehicleData.stabDerivs[4], " s⁻¹", "Change in X-direction acceleration with respect to Z-direction velocity; sign unimportant", 160, 0);
StabilityLabel("Xu: ", vehicleData.stabDerivs[7], " s⁻¹", "Change in X-direction acceleration with respect to X-direction velocity; should be negative", 160, -1);
StabilityLabel("Xq: ", vehicleData.stabDerivs[10], " m/s", "Change in X-direction acceleration with respect to pitch-up rate; sign unimportant", 160, 0);
StabilityLabel("Xδe: ", vehicleData.stabDerivs[13], " m/s²", "Change in X-direction acceleration with respect to pitch control input; sign unimportant", 160, 0);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel("Mw: ", vehicleData.stabDerivs[5], " (m * s)⁻¹", "Change in pitch-up angular acceleration with respect to Z-direction velocity; should be negative", 160, -1);
StabilityLabel("Mu: ", vehicleData.stabDerivs[8], " (m * s)⁻¹", "Change in pitch-up angular acceleration acceleration with respect to X-direction velocity; sign unimportant", 160, 0);
StabilityLabel("Mq: ", vehicleData.stabDerivs[11], " s⁻¹", "Change in pitch-up angular acceleration acceleration with respect to pitch-up rate; should be negative", 160, -1);
StabilityLabel("Mδe: ", vehicleData.stabDerivs[14], " s⁻²", "Change in pitch-up angular acceleration acceleration with respect to pitch control input; should be positive", 160, 1);
GUILayout.EndHorizontal();
GUILayout.EndVertical();
GUILayout.EndVertical();
}
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 StabilityDerivOutput CalculateStabilityDerivs(
CelestialBody body,
double alt,
double machNumber,
int flapSetting,
bool spoilers,
double alpha,
double beta,
double phi
)
{
GasProperties properties = FARAtmosphere.GetGasProperties(body,
new Vector3d(0, 0, alt),
Planetarium.GetUniversalTime());
double pressure = properties.Pressure;
double temperature = properties.Temperature;
double density = properties.Density;
double sspeed = properties.SpeedOfSound;
double u0 = sspeed * machNumber;
double q = u0 * u0 * density * 0.5f;
var stabDerivOutput = new StabilityDerivOutput
{
nominalVelocity = u0,
altitude = alt,
body = body
};
Vector3d CoM = Vector3d.zero;
double mass = 0;
double MAC = 0;
double b = 0;
double area = 0;
double Ix = 0;
double Iy = 0;
double Iz = 0;
double Ixy = 0;
double Iyz = 0;
double Ixz = 0;
var input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
var pertOutput = new InstantConditionSimOutput();
_instantCondition.GetClCdCmSteady(input, out InstantConditionSimOutput nominalOutput, true);
List <Part> partsList = EditorLogic.SortedShipList;
foreach (Part p in partsList)
{
if (FARAeroUtil.IsNonphysical(p))
{
continue;
}
double partMass = p.mass;
if (p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
CoM += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
mass += partMass;
FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>();
if (w == null)
{
continue;
}
if (w.isShielded)
{
continue;
}
area += w.S;
MAC += w.GetMAC() * w.S;
b += w.Getb_2() * w.S;
if (w is FARControllableSurface controllableSurface)
{
controllableSurface.SetControlStateEditor(CoM,
p.transform.up,
0,
0,
0,
input.flaps,
input.spoilers);
}
}
if (area.NearlyEqual(0))
{
area = _instantCondition._maxCrossSectionFromBody;
MAC = _instantCondition._bodyLength;
b = 1;
}
MAC /= area;
b /= area;
CoM /= mass;
mass *= 1000;
stabDerivOutput.b = b;
stabDerivOutput.MAC = MAC;
stabDerivOutput.area = area;
foreach (Part p in partsList)
{
if (p == null || FARAeroUtil.IsNonphysical(p))
{
continue;
}
//This section handles the parallel axis theorem
Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
double x2 = relPos.z * relPos.z;
double y2 = relPos.x * relPos.x;
double z2 = relPos.y * relPos.y;
double x = relPos.z;
double y = relPos.x;
double z = relPos.y;
double partMass = p.mass;
if (p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
Ix += (y2 + z2) * partMass;
Iy += (x2 + z2) * partMass;
Iz += (x2 + y2) * partMass;
Ixy += -x * y * partMass;
Iyz += -z * y * partMass;
Ixz += -x * z * partMass;
//And this handles the part's own moment of inertia
Vector3 principalInertia = p.Rigidbody.inertiaTensor;
Quaternion prncInertRot = p.Rigidbody.inertiaTensorRotation;
//The rows of the direction cosine matrix for a quaternion
var Row1 =
new Vector3(prncInertRot.x * prncInertRot.x -
prncInertRot.y * prncInertRot.y -
prncInertRot.z * prncInertRot.z +
prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));
var Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
-prncInertRot.x * prncInertRot.x +
prncInertRot.y * prncInertRot.y -
prncInertRot.z * prncInertRot.z +
prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));
var Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
-prncInertRot.x * prncInertRot.x -
prncInertRot.y * prncInertRot.y +
prncInertRot.z * prncInertRot.z +
prncInertRot.w * prncInertRot.w);
//And converting the principal moments of inertia into the coordinate system used by the system
Ix += principalInertia.x * Row1.x * Row1.x +
principalInertia.y * Row1.y * Row1.y +
principalInertia.z * Row1.z * Row1.z;
Iy += principalInertia.x * Row2.x * Row2.x +
principalInertia.y * Row2.y * Row2.y +
principalInertia.z * Row2.z * Row2.z;
Iz += principalInertia.x * Row3.x * Row3.x +
principalInertia.y * Row3.y * Row3.y +
principalInertia.z * Row3.z * Row3.z;
Ixy += principalInertia.x * Row1.x * Row2.x +
principalInertia.y * Row1.y * Row2.y +
principalInertia.z * Row1.z * Row2.z;
Ixz += principalInertia.x * Row1.x * Row3.x +
principalInertia.y * Row1.y * Row3.y +
principalInertia.z * Row1.z * Row3.z;
Iyz += principalInertia.x * Row2.x * Row3.x +
principalInertia.y * Row2.y * Row3.y +
principalInertia.z * Row2.z * Row3.z;
}
Ix *= 1000;
Iy *= 1000;
Iz *= 1000;
stabDerivOutput.stabDerivs[0] = Ix;
stabDerivOutput.stabDerivs[1] = Iy;
stabDerivOutput.stabDerivs[2] = Iz;
stabDerivOutput.stabDerivs[24] = Ixy;
stabDerivOutput.stabDerivs[25] = Iyz;
stabDerivOutput.stabDerivs[26] = Ixz;
//This is the effect of gravity
double effectiveG = InstantConditionSim.CalculateAccelerationDueToGravity(body, alt);
//This is the effective reduction of gravity due to high velocity
effectiveG -= u0 * u0 / (alt + body.Radius);
double neededCl = mass * effectiveG / (q * area);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
//Longitudinal Mess
_instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);
FARMathUtil.OptimizationResult optResult =
FARMathUtil.Secant(_instantCondition.FunctionIterateForAlpha,
0,
10,
1e-4,
1e-4,
minLimit: -90,
maxLimit: 90);
int calls = optResult.FunctionCalls;
// if stable AoA doesn't exist, calculate derivatives at 0 incidence
if (!optResult.Converged)
{
FARLogger.Info("Stable angle of attack not found, calculating derivatives at 0 incidence instead");
alpha = 0;
_instantCondition.FunctionIterateForAlpha(alpha);
calls += 1;
}
else
{
alpha = optResult.Result;
}
input.alpha = alpha;
nominalOutput = _instantCondition.iterationOutput;
input.alpha = alpha + 2;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
stabDerivOutput.stableCl = neededCl;
stabDerivOutput.stableCd = nominalOutput.Cd;
stabDerivOutput.stableAoA = alpha;
stabDerivOutput.stableAoAState = "";
if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
{
stabDerivOutput.stableAoAState = nominalOutput.Cl > neededCl ? "<" : ">";
}
FARLogger.Info("Cl needed: " +
neededCl.ToString(CultureInfo.InvariantCulture) +
", AoA: " +
stabDerivOutput.stableAoA.ToString(CultureInfo.InvariantCulture) +
", Cl: " +
nominalOutput.Cl.ToString(CultureInfo.InvariantCulture) +
", Cd: " +
nominalOutput.Cd.ToString(CultureInfo.InvariantCulture) +
", function calls: " +
calls.ToString());
//vert vel derivs
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad);
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad);
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad);
pertOutput.Cl += nominalOutput.Cd;
pertOutput.Cd -= nominalOutput.Cl;
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (Iy * u0);
stabDerivOutput.stabDerivs[3] = pertOutput.Cl; //Zw
stabDerivOutput.stabDerivs[4] = pertOutput.Cd; //Xw
stabDerivOutput.stabDerivs[5] = pertOutput.Cm; //Mw
// Rodhern: The motivation for the revised stability derivatives sign interpretations of Zq, Xq, Ze and Xe
// is to align the sign conventions used for Zu, Zq, Ze, Xu, Xq and Xe. Further explanation can be found
// here: https://forum.kerbalspaceprogram.com/index.php?/topic/109098-official-far-craft-repository/&do=findComment&comment=2425057
input.alpha = alpha;
input.machNumber = machNumber + 0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
//fwd vel derivs
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber;
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.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (u0 * Iy);
stabDerivOutput.stabDerivs[6] = pertOutput.Cl; //Zu
stabDerivOutput.stabDerivs[7] = pertOutput.Cd; //Xu
stabDerivOutput.stabDerivs[8] = pertOutput.Cm; //Mu
input.machNumber = machNumber;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.alphaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
//pitch rate derivs
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05;
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;
pertOutput.Cl *= -q * area * MAC / (2 * u0 * mass); // Rodhern: Replaced 'q' by '-q', so that formulas
pertOutput.Cd *= -q * area * MAC / (2 * u0 * mass); // for Zq and Xq match those for Zu and Xu.
pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);
stabDerivOutput.stabDerivs[9] = pertOutput.Cl; //Zq
stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq
input.alphaDot = 0;
input.pitchValue = 0.1;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
//elevator derivs
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1;
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;
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;
stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me
//Lateral Mess
input.pitchValue = 0;
input.beta = beta + 2;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
//sideslip angle derivs
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad);
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;
stabDerivOutput.stabDerivs[15] = pertOutput.Cy; //Yb
stabDerivOutput.stabDerivs[17] = pertOutput.Cn; //Nb
stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb
input.beta = beta;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.phiDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
//roll rate derivs
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05;
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[18] = pertOutput.Cy; //Yp
stabDerivOutput.stabDerivs[20] = pertOutput.Cn; //Np
stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp
input.phiDot = 0;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.betaDot = -0.05;
//yaw rate derivs
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f;
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[21] = pertOutput.Cy; //Yr
stabDerivOutput.stabDerivs[23] = pertOutput.Cn; //Nr
stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr
return(stabDerivOutput);
}
public GraphData RunTransientSimLateral(StabilityDerivOutput vehicleData, double endTime, double initDt, double[] InitCond)
{
SimMatrix A = new SimMatrix(4, 4);
A.PrintToConsole();
int i = 0;
int j = 0;
int num = 0;
double[] Derivs = new double[27];
vehicleData.stabDerivs.CopyTo(Derivs, 0);
Derivs[15] = Derivs[15] / vehicleData.nominalVelocity;
Derivs[18] = Derivs[18] / vehicleData.nominalVelocity;
Derivs[21] = Derivs[21] / vehicleData.nominalVelocity - 1;
double Lb = Derivs[16] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Nb = Derivs[17] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Lp = Derivs[19] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Np = Derivs[20] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Lr = Derivs[22] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Nr = Derivs[23] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
Derivs[16] = Lb + Derivs[26] / Derivs[0] * Nb;
Derivs[17] = Nb + Derivs[26] / Derivs[2] * Lb;
Derivs[19] = Lp + Derivs[26] / Derivs[0] * Np;
Derivs[20] = Np + Derivs[26] / Derivs[2] * Lp;
Derivs[22] = Lr + Derivs[26] / Derivs[0] * Nr;
Derivs[23] = Nr + Derivs[26] / Derivs[2] * Lr;
for (int k = 0; k < Derivs.Length; k++)
{
double f = Derivs[k];
if (num < 15)
{
num++; //Avoid Ix, Iy, Iz and long derivs
continue;
}
else
{
num++;
}
FARLogger.Info("" + i + "," + j);
if (i <= 2)
{
A.Add(f, i, j);
}
if (j < 2)
{
j++;
}
else
{
j = 0;
i++;
}
}
A.Add(_instantCondition.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude) * Math.Cos(vehicleData.stableAoA * Math.PI / 180) / vehicleData.nominalVelocity, 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 //And one that looks like this:
*
* [ Z e ]
* [ X e ]
* [ M e ]
* [ 0 ]
*
*
*/
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);
}
public GraphData RunTransientSimLongitudinal(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 MAC2u = vehicleData.MAC / (2 * vehicleData.nominalVelocity);
double effg = _instantCondition.CalculateEffectiveGravity(vehicleData.body, vehicleData.altitude, vehicleData.nominalVelocity);
FARLogger.Info("MAC/(2u)= " + MAC2u + " IGNORED!");
FARLogger.Info("effg= " + effg);
// Rodhern: For possible backward compability the rotation (moment) derivatives can be
// scaled by "mac/(2u)" (pitch) and "b/(2u)" (roll and yaw).
//for (int h = 9; h <= 11; h++)
// Derivs[h] = Derivs[h] * MAC2u;
Derivs[9] = Derivs[9] + vehicleData.nominalVelocity;
for (int k = 3; k < 15 && k < Derivs.Length; k++)
{
double f = Derivs[k];
if (i <= 2)
{
FARLogger.Info("A[" + i + "," + j + "]= f_" + k + " = " + f);
A.Add(f, i, j);
}
else
{
FARLogger.Debug("Ignore B[0," + j + "]= " + f);
}
if (j < 2)
{
j++;
}
else
{
j = 0;
i++;
}
}
A.Add(-effg, 3, 1);
A.Add(1, 2, 3);
A.PrintToConsole(); //We should have an array that looks like this:
/* i --------------->
* j [ Z w , Z u , Z q + u, 0 ]
* | [ X w , X u , X q , -g ]
* | [ M w , M u , M q , 0 ]
* \ / [ 0 , 0 , 1 , 0 ]
* V
*/
//And one that looks like this: (Unused)
/*
* [ Z e ]
* [ X e ]
* [ M e ]
* [ 0 ]
*
*/
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, 1, 50);
lines.AddData(yVal, GUIColors.GetColor(3), "w", true);
yVal = transSolve.GetSolution(1);
ScaleAndClampValues(yVal, 1, 50);
lines.AddData(yVal, GUIColors.GetColor(2), "u", true);
yVal = transSolve.GetSolution(2);
ScaleAndClampValues(yVal, 180 / Math.PI, 50);
lines.AddData(yVal, GUIColors.GetColor(1), "q", 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);
}