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); }