private void BelowGraphInputsGUI(GraphInputs input)
{
GUILayout.BeginHorizontal();
GUILayout.Label("Lower: ", GUILayout.Width(50.0F), GUILayout.Height(25.0F));
input.lowerBound = GUILayout.TextField(input.lowerBound, GUILayout.ExpandWidth(true));
GUILayout.Label("Upper: ", GUILayout.Width(50.0F), GUILayout.Height(25.0F));
input.upperBound = GUILayout.TextField(input.upperBound, GUILayout.ExpandWidth(true));
GUILayout.Label("Num Pts: ", GUILayout.Width(70.0F), GUILayout.Height(25.0F));
input.numPts = GUILayout.TextField(input.numPts, GUILayout.ExpandWidth(true));
GUILayout.Label(isMachMode ? "AoA" : "Mach", GUILayout.Width(50.0F), GUILayout.Height(25.0F));
input.otherInput = GUILayout.TextField(input.otherInput, GUILayout.ExpandWidth(true));
if (GUILayout.Button(isMachMode ? "Sweep Mach" : "Sweep AoA", GUILayout.Width(100.0F), GUILayout.Height(25.0F)))
{
input.lowerBound = Regex.Replace(input.lowerBound, @"[^-?[0-9]*(\.[0-9]*)?]", "");
input.upperBound = Regex.Replace(input.upperBound, @"[^-?[0-9]*(\.[0-9]*)?]", "");
input.numPts = Regex.Replace(input.numPts, @"[^-?[0-9]*(\.[0-9]*)?]", "");
input.pitchSetting = Regex.Replace(input.pitchSetting, @"[^-?[0-9]*(\.[0-9]*)?]", "");
input.otherInput = Regex.Replace(input.otherInput, @"[^-?[0-9]*(\.[0-9]*)?]", "");
double lowerBound, upperBound, numPts, pitchSetting, otherInput;
lowerBound = double.Parse(input.lowerBound);
lowerBound = FARMathUtil.Clamp(lowerBound, -90, 90);
input.lowerBound = lowerBound.ToString();
upperBound = double.Parse(input.upperBound);
upperBound = FARMathUtil.Clamp(upperBound, lowerBound, 90);
input.upperBound = upperBound.ToString();
numPts = double.Parse(input.numPts);
numPts = Math.Ceiling(numPts);
input.numPts = numPts.ToString();
pitchSetting = double.Parse(input.pitchSetting);
pitchSetting = FARMathUtil.Clamp(pitchSetting, -1, 1);
input.pitchSetting = pitchSetting.ToString();
otherInput = double.Parse(input.otherInput);
GraphData data;
if (isMachMode)
{
data = simManager.SweepSim.MachNumberSweep(otherInput, pitchSetting, lowerBound, upperBound, (int)numPts, input.flapSetting, input.spoilers, bodySettingDropdown.ActiveSelection);
SetAngleVectors(pitchSetting, pitchSetting);
}
else
{
data = simManager.SweepSim.AngleOfAttackSweep(otherInput, pitchSetting, lowerBound, upperBound, (int)numPts, input.flapSetting, input.spoilers, bodySettingDropdown.ActiveSelection);
SetAngleVectors(lowerBound, upperBound);
}
UpdateGraph(data, isMachMode ? "Mach Number" : "Angle of Attack, degrees", "Cl\nCd\nCm\nL/D / 10", lowerBound, upperBound);
}
GUILayout.EndHorizontal();
}
private void StabDerivCalcButtonAction(CalcAndExportEnum exportflag)
{
CelestialBody body = _bodySettingDropdown.ActiveSelection;
FARAeroUtil.UpdateCurrentActiveBody(body);
altitude = Regex.Replace(altitude, @"[^-?[0-9]*(\.[0-9]*)?]", "");
double altitudeDouble = Convert.ToDouble(altitude) * 1000;
machNumber = Regex.Replace(machNumber, @"[^-?[0-9]*(\.[0-9]*)?]", "");
double machDouble = FARMathUtil.Clamp(Convert.ToSingle(machNumber), 0.001, float.PositiveInfinity);
int flapsettingInt = _flapSettingDropdown.ActiveSelection;
bool spoilersDeployedBool = spoilersDeployed;
if (exportflag == CalcAndExportEnum.LoopExport)
{
int n = 0;
ExportTextFileCache filecache = new ExportTextFileCache();
foreach (Vector2 altmach in StabilityDerivativeExportFile.LoadConfigList())
{
StabilityDerivExportOutput output = simManager.StabDerivCalculator.CalculateStabilityDerivs(body, (double)altmach.x, (double)altmach.y, flapsettingInt, spoilersDeployedBool);
if (AoAOk(output, exportflag) && StabilityDerivativeExportFile.Export(output, filecache))
{
n++;
}
}
if (n > 0)
{
filecache.FlushTextFileLines();
PopupDialog.SpawnPopupDialog(new Vector2(0, 0), new Vector2(0, 0), "FARStabDerivLoopCount", Localizer.Format("FAREditorStabDerivLoopDone"), Localizer.Format("FAREditorStabDerivLoopDoneExp", n), Localizer.Format("FARGUIOKButton"), true, HighLogic.UISkin);
}
else
{
PopupDialog.SpawnPopupDialog(new Vector2(0, 0), new Vector2(0, 0), "FARStabDerivSaveError", Localizer.Format("FAREditorStabDerivSaveError"), Localizer.Format("FAREditorStabDerivSaveErrorExp"), Localizer.Format("FARGUIOKButton"), true, HighLogic.UISkin);
}
return; // in the LoopExport case skip the usual calculation
}
StabilityDerivExportOutput stabDerivResult = simManager.StabDerivCalculator.CalculateStabilityDerivs(body, altitudeDouble, machDouble, flapsettingInt, spoilersDeployedBool);
if (!AoAOk(stabDerivResult, exportflag))
{
PopupDialog.SpawnPopupDialog(new Vector2(0, 0), new Vector2(0, 0), "FARStabDerivError", Localizer.Format("FAREditorStabDerivError"), Localizer.Format("FAREditorStabDerivErrorExp"), Localizer.Format("FARGUIOKButton"), true, HighLogic.UISkin);
}
else
{
stabDerivOutput = stabDerivResult.outputvals;
simManager.vehicleData = stabDerivResult.outputvals;
SetAngleVectors(stabDerivResult.outputvals.stableCondition.stableAoA);
if (exportflag == CalcAndExportEnum.CalculateAndExport && !StabilityDerivativeExportFile.Export(stabDerivResult))
{
PopupDialog.SpawnPopupDialog(new Vector2(0, 0), new Vector2(0, 0), "FARStabDerivSaveError", Localizer.Format("FAREditorStabDerivSaveError"), Localizer.Format("FAREditorStabDerivSaveErrorExp"), Localizer.Format("FARGUIOKButton"), true, HighLogic.UISkin);
}
}
}
public double FindPitchForAlpha(double alpha)
{
if (Double.IsNaN(alpha))
{
return(Double.NaN);
}
else
{
iterationInput.alpha = alpha;
return(FARMathUtil.SegmentSearchMethod(this.FunctionIterateForPitch, pitchtol));
}
}
public double FindAlphaForPitch(double pitch)
{
if (Double.IsNaN(pitch))
{
return(Double.NaN);
}
else
{
iterationInput.pitchValue = pitch;
return(FARMathUtil.SegmentSearchMethod(this.FunctionIterateForAlpha, alphatol));
}
}
public CrossSection GetCrossSectionAtStation(double station)
{
CrossSection section = new CrossSection();
section.station = station;
CrossSection lowerSection, upperSection;
crossSectionsTree.FindNearestData(section, out lowerSection, out upperSection);
section.centroid.x = FARMathUtil.Lerp(lowerSection.station, upperSection.station, lowerSection.centroid.x, upperSection.centroid.x, station);
section.centroid.y = FARMathUtil.Lerp(lowerSection.station, upperSection.station, lowerSection.centroid.y, upperSection.centroid.y, station);
section.lengths.x = FARMathUtil.Lerp(lowerSection.station, upperSection.station, lowerSection.lengths.x, upperSection.lengths.x, station);
section.lengths.y = FARMathUtil.Lerp(lowerSection.station, upperSection.station, lowerSection.lengths.y, upperSection.lengths.y, station);
section.areaFraction = FARMathUtil.Lerp(lowerSection.station, upperSection.station, lowerSection.areaFraction, upperSection.areaFraction, station);
return(section);
}
private void StabDerivCalcButtonAction()
{
CelestialBody body = _bodySettingDropdown.ActiveSelection;
FARAeroUtil.UpdateCurrentActiveBody(body);
altitude = Regex.Replace(altitude, @"[^-?[0-9]*(\.[0-9]*)?]", "");
double altitudeDouble = Convert.ToDouble(altitude) * 1000;
machNumber = Regex.Replace(machNumber, @"[^-?[0-9]*(\.[0-9]*)?]", "");
double machDouble = FARMathUtil.Clamp(Convert.ToSingle(machNumber), 0.001, float.PositiveInfinity);
int flapsettingInt = _flapSettingDropdown.ActiveSelection;
bool spoilersDeployedBool = spoilersDeployed;
if (FARAtmosphere.GetPressure(body, new Vector3d(0, 0, altitudeDouble), Planetarium.GetUniversalTime()) > 0)
{
stabDerivOutput =
simManager.StabDerivCalculator.CalculateStabilityDerivs(body,
altitudeDouble,
machDouble,
flapsettingInt,
spoilersDeployedBool,
0,
0,
0);
simManager.vehicleData = stabDerivOutput;
SetAngleVectors(stabDerivOutput.stableAoA);
}
else
{
PopupDialog.SpawnPopupDialog(new Vector2(0, 0),
new Vector2(0, 0),
"FARStabDerivError",
Localizer.Format("FAREditorStabDerivError"),
Localizer.Format("FAREditorStabDerivErrorExp"),
Localizer.Format("FARGUIOKButton"),
true,
HighLogic.UISkin);
}
}
private void StabDerivCalcButtonAction(CalcAndExportEnum exportflag)
{
CelestialBody body = _bodySettingDropdown.ActiveSelection;
FARAeroUtil.UpdateCurrentActiveBody(body);
altitude = Regex.Replace(altitude, @"[^-?[0-9]*(\.[0-9]*)?]", "");
double altitudeDouble = Convert.ToDouble(altitude) * 1000;
machNumber = Regex.Replace(machNumber, @"[^-?[0-9]*(\.[0-9]*)?]", "");
double machDouble = FARMathUtil.Clamp(Convert.ToSingle(machNumber), 0.001, float.PositiveInfinity);
int flapsettingInt = _flapSettingDropdown.ActiveSelection;
bool spoilersDeployedBool = spoilersDeployed;
// Rodhern: Export of stability derivatives disabled in dkavolis branch.
// if (exportflag == CalcAndExportEnum.LoopExport ...
if (body.GetPressure(altitudeDouble) > 0)
{
StabilityDerivExportOutput stabDerivResult = simManager.StabDerivCalculator.CalculateStabilityDerivs(body, altitudeDouble, machDouble, flapsettingInt, spoilersDeployedBool, 0, 0, 0);
// if (stabDerivResult.outputvals.stableAoAState == "")
// {
stabDerivOutput = stabDerivResult.outputvals;
simManager.vehicleData = stabDerivResult.outputvals;
SetAngleVectors(stabDerivResult.outputvals.stableAoA);
// Rodhern: Export of stability derivatives disabled in dkavolis branch.
// if (exportflag == CalcAndExportEnum.CalculateAndExport ...
// }
// else
// PopupDialog.SpawnPopupDialog(new Vector2(0, 0), new Vector2(0, 0), "FARStabDerivError", Localizer.Format("FAREditorStabDerivError"), Localizer.Format("FAREditorStabDerivErrorExp"), Localizer.Format("FARGUIOKButton"), true, HighLogic.UISkin);
}
else
{
PopupDialog.SpawnPopupDialog(new Vector2(0, 0), new Vector2(0, 0), "FARStabDerivError", Localizer.Format("FAREditorStabDerivError"), Localizer.Format("FAREditorStabDerivErrorExp"), Localizer.Format("FARGUIOKButton"), true, HighLogic.UISkin);
}
}
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 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();
}
private void BelowGraphInputsGUI(GraphInputs input)
{
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("FAREditorStaticGraphLowLim"), GUILayout.Width(50.0F), GUILayout.Height(25.0F));
input.lowerBound = GUILayout.TextField(input.lowerBound, GUILayout.ExpandWidth(true));
GUILayout.Label(Localizer.Format("FAREditorStaticGraphUpLim"), GUILayout.Width(50.0F), GUILayout.Height(25.0F));
input.upperBound = GUILayout.TextField(input.upperBound, GUILayout.ExpandWidth(true));
GUILayout.Label(Localizer.Format("FAREditorStaticGraphPtCount"), GUILayout.Width(70.0F), GUILayout.Height(25.0F));
input.numPts = GUILayout.TextField(input.numPts, GUILayout.ExpandWidth(true));
GUILayout.Label(isMachMode ? Localizer.Format("FARAbbrevAoA") : Localizer.Format("FARAbbrevMach"), GUILayout.Width(50.0F), GUILayout.Height(25.0F));
input.otherInput = GUILayout.TextField(input.otherInput, GUILayout.ExpandWidth(true));
if (GUILayout.Button(isMachMode ? Localizer.Format("FAREditorStaticSweepMach") : Localizer.Format("FAREditorStaticSweepAoA"), GUILayout.Width(100.0F), GUILayout.Height(25.0F)))
{
input.lowerBound = Regex.Replace(input.lowerBound, @"[^-?[0-9]*(\.[0-9]*)?]", "");
input.upperBound = Regex.Replace(input.upperBound, @"[^-?[0-9]*(\.[0-9]*)?]", "");
input.numPts = Regex.Replace(input.numPts, @"[^-?[0-9]*(\.[0-9]*)?]", "");
input.pitchSetting = Regex.Replace(input.pitchSetting, @"[^-?[0-9]*(\.[0-9]*)?]", "");
input.otherInput = Regex.Replace(input.otherInput, @"[^-?[0-9]*(\.[0-9]*)?]", "");
double lowerBound, upperBound, numPts, pitchSetting, otherInput;
lowerBound = double.Parse(input.lowerBound);
lowerBound = FARMathUtil.Clamp(lowerBound, -90, 90);
input.lowerBound = lowerBound.ToString();
upperBound = double.Parse(input.upperBound);
upperBound = FARMathUtil.Clamp(upperBound, lowerBound, 90);
input.upperBound = upperBound.ToString();
numPts = double.Parse(input.numPts);
numPts = Math.Ceiling(numPts);
input.numPts = numPts.ToString();
pitchSetting = double.Parse(input.pitchSetting);
pitchSetting = FARMathUtil.Clamp(pitchSetting, -1, 1);
input.pitchSetting = pitchSetting.ToString();
otherInput = double.Parse(input.otherInput);
GraphData data;
var sim = simManager.SweepSim;
if (sim.IsReady())
{
if (isMachMode)
{
data = sim.MachNumberSweep(otherInput, pitchSetting, lowerBound, upperBound, (int)numPts, input.flapSetting, input.spoilers, bodySettingDropdown.ActiveSelection);
SetAngleVectors(pitchSetting, pitchSetting);
}
else
{
data = sim.AngleOfAttackSweep(otherInput, pitchSetting, lowerBound, upperBound, (int)numPts, input.flapSetting, input.spoilers, bodySettingDropdown.ActiveSelection);
SetAngleVectors(lowerBound, upperBound);
}
UpdateGraph(data, isMachMode ? Localizer.Format("FAREditorStaticGraphMach") : Localizer.Format("FAREditorStaticGraphAoA"), Localizer.Format("FAREditorStaticGraphCoeff"), lowerBound, upperBound);
}
}
GUILayout.EndHorizontal();
}
public void Display()
{
//stabDerivHelp = GUILayout.Toggle(stabDerivHelp, "?", ButtonStyle, GUILayout.Width(200));
GUILayout.Label(Localizer.Format("FAREditorStabDerivFlightCond"));
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("FAREditorStabDerivPlanet"));
_bodySettingDropdown.GUIDropDownDisplay();
GUILayout.Label(Localizer.Format("FAREditorStabDerivAlt"));
altitude = GUILayout.TextField(altitude, GUILayout.ExpandWidth(true));
GUILayout.Label(Localizer.Format("FAREditorStabDerivMach"));
machNumber = GUILayout.TextField(machNumber, GUILayout.ExpandWidth(true));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("FAREditorStabDerivFlap"));
_flapSettingDropdown.GUIDropDownDisplay();
GUILayout.Label(Localizer.Format("FAREditorStabDerivSpoiler"));
spoilersDeployed = GUILayout.Toggle(spoilersDeployed, spoilersDeployed ? Localizer.Format("FAREditorStabDerivSDeploy") : Localizer.Format("FAREditorStabDerivSRetract"), GUILayout.Width(100));
GUILayout.EndHorizontal();
if (GUILayout.Button(Localizer.Format("FAREditorStabDerivCalcButton"), 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(new Vector2(0, 0), new Vector2(0, 0), "FARStabDerivError", Localizer.Format("FAREditorStabDerivError"), Localizer.Format("FAREditorStabDerivErrorExp"), Localizer.Format("FARGUIOKButton "), true, HighLogic.UISkin);
}
}
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("FAREditorStabDerivAirProp"), GUILayout.Width(180));
GUILayout.Label(Localizer.Format("FAREditorStabDerivMoI"), GUILayout.Width(160));
GUILayout.Label(Localizer.Format("FAREditorStabDerivPoI"), GUILayout.Width(160));
GUILayout.Label(Localizer.Format("FAREditorStabDerivLvlFl"), GUILayout.Width(140));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.BeginVertical(GUILayout.Width(180));
GUILayout.Label(Localizer.Format("FAREditorStabDerivRefArea") + stabDerivOutput.area.ToString("G3") + " " + Localizer.Format("FARUnitMSq"));
GUILayout.Label(Localizer.Format("FAREditorStabDerivScaledChord") + stabDerivOutput.MAC.ToString("G3") + " " + Localizer.Format("FARUnitM"));
GUILayout.Label(Localizer.Format("FAREditorStabDerivScaledSpan") + stabDerivOutput.b.ToString("G3") + " " + Localizer.Format("FARUnitM"));
GUILayout.EndVertical();
GUILayout.BeginVertical(GUILayout.Width(160));
GUILayout.Label(new GUIContent(Localizer.Format("FAREditorStabDerivIxx") + stabDerivOutput.stabDerivs[0].ToString("G6") + " " + Localizer.Format("FARUnitKgMSq"), Localizer.Format("FAREditorStabDerivIxxExp")));
GUILayout.Label(new GUIContent(Localizer.Format("FAREditorStabDerivIyy") + stabDerivOutput.stabDerivs[1].ToString("G6") + " " + Localizer.Format("FARUnitKgMSq"), Localizer.Format("FAREditorStabDerivIyyExp")));
GUILayout.Label(new GUIContent(Localizer.Format("FAREditorStabDerivIzz") + stabDerivOutput.stabDerivs[2].ToString("G6") + " " + Localizer.Format("FARUnitKgMSq"), Localizer.Format("FAREditorStabDerivIzzExp")));
GUILayout.EndVertical();
GUILayout.BeginVertical(GUILayout.Width(160));
GUILayout.Label(new GUIContent(Localizer.Format("FAREditorStabDerivIxy") + stabDerivOutput.stabDerivs[24].ToString("G6") + " " + Localizer.Format("FARUnitKgMSq"), Localizer.Format("FAREditorStabDerivIxyExp")));
GUILayout.Label(new GUIContent(Localizer.Format("FAREditorStabDerivIyz") + stabDerivOutput.stabDerivs[25].ToString("G6") + " " + Localizer.Format("FARUnitKgMSq"), Localizer.Format("FAREditorStabDerivIyzExp")));
GUILayout.Label(new GUIContent(Localizer.Format("FAREditorStabDerivIxz") + stabDerivOutput.stabDerivs[26].ToString("G6") + " " + Localizer.Format("FARUnitKgMSq"), Localizer.Format("FAREditorStabDerivIxzExp")));
GUILayout.EndVertical();
GUILayout.BeginVertical(GUILayout.Width(140));
GUILayout.Label(new GUIContent(Localizer.Format("FAREditorStabDerivu0") + stabDerivOutput.nominalVelocity.ToString("G6") + " " + Localizer.Format("FARUnitMPerSec"), Localizer.Format("FAREditorStabDerivu0Exp")));
GUILayout.BeginHorizontal();
GUILayout.Label(new GUIContent(Localizer.Format("FARAbbrevCl") + ": " + stabDerivOutput.stableCl.ToString("G3"), Localizer.Format("FAREditorStabDerivClExp")));
GUILayout.Label(new GUIContent(Localizer.Format("FARAbbrevCd") + ": " + stabDerivOutput.stableCd.ToString("G3"), Localizer.Format("FAREditorStabDerivCdExp")));
GUILayout.EndHorizontal();
GUILayout.Label(new GUIContent(Localizer.Format("FARAbbrevAoA") + ": " + stabDerivOutput.stableAoAState + stabDerivOutput.stableAoA.ToString("G6") + " " + Localizer.Format("FARUnitDeg"), Localizer.Format("FAREditorStabDerivAoAExp")));
GUILayout.EndVertical();
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("FAREditorLongDeriv"), 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(Localizer.Format("FAREditorDownVelDeriv"), GUILayout.Width(160));
GUILayout.Label(Localizer.Format("FAREditorFwdVelDeriv"), GUILayout.Width(160));
GUILayout.Label(Localizer.Format("FAREditorPitchRateDeriv"), GUILayout.Width(160));
GUILayout.Label(Localizer.Format("FAREditorPitchCtrlDeriv"), GUILayout.Width(160));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel(Localizer.Format("FAREditorZw"), stabDerivOutput.stabDerivs[3], " " + Localizer.Format("FARUnitInvSec"), Localizer.Format("FAREditorZwExp"), 160, -1);
StabilityLabel(Localizer.Format("FAREditorZu"), stabDerivOutput.stabDerivs[6], " " + Localizer.Format("FARUnitInvSec"), Localizer.Format("FAREditorZuExp"), 160, -1);
StabilityLabel(Localizer.Format("FAREditorZq"), stabDerivOutput.stabDerivs[9], " " + Localizer.Format("FARUnitMPerSec"), Localizer.Format("FAREditorZqExp"), 160, 0);
StabilityLabel(Localizer.Format("FAREditorZDeltae"), stabDerivOutput.stabDerivs[12], " " + Localizer.Format("FARUnitMPerSecSq"), Localizer.Format("FAREditorZDeltaeExp"), 160, 0);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel(Localizer.Format("FAREditorXw"), stabDerivOutput.stabDerivs[4], " " + Localizer.Format("FARUnitInvSec"), Localizer.Format("FAREditorXwExp"), 160, 0);
StabilityLabel(Localizer.Format("FAREditorXu"), stabDerivOutput.stabDerivs[7], " " + Localizer.Format("FARUnitInvSec"), Localizer.Format("FAREditorXuExp"), 160, -1);
StabilityLabel(Localizer.Format("FAREditorXq"), stabDerivOutput.stabDerivs[10], " " + Localizer.Format("FARUnitMPerSec"), Localizer.Format("FAREditorXqExp"), 160, 0);
StabilityLabel(Localizer.Format("FAREditorXDeltae"), stabDerivOutput.stabDerivs[13], " " + Localizer.Format("FARUnitMPerSecSq"), Localizer.Format("FAREditorXDeltaeExp"), 160, 0);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel(Localizer.Format("FAREditorMw"), stabDerivOutput.stabDerivs[5], " " + Localizer.Format("FARUnitInvMSec"), Localizer.Format("FAREditorMwExp"), 160, -1);
StabilityLabel(Localizer.Format("FAREditorMu"), stabDerivOutput.stabDerivs[8], " " + Localizer.Format("FARUnitInvMSec"), Localizer.Format("FAREditorMuExp"), 160, 0);
StabilityLabel(Localizer.Format("FAREditorMq"), stabDerivOutput.stabDerivs[11], " " + Localizer.Format("FARUnitInvSec"), Localizer.Format("FAREditorMqExp"), 160, -1);
StabilityLabel(Localizer.Format("FAREditorMDeltae"), stabDerivOutput.stabDerivs[14], " " + Localizer.Format("FARUnitInvSecSq"), Localizer.Format("FAREditorMDeltaeExp"), 160, 1);
GUILayout.EndHorizontal();
GUILayout.EndVertical();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("FAREditorLatDeriv"), GUILayout.Width(160));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("FAREditorSideslipDeriv"), GUILayout.Width(160));
GUILayout.Label(Localizer.Format("FAREditorRollRateDeriv"), GUILayout.Width(160));
GUILayout.Label(Localizer.Format("FAREditorYawRateDeriv"), GUILayout.Width(160));
GUILayout.EndHorizontal();
GUILayout.BeginVertical(BackgroundStyle);
GUILayout.BeginHorizontal();
StabilityLabel(Localizer.Format("FAREditorYβ"), stabDerivOutput.stabDerivs[15], " " + Localizer.Format("FARUnitMPerSecSq"), Localizer.Format("FAREditorYβExp"), 160, -1);
StabilityLabel(Localizer.Format("FAREditorYp"), stabDerivOutput.stabDerivs[18], " " + Localizer.Format("FARUnitMPerSec"), Localizer.Format("FAREditorYpExp"), 160, 0);
StabilityLabel(Localizer.Format("FAREditorYr"), stabDerivOutput.stabDerivs[21], " " + Localizer.Format("FARUnitMPerSec"), Localizer.Format("FAREditorYrExp"), 160, 1);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel(Localizer.Format("FAREditorLβ"), stabDerivOutput.stabDerivs[16], " " + Localizer.Format("FARUnitInvSecSq"), Localizer.Format("FAREditorLβExp"), 160, -1);
StabilityLabel(Localizer.Format("FAREditorLp"), stabDerivOutput.stabDerivs[19], " " + Localizer.Format("FARUnitInvSec"), Localizer.Format("FAREditorLpExp"), 160, -1);
StabilityLabel(Localizer.Format("FAREditorLr"), stabDerivOutput.stabDerivs[22], " " + Localizer.Format("FARUnitInvSec"), Localizer.Format("FAREditorLrExp"), 160, 1);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
StabilityLabel(Localizer.Format("FAREditorNβ"), stabDerivOutput.stabDerivs[17], " " + Localizer.Format("FARUnitInvSecSq"), Localizer.Format("FAREditorNβExp"), 160, 1);
StabilityLabel(Localizer.Format("FAREditorNp"), stabDerivOutput.stabDerivs[20], " " + Localizer.Format("FARUnitInvSec"), Localizer.Format("FAREditorNpExp"), 160, 0);
StabilityLabel(Localizer.Format("FAREditorNr"), stabDerivOutput.stabDerivs[23], " " + Localizer.Format("FARUnitInvSec"), Localizer.Format("FAREditorNrExp"), 160, -1);
GUILayout.EndHorizontal();
GUILayout.EndVertical();
DrawTooltip();
}
private void CalculateAeroForces(float atmDensity, float machNumber, float reynoldsPerUnitLength, float pseudoKnudsenNumber, float skinFrictionDrag,
IForceContext forceContext)
{
double skinFrictionForce = skinFrictionDrag * xForceSkinFriction.Evaluate(machNumber); //this will be the same for each part, so why recalc it multiple times?
double xForceAoA0 = xForcePressureAoA0.Evaluate(machNumber);
double xForceAoA180 = xForcePressureAoA180.Evaluate(machNumber);
for (int i = 0; i < partData.Count; i++)
{
PartData data = partData[i];
FARAeroPartModule aeroModule = data.aeroModule;
if ((object)aeroModule == null)
{
continue;
}
Vector3 xRefVector = data.xRefVectorPartSpace;
Vector3 nRefVector = data.nRefVectorPartSpace;
Vector3 velLocal = forceContext.LocalVelocity(data);
// Rejects both negligable speed and invalid simulation cases
if (FARMathUtil.NearlyEqual(velLocal.sqrMagnitude, 0.0f))
{
continue;
}
Vector3 angVelLocal = aeroModule.partLocalAngVel;
velLocal += Vector3.Cross(data.centroidPartSpace, angVelLocal); //some transform issue here, needs investigation
Vector3 velLocalNorm = velLocal.normalized;
Vector3 localNormalForceVec = Vector3.ProjectOnPlane(-velLocalNorm, xRefVector).normalized;
double cosAoA = Vector3.Dot(xRefVector, velLocalNorm);
double cosSqrAoA = cosAoA * cosAoA;
double sinSqrAoA = Math.Max(1 - cosSqrAoA, 0);
double sinAoA = Math.Sqrt(sinSqrAoA);
double sin2AoA = 2 * sinAoA * Math.Abs(cosAoA);
double cosHalfAoA = Math.Sqrt(0.5 + 0.5 * Math.Abs(cosAoA));
double nForce = 0;
nForce = potentialFlowNormalForce * Math.Sign(cosAoA) * cosHalfAoA * sin2AoA; //potential flow normal force
if (nForce < 0) //potential flow is not significant over the rear face of things
{
nForce = 0;
}
//if (machNumber > 3)
// nForce *= 2d - machNumber * 0.3333333333333333d;
float normalForceFactor = Math.Abs(Vector3.Dot(localNormalForceVec, nRefVector));
normalForceFactor *= normalForceFactor;
normalForceFactor = invFlatnessRatio * (1 - normalForceFactor) + flatnessRatio * normalForceFactor; //accounts for changes in relative flatness of shape
float crossFlowMach, crossFlowReynolds;
crossFlowMach = machNumber * (float)sinAoA;
crossFlowReynolds = reynoldsPerUnitLength * diameter * (float)sinAoA / normalForceFactor;
nForce += viscCrossflowDrag * sinSqrAoA * CalculateCrossFlowDrag(crossFlowMach, crossFlowReynolds); //viscous crossflow normal force
nForce *= normalForceFactor;
double xForce = -skinFrictionForce *Math.Sign(cosAoA) * cosSqrAoA;
double localVelForce = xForce * pseudoKnudsenNumber;
xForce -= localVelForce;
localVelForce = Math.Abs(localVelForce);
float moment = (float)(cosAoA * sinAoA);
float dampingMoment = 4f * moment;
if (cosAoA > 0)
{
xForce += cosSqrAoA * xForceAoA0;
float momentFactor;
if (machNumber > 6)
{
momentFactor = hypersonicMomentForward;
}
else if (machNumber < 0.6)
{
momentFactor = 0.6f * hypersonicMomentBackward;
}
else
{
float tmp = (-0.185185185f * machNumber + 1.11111111111f);
momentFactor = tmp * hypersonicMomentBackward * 0.6f + (1 - tmp) * hypersonicMomentForward;
}
//if (machNumber < 1.5)
// momentFactor += hypersonicMomentBackward * (0.5f - machNumber * 0.33333333333333333333333333333333f) * 0.2f;
moment *= momentFactor;
dampingMoment *= momentFactor;
}
else
{
xForce += cosSqrAoA * xForceAoA180;
float momentFactor; //negative to deal with the ref vector facing the opposite direction, causing the moment vector to point in the opposite direction
if (machNumber > 6)
{
momentFactor = hypersonicMomentBackward;
}
else if (machNumber < 0.6)
{
momentFactor = 0.6f * hypersonicMomentForward;
}
else
{
float tmp = (-0.185185185f * machNumber + 1.11111111111f);
momentFactor = tmp * hypersonicMomentForward * 0.6f + (1 - tmp) * hypersonicMomentBackward;
}
//if (machNumber < 1.5)
// momentFactor += hypersonicMomentForward * (0.5f - machNumber * 0.33333333333333333333333333333333f) * 0.2f;
moment *= momentFactor;
dampingMoment *= momentFactor;
}
moment /= normalForceFactor;
dampingMoment = Math.Abs(dampingMoment) * 0.1f;
//dampingMoment += (float)Math.Abs(skinFrictionForce) * 0.1f;
float rollDampingMoment = (float)(skinFrictionForce * 0.5 * diameter); //skin friction force times avg moment arm for vehicle
rollDampingMoment *= (0.75f + flatnessRatio * 0.25f); //this is just an approximation for now
Vector3 forceVector = (float)xForce * xRefVector + (float)nForce * localNormalForceVec;
forceVector -= (float)localVelForce * velLocalNorm;
Vector3 torqueVector = Vector3.Cross(xRefVector, localNormalForceVec) * moment;
Vector3 axialAngLocalVel = Vector3.Dot(xRefVector, angVelLocal) * xRefVector;
Vector3 nonAxialAngLocalVel = angVelLocal - axialAngLocalVel;
if (velLocal.sqrMagnitude > 0.001f)
{
torqueVector -= (dampingMoment * nonAxialAngLocalVel) + (rollDampingMoment * axialAngLocalVel * axialAngLocalVel.magnitude) / velLocal.sqrMagnitude;
}
else
{
torqueVector -= (dampingMoment * nonAxialAngLocalVel) + (rollDampingMoment * axialAngLocalVel * axialAngLocalVel.magnitude) / 0.001f;
}
//float dynPresAndScaling = 0.0005f * atmDensity * velLocal.sqrMagnitude * data.dragFactor; //dyn pres and N -> kN conversion
//forceVector *= dynPresAndScaling;
//torqueVector *= dynPresAndScaling;
forceVector *= data.dragFactor;
torqueVector *= data.dragFactor;
forceContext.ApplyForce(data, velLocal, forceVector, torqueVector);
}
}
public void IterateOnce(double alpha0, double pitch0, double alpha1, double pitch1, out double alpha2, out double pitch2)
{
iterationInput.alpha = alpha1;
iterationInput.pitchValue = pitch0;
parent.GetClCdCmSteady(iterationInput, out iterationOutput, true, true);
double Cl10 = iterationOutput.Cl;
double Cm10 = iterationOutput.Cm;
iterationInput.alpha = alpha0;
iterationInput.pitchValue = pitch1;
parent.GetClCdCmSteady(iterationInput, out iterationOutput, true, true);
double Cl01 = iterationOutput.Cl;
double Cm01 = iterationOutput.Cm;
iterationInput.alpha = alpha1;
parent.GetClCdCmSteady(iterationInput, out iterationOutput, true, true);
double Cl11 = iterationOutput.Cl;
double Cm11 = iterationOutput.Cm;
// Assume the solution did not yet converge
double clgrad = (Cl11 - Cl01) / (alpha1 - alpha0);
double cmcrss = (Cm11 - Cm01) / (alpha1 - alpha0);
double cmgrad = (Cm11 - Cm10) / (pitch1 - pitch0);
double clcrss = (Cl11 - Cl10) / (pitch1 - pitch0);
double gradientscale = clgrad * cmgrad - clcrss * cmcrss;
double cl = neededCl - Cl11; // for improved readability
double cm = -Cm11;
const double gradeps = 2E-5; // a gradient close to zero can be ignored
double deltaalpha, deltapitch;
if (Math.Abs(clgrad) > gradeps && Math.Abs(cmgrad) > gradeps &&
Math.Abs(gradientscale) > gradeps * gradeps)
{
deltaalpha = (cmgrad * cl - clcrss * cm) / gradientscale;
deltapitch = (clgrad * cm - cmcrss * cl) / gradientscale;
}
else // update just the variable that have a non-zero gradient
{
if (Math.Abs(clgrad) > gradeps)
{
deltaalpha = cl / clgrad;
}
else
{
deltaalpha = 0;
}
if (Math.Abs(cmgrad) > gradeps)
{
deltapitch = cm / cmgrad;
}
else
{
deltapitch = 0;
}
}
double maxdeltaalpha = Math.Abs(alpha1 - alpha0);
double maxdeltapitch = Math.Abs(pitch1 - pitch0);
deltaalpha = FARMathUtil.Clamp(deltaalpha, -maxdeltaalpha, +maxdeltaalpha);
deltapitch = FARMathUtil.Clamp(deltapitch, -maxdeltapitch, +maxdeltapitch);
alpha2 = FARMathUtil.Clamp(alpha1 + deltaalpha, alphatol.leftedge, alphatol.rightedge);
pitch2 = FARMathUtil.Clamp(pitch1 + deltapitch, pitchtol.leftedge, pitchtol.rightedge);
}
private void partModuleUpdate(PartModule pm)
{
if (isFarLoaded && pm is FARControllableSurface)
{
FARControllableSurface fcs = (FARControllableSurface)pm;
if (vessel.atmDensity > 0)
{
Vector3d forcePosition = fcs.AerodynamicCenter - vesselState.CoM;
Vector3 velocity = fcs.GetVelocity();
double soundspeed, v_scalar = velocity.magnitude;
double rho = FARAeroUtil.GetCurrentDensity(vessel, out soundspeed);
if (rho <= 0.0 || v_scalar <= 0.1 || fcs.isShielded)
{
return;
}
// First we save the curent state of the part
double YmaxForce = fcs.YmaxForce;
double XZmaxForce = fcs.XZmaxForce;
double AoAcurrentFlap = (double)(FieldAoAcurrentFlap.GetValue(fcs));
double MaxAoAdesiredControl = 0;
if (fcs.pitchaxis != 0.0)
{
MaxAoAdesiredControl += (double)(FieldPitchLocation.GetValue(fcs)) * fcs.pitchaxis * 0.01;
}
if (fcs.yawaxis != 0.0)
{
MaxAoAdesiredControl += (double)(FieldYawLocation.GetValue(fcs)) * fcs.yawaxis * 0.01;;
}
if (fcs.rollaxis != 0.0)
{
MaxAoAdesiredControl += (double)(FieldRollLocation.GetValue(fcs)) * fcs.rollaxis * 0.01;;
}
MaxAoAdesiredControl *= fcs.maxdeflect;
if (fcs.pitchaxisDueToAoA != 0.0)
{
double _AoA = (fcs as FARWingAerodynamicModel).CalculateAoA(velocity.normalized);
_AoA = FARMathUtil.rad2deg * Math.Asin(_AoA);
if (double.IsNaN(_AoA))
{
_AoA = 0;
}
MaxAoAdesiredControl += _AoA * fcs.pitchaxisDueToAoA * 0.01;
}
MaxAoAdesiredControl = FARMathUtil.Clamp(MaxAoAdesiredControl, -Math.Abs(fcs.maxdeflect), Math.Abs(fcs.maxdeflect));
double MachNumber = v_scalar / soundspeed;
fcs.YmaxForce = double.MaxValue;
fcs.XZmaxForce = double.MaxValue;
// Then we turn it one way
double AoA = fcs.CalculateAoA(velocity, AoAcurrentFlap + MaxAoAdesiredControl);
vesselState.ctrlTorqueAvailable.Add(vessel.GetTransform().InverseTransformDirection(Vector3.Cross(forcePosition, fcs.CalculateForces(velocity, MachNumber, AoA))));
// We restore it to the initial state
AoA = fcs.CalculateAoA(velocity);
fcs.CalculateForces(velocity, MachNumber, AoA);
// And we turn it the other way
AoA = fcs.CalculateAoA(velocity, AoAcurrentFlap - MaxAoAdesiredControl);
vesselState.ctrlTorqueAvailable.Add(vessel.GetTransform().InverseTransformDirection(Vector3.Cross(forcePosition, fcs.CalculateForces(velocity, MachNumber, AoA))));
// And in the end we restore its initial state
AoA = fcs.CalculateAoA(velocity);
fcs.CalculateForces(velocity, MachNumber, AoA);
fcs.YmaxForce = YmaxForce;
fcs.XZmaxForce = XZmaxForce;
}
}
}
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);
}
private void OnAutoPilotUpdate(FlightCtrlState state)
{
if (_vessel.srfSpeed < 5)
{
return;
}
ControlSystem sys = systemInstances[0]; //wing leveler
if (sys.active)
{
double phi = info.rollAngle - sys.zeroPoint;
if (sys.kP < 0)
{
phi += 180;
if (phi > 180)
{
phi -= 360;
}
}
else
{
phi = -phi;
}
phi *= -FARMathUtil.deg2rad;
double output = ControlStateChange(sys, phi);
if (Math.Abs(state.roll - state.rollTrim) < 0.01)
{
if (output > 1)
{
output = 1;
}
else if (output < -1)
{
output = -1;
}
state.roll = (float)output + state.rollTrim;
}
}
else
{
sys.errorIntegral = 0;
}
sys = systemInstances[1];
if (sys.active)
{
double beta = -(info.sideslipAngle - sys.zeroPoint) * FARMathUtil.deg2rad;
double output = ControlStateChange(sys, beta);
if (Math.Abs(state.yaw - state.yawTrim) < 0.01)
{
if (output > 1)
{
output = 1;
}
else if (output < -1)
{
output = -1;
}
state.yaw = (float)output + state.yawTrim;
}
}
else
{
sys.errorIntegral = 0;
}
sys = systemInstances[2];
if (sys.active)
{
double pitch = (info.aoA - sys.zeroPoint) * FARMathUtil.deg2rad;
double output = ControlStateChange(sys, pitch);
if (Math.Abs(state.pitch - state.pitchTrim) < 0.01)
{
if (output > 1)
{
output = 1;
}
else if (output < -1)
{
output = -1;
}
state.pitch = (float)output + state.pitchTrim;
}
}
else
{
sys.errorIntegral = 0;
}
sys = systemInstances[3];
if (sys.active)
{
if (info.aoA > aoAHighLim)
{
state.pitch = (float)FARMathUtil.Clamp(ControlStateChange(sys, info.aoA - aoAHighLim), -1, 1) + state.pitchTrim;
}
else if (info.aoA < aoALowLim)
{
state.pitch = (float)FARMathUtil.Clamp(ControlStateChange(sys, info.aoA - aoALowLim), -1, 1) + state.pitchTrim;
}
}
else
{
sys.errorIntegral = 0;
}
sys = systemInstances[4];
if (sys.active)
{
double scalingFactor = scalingDynPres / info.dynPres;
if (scalingFactor > 1)
{
scalingFactor = 1;
}
state.pitch = state.pitchTrim + (state.pitch - state.pitchTrim) * (float)scalingFactor;
state.yaw = state.yawTrim + (state.yaw - state.yawTrim) * (float)scalingFactor;
state.roll = state.rollTrim + (state.roll - state.rollTrim) * (float)scalingFactor;
}
}
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);
}
