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