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