public InstantConditionSimVars(InstantConditionSim parent, CelestialBody body, double altitude, double machNumber, double neededCl, double beta, double phi, int flap, bool spoilers) { this.parent = parent; this.neededCl = neededCl; this.CoM = parent.GetCoM(); FlightEnv fltenv = FlightEnv.NewSim(body, altitude, machNumber); iterationInput = new InstantConditionSimInput(0, beta, phi, 0, 0, 0, fltenv, 0, flap, spoilers); iterationOutput = new InstantConditionSimOutput(); }
public void GetClCdCmSteady(InstantConditionSimInput input, out InstantConditionSimOutput output, bool clear, bool reset_stall = false) { output = new InstantConditionSimOutput(); double area = 0; double MAC = 0; double b_2 = 0; Vector3d forward = Vector3.forward; Vector3d up = Vector3.up; Vector3d right = Vector3.right; Vector3d CoM = Vector3d.zero; double mass = 0; 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; } CoM /= mass; if (EditorDriver.editorFacility == EditorFacility.VAB) { forward = Vector3.up; up = -Vector3.forward; } double sinAlpha = Math.Sin(input.alpha * Math.PI / 180); double cosAlpha = Math.Sqrt(Math.Max(1 - sinAlpha * sinAlpha, 0)); double sinBeta = Math.Sin(input.beta * Math.PI / 180); double cosBeta = Math.Sqrt(Math.Max(1 - sinBeta * sinBeta, 0)); double sinPhi = Math.Sin(input.phi * Math.PI / 180); double cosPhi = Math.Sqrt(Math.Max(1 - sinPhi * sinPhi, 0)); double alphaDot = input.alphaDot * Math.PI / 180; double betaDot = input.betaDot * Math.PI / 180; double phiDot = input.phiDot * Math.PI / 180; Vector3d AngVel = (phiDot - sinAlpha * betaDot) * forward; AngVel += (cosPhi * alphaDot + cosAlpha * sinPhi * betaDot) * right; AngVel += (sinPhi * alphaDot - cosAlpha * cosPhi * betaDot) * up; Vector3d velocity = forward * cosAlpha * cosBeta; velocity += right * (sinPhi * cosAlpha * cosBeta + cosPhi * sinBeta); velocity += -up * cosPhi * (sinAlpha * cosBeta + sinBeta); velocity.Normalize(); //this is negative wrt the ground Vector3d liftVector = -forward * sinAlpha + right * sinPhi * cosAlpha - up * cosPhi * cosAlpha; Vector3d sideways = Vector3.Cross(velocity, liftVector).normalized; for (int i = 0; i < _wingAerodynamicModel.Count; i++) { FARWingAerodynamicModel w = _wingAerodynamicModel[i]; if (!(w && w.part)) { continue; } w.ComputeForceEditor(velocity.normalized, input.machNumber, 2); if (clear) { w.EditorClClear(reset_stall); } Vector3d relPos = w.GetAerodynamicCenter() - CoM; Vector3d vel = velocity + Vector3d.Cross(AngVel, relPos); if (w is FARControllableSurface) { (w as FARControllableSurface).SetControlStateEditor(CoM, vel, (float)input.pitchValue, 0, 0, input.flaps, input.spoilers); } else if (w.isShielded) { continue; } //w.ComputeForceEditor(velocity, input.machNumber); //do this just to get the AC right Vector3d force = w.ComputeForceEditor(vel.normalized, input.machNumber, 2) * 1000; output.Cl += -Vector3d.Dot(force, liftVector); output.Cy += Vector3d.Dot(force, sideways); output.Cd += -Vector3d.Dot(force, velocity); Vector3d moment = -Vector3d.Cross(relPos, force); output.Cm += Vector3d.Dot(moment, sideways); output.Cn += Vector3d.Dot(moment, liftVector); output.C_roll += Vector3d.Dot(moment, velocity); //w.ComputeClCdEditor(vel.normalized, input.machNumber); /*double tmpCl = w.GetCl() * w.S; * output.Cl += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), liftVector); * output.Cy += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), sideways); * double tmpCd = w.GetCd() * w.S; * output.Cd += tmpCd; * output.Cm += tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), liftVector) + tmpCd * -Vector3d.Dot((relPos), liftVector); * output.Cn += tmpCd * Vector3d.Dot((relPos), sideways) + tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), sideways); * output.C_roll += tmpCl * Vector3d.Dot((relPos), sideways) * -Vector3d.Dot(w.GetLiftDirection(), liftVector);*/ area += w.S; MAC += w.GetMAC() * w.S; b_2 += w.Getb_2() * w.S; } FARCenterQuery center = new FARCenterQuery(); for (int i = 0; i < _currentAeroSections.Count; i++) { _currentAeroSections[i].PredictionCalculateAeroForces(2, (float)input.machNumber, 10000, 0, 0.005f, velocity.normalized, center); } Vector3d centerForce = center.force * 1000; output.Cl += -Vector3d.Dot(centerForce, liftVector); output.Cy += Vector3d.Dot(centerForce, sideways); output.Cd += -Vector3d.Dot(centerForce, velocity); Vector3d centerMoment = -center.TorqueAt(CoM) * 1000; output.Cm += Vector3d.Dot(centerMoment, sideways); output.Cn += Vector3d.Dot(centerMoment, liftVector); output.C_roll += Vector3d.Dot(centerMoment, velocity); /*for (int i = 0; i < FARAeroUtil.CurEditorParts.Count; i++) * { * Part p = FARAeroUtil.CurEditorParts[i]; * if (FARAeroUtil.IsNonphysical(p)) * continue; * * Vector3 part_pos = p.transform.TransformPoint(p.CoMOffset) - CoM; * double partMass = p.mass; * if (p.Resources.Count > 0) * partMass += p.GetResourceMass(); * * double stock_drag = partMass * p.maximum_drag * FlightGlobals.DragMultiplier * 1000; * output.Cd += stock_drag; * output.Cm += stock_drag * -Vector3d.Dot(part_pos, liftVector); * output.Cn += stock_drag * Vector3d.Dot(part_pos, sideways); * }*/ if (area == 0) { area = _maxCrossSectionFromBody; b_2 = 1; MAC = _bodyLength; } double recipArea = 1 / area; MAC *= recipArea; b_2 *= recipArea; output.Cl *= recipArea; output.Cd *= recipArea; output.Cm *= recipArea / MAC; output.Cy *= recipArea; output.Cn *= recipArea / b_2; output.C_roll *= recipArea / b_2; }
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 CalculateStabilityDerivs(CelestialBody body, double alt, double machNumber, int flapSetting, bool spoilers, double alpha, double beta, double phi) { double pressure = body.GetPressure(alt); double temperature = body.GetTemperature(alt); double density = body.GetDensity(pressure, temperature); double sspeed = body.GetSpeedOfSound(pressure, density); double u0 = sspeed * machNumber; double q = u0 * u0 * density * 0.5f; StabilityDerivOutput stabDerivOutput = new StabilityDerivOutput(); StabilityDerivExportVariables stabDerivExport = new StabilityDerivExportVariables(); stabDerivOutput.nominalVelocity = u0; stabDerivOutput.altitude = alt; stabDerivOutput.body = body; Vector3d CoM = Vector3d.zero; double mass = 0; double MAC = 0; double b = 0; double area = 0; double Ix = 0; double Iy = 0; double Iz = 0; double Ixy = 0; double Iyz = 0; double Ixz = 0; InstantConditionSimInput input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers); InstantConditionSimOutput nominalOutput; InstantConditionSimOutput pertOutput = new InstantConditionSimOutput(); _instantCondition.GetClCdCmSteady(input, out nominalOutput, true); List <Part> partsList = EditorLogic.SortedShipList; for (int i = 0; i < partsList.Count; i++) { Part p = partsList[i]; if (FARAeroUtil.IsNonphysical(p)) { continue; } double partMass = p.mass; if (p.Resources.Count > 0) { partMass += p.GetResourceMass(); } //partMass += p.GetModuleMass(p.mass); // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass CoM += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset); mass += partMass; FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>(); if (w != null) { if (w.isShielded) { continue; } area += w.S; MAC += w.GetMAC() * w.S; b += w.Getb_2() * w.S; if (w is FARControllableSurface) { (w as FARControllableSurface).SetControlStateEditor(CoM, p.transform.up, 0, 0, 0, input.flaps, input.spoilers); } } } if (area.NearlyEqual(0)) { area = _instantCondition._maxCrossSectionFromBody; MAC = _instantCondition._bodyLength; b = 1; } MAC /= area; b /= area; CoM /= mass; mass *= 1000; stabDerivOutput.b = b; stabDerivOutput.MAC = MAC; stabDerivOutput.area = area; for (int i = 0; i < partsList.Count; i++) { Part p = partsList[i]; if (p == null || FARAeroUtil.IsNonphysical(p)) { continue; } //This section handles the parallel axis theorem Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM; double x2, y2, z2, x, y, z; x2 = relPos.z * relPos.z; y2 = relPos.x * relPos.x; z2 = relPos.y * relPos.y; x = relPos.z; y = relPos.x; z = relPos.y; double partMass = p.mass; if (p.Resources.Count > 0) { partMass += p.GetResourceMass(); } //partMass += p.GetModuleMass(p.mass); // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass Ix += (y2 + z2) * partMass; Iy += (x2 + z2) * partMass; Iz += (x2 + y2) * partMass; Ixy += -x * y * partMass; Iyz += -z * y * partMass; Ixz += -x * z * partMass; //And this handles the part's own moment of inertia Vector3 principalInertia = p.Rigidbody.inertiaTensor; Quaternion prncInertRot = p.Rigidbody.inertiaTensorRotation; //The rows of the direction cosine matrix for a quaternion Vector3 Row1 = new Vector3(prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w, 2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w), 2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w)); Vector3 Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w), -prncInertRot.x * prncInertRot.x + prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w, 2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w)); Vector3 Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w), 2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w), -prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y + prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w); //And converting the principal moments of inertia into the coordinate system used by the system Ix += principalInertia.x * Row1.x * Row1.x + principalInertia.y * Row1.y * Row1.y + principalInertia.z * Row1.z * Row1.z; Iy += principalInertia.x * Row2.x * Row2.x + principalInertia.y * Row2.y * Row2.y + principalInertia.z * Row2.z * Row2.z; Iz += principalInertia.x * Row3.x * Row3.x + principalInertia.y * Row3.y * Row3.y + principalInertia.z * Row3.z * Row3.z; Ixy += principalInertia.x * Row1.x * Row2.x + principalInertia.y * Row1.y * Row2.y + principalInertia.z * Row1.z * Row2.z; Ixz += principalInertia.x * Row1.x * Row3.x + principalInertia.y * Row1.y * Row3.y + principalInertia.z * Row1.z * Row3.z; Iyz += principalInertia.x * Row2.x * Row3.x + principalInertia.y * Row2.y * Row3.y + principalInertia.z * Row2.z * Row3.z; } Ix *= 1000; Iy *= 1000; Iz *= 1000; stabDerivOutput.stabDerivs[0] = Ix; stabDerivOutput.stabDerivs[1] = Iy; stabDerivOutput.stabDerivs[2] = Iz; stabDerivOutput.stabDerivs[24] = Ixy; stabDerivOutput.stabDerivs[25] = Iyz; stabDerivOutput.stabDerivs[26] = Ixz; double effectiveG = _instantCondition.CalculateAccelerationDueToGravity(body, alt); //This is the effect of gravity effectiveG -= u0 * u0 / (alt + body.Radius); //This is the effective reduction of gravity due to high velocity double neededCl = mass * effectiveG / (q * area); _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true); //Longitudinal Mess _instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers); alpha = FARMathUtil.SelectedSearchMethod(machNumber, _instantCondition.FunctionIterateForAlpha); input.alpha = alpha; nominalOutput = _instantCondition.iterationOutput; //alpha_str = (alpha * Mathf.PI / 180).ToString(); input.alpha = (alpha + 2); _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true); stabDerivOutput.stableCl = neededCl; stabDerivOutput.stableCd = nominalOutput.Cd; stabDerivOutput.stableAoA = alpha; stabDerivOutput.stableAoAState = ""; if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1) { stabDerivOutput.stableAoAState = ((nominalOutput.Cl > neededCl) ? "<" : ">"); } FARLogger.Info("Cl needed: " + neededCl + ", AoA: " + alpha + ", Cl: " + nominalOutput.Cl + ", Cd: " + nominalOutput.Cd); pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad); //vert vel derivs pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad); pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad); pertOutput.Cl += nominalOutput.Cd; pertOutput.Cd -= nominalOutput.Cl; pertOutput.Cl *= -q * area / (mass * u0); pertOutput.Cd *= -q * area / (mass * u0); pertOutput.Cm *= q * area * MAC / (Iy * u0); stabDerivOutput.stabDerivs[3] = pertOutput.Cl; //Zw stabDerivOutput.stabDerivs[4] = pertOutput.Cd; //Xw stabDerivOutput.stabDerivs[5] = pertOutput.Cm; //Mw // Rodhern: The motivation for the revised stability derivatives sign interpretations of Zq, Xq, Ze and Xe // is to align the sign conventions used for Zu, Zq, Ze, Xu, Xq and Xe. Further explanation can be found // here: https://forum.kerbalspaceprogram.com/index.php?/topic/109098-official-far-craft-repository/&do=findComment&comment=2425057 input.alpha = alpha; input.machNumber = machNumber + 0.05; _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber; //fwd vel derivs pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05 * machNumber; pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05 * machNumber; pertOutput.Cl += 2 * nominalOutput.Cl; pertOutput.Cd += 2 * nominalOutput.Cd; pertOutput.Cl *= -q * area / (mass * u0); pertOutput.Cd *= -q * area / (mass * u0); pertOutput.Cm *= q * area * MAC / (u0 * Iy); stabDerivOutput.stabDerivs[6] = pertOutput.Cl; //Zu stabDerivOutput.stabDerivs[7] = pertOutput.Cd; //Xu stabDerivOutput.stabDerivs[8] = pertOutput.Cm; //Mu input.machNumber = machNumber; _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true); input.alphaDot = -0.05; _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05; //pitch rate derivs pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05; pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05; pertOutput.Cl *= -q * area * MAC / (2 * u0 * mass); // Rodhern: Replaced 'q' by '-q', so that formulas pertOutput.Cd *= -q * area * MAC / (2 * u0 * mass); // for Zq and Xq match those for Zu and Xu. pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy); stabDerivOutput.stabDerivs[9] = pertOutput.Cl; //Zq stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq input.alphaDot = 0; input.pitchValue = 0.1; _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1; //elevator derivs pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1; pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1; pertOutput.Cl *= -q * area / mass; // Rodhern: Replaced 'q' by '-q', so that formulas pertOutput.Cd *= -q * area / mass; // for Ze and Xe match those for Zu and Xu. pertOutput.Cm *= q * area * MAC / Iy; stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me //Lateral Mess input.pitchValue = 0; input.beta = (beta + 2); _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad); //sideslip angle derivs pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / (2 * FARMathUtil.deg2rad); pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (2 * FARMathUtil.deg2rad); pertOutput.Cy *= q * area / mass; pertOutput.Cn *= q * area * b / Iz; pertOutput.C_roll *= q * area * b / Ix; stabDerivOutput.stabDerivs[15] = pertOutput.Cy; //Yb stabDerivOutput.stabDerivs[17] = pertOutput.Cn; //Nb stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb input.beta = beta; _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true); input.phiDot = -0.05; _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05; //roll rate derivs pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05; pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05; pertOutput.Cy *= q * area * b / (2 * mass * u0); pertOutput.Cn *= q * area * b * b / (2 * Iz * u0); pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0); stabDerivOutput.stabDerivs[18] = pertOutput.Cy; //Yp stabDerivOutput.stabDerivs[20] = pertOutput.Cn; //Np stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp input.phiDot = 0; _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true); input.betaDot = -0.05; _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f; //yaw rate derivs pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05f; pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f; pertOutput.Cy *= q * area * b / (2 * mass * u0); pertOutput.Cn *= q * area * b * b / (2 * Iz * u0); pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0); stabDerivOutput.stabDerivs[21] = pertOutput.Cy; //Yr stabDerivOutput.stabDerivs[23] = pertOutput.Cn; //Nr stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr // Assign values to export variables stabDerivExport.craftmass = mass; stabDerivExport.envpressure = pressure; stabDerivExport.envtemperature = temperature; stabDerivExport.envdensity = density; stabDerivExport.envsoundspeed = sspeed; stabDerivExport.envg = _instantCondition.CalculateAccelerationDueToGravity(body, alt); stabDerivExport.sitmach = machNumber; stabDerivExport.sitdynpres = q; stabDerivExport.siteffg = effectiveG; return(new StabilityDerivExportOutput(stabDerivOutput, stabDerivExport)); }
public void GetClCdCmSteady(InstantConditionSimInput input, out InstantConditionSimOutput output, bool clear, bool reset_stall = false) { output = new InstantConditionSimOutput(); double area = 0; double MAC = 0; double b_2 = 0; Vector3d forward = Vector3.forward; Vector3d up = Vector3.up; Vector3d right = Vector3.right; Vector3d CoM = Vector3d.zero; double mass = 0; 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; } CoM /= mass; if (EditorDriver.editorFacility == EditorFacility.VAB) { forward = Vector3.up; up = -Vector3.forward; } double sinAlpha = Math.Sin(input.alpha * Math.PI / 180); double cosAlpha = Math.Sqrt(Math.Max(1 - sinAlpha * sinAlpha, 0)); double sinBeta = Math.Sin(input.beta * Math.PI / 180); double cosBeta = Math.Sqrt(Math.Max(1 - sinBeta * sinBeta, 0)); double sinPhi = Math.Sin(input.phi * Math.PI / 180); double cosPhi = Math.Sqrt(Math.Max(1 - sinPhi * sinPhi, 0)); double alphaDot = input.alphaDot * Math.PI / 180; double betaDot = input.betaDot * Math.PI / 180; double phiDot = input.phiDot * Math.PI / 180; Vector3d AngVel = (phiDot - sinAlpha * betaDot) * forward; AngVel += (cosPhi * alphaDot + cosAlpha * sinPhi * betaDot) * right; AngVel += (sinPhi * alphaDot - cosAlpha * cosPhi * betaDot) * up; Vector3d velocity = forward * cosAlpha * cosBeta; velocity += right * (sinPhi * cosAlpha * cosBeta + cosPhi * sinBeta); velocity += -up * cosPhi * (sinAlpha * cosBeta + sinBeta); velocity.Normalize(); //this is negative wrt the ground Vector3d liftVector = -forward * sinAlpha + right * sinPhi * cosAlpha - up * cosPhi * cosAlpha; Vector3d sideways = Vector3.Cross(velocity, liftVector).normalized; for (int i = 0; i < _wingAerodynamicModel.Count; i++) { FARWingAerodynamicModel w = _wingAerodynamicModel[i]; if (!(w && w.part)) continue; w.ComputeForceEditor(velocity.normalized, input.machNumber, 2); if (clear) w.EditorClClear(reset_stall); Vector3d relPos = w.GetAerodynamicCenter() - CoM; Vector3d vel = velocity + Vector3d.Cross(AngVel, relPos); if (w is FARControllableSurface) (w as FARControllableSurface).SetControlStateEditor(CoM, vel, (float)input.pitchValue, 0, 0, input.flaps, input.spoilers); else if (w.isShielded) continue; //w.ComputeForceEditor(velocity, input.machNumber); //do this just to get the AC right Vector3d force = w.ComputeForceEditor(vel.normalized, input.machNumber, 2) * 1000; output.Cl += -Vector3d.Dot(force, liftVector); output.Cy += Vector3d.Dot(force, sideways); output.Cd += -Vector3d.Dot(force, velocity); Vector3d moment = -Vector3d.Cross(relPos, force); output.Cm += Vector3d.Dot(moment, sideways); output.Cn += Vector3d.Dot(moment, liftVector); output.C_roll += Vector3d.Dot(moment, velocity); //w.ComputeClCdEditor(vel.normalized, input.machNumber); /*double tmpCl = w.GetCl() * w.S; output.Cl += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), liftVector); output.Cy += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), sideways); double tmpCd = w.GetCd() * w.S; output.Cd += tmpCd; output.Cm += tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), liftVector) + tmpCd * -Vector3d.Dot((relPos), liftVector); output.Cn += tmpCd * Vector3d.Dot((relPos), sideways) + tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), sideways); output.C_roll += tmpCl * Vector3d.Dot((relPos), sideways) * -Vector3d.Dot(w.GetLiftDirection(), liftVector);*/ area += w.S; MAC += w.GetMAC() * w.S; b_2 += w.Getb_2() * w.S; } FARCenterQuery center = new FARCenterQuery(); for (int i = 0; i < _currentAeroSections.Count; i++) { _currentAeroSections[i].PredictionCalculateAeroForces(2, (float)input.machNumber, 10000, 0.005f, velocity.normalized, center); } Vector3d centerForce = center.force * 1000; output.Cl += -Vector3d.Dot(centerForce, liftVector); output.Cy += Vector3d.Dot(centerForce, sideways); output.Cd += -Vector3d.Dot(centerForce, velocity); Vector3d centerMoment = -center.TorqueAt(CoM) * 1000; output.Cm += Vector3d.Dot(centerMoment, sideways); output.Cn += Vector3d.Dot(centerMoment, liftVector); output.C_roll += Vector3d.Dot(centerMoment, velocity); /*for (int i = 0; i < FARAeroUtil.CurEditorParts.Count; i++) { Part p = FARAeroUtil.CurEditorParts[i]; if (FARAeroUtil.IsNonphysical(p)) continue; Vector3 part_pos = p.transform.TransformPoint(p.CoMOffset) - CoM; double partMass = p.mass; if (p.Resources.Count > 0) partMass += p.GetResourceMass(); double stock_drag = partMass * p.maximum_drag * FlightGlobals.DragMultiplier * 1000; output.Cd += stock_drag; output.Cm += stock_drag * -Vector3d.Dot(part_pos, liftVector); output.Cn += stock_drag * Vector3d.Dot(part_pos, sideways); }*/ if (area == 0) { area = _maxCrossSectionFromBody; b_2 = 1; MAC = _bodyLength; } double recipArea = 1 / area; MAC *= recipArea; b_2 *= recipArea; output.Cl *= recipArea; output.Cd *= recipArea; output.Cm *= recipArea / MAC; output.Cy *= recipArea; output.Cn *= recipArea / b_2; output.C_roll *= recipArea / b_2; }
public void GetClCdCmSteady( InstantConditionSimInput input, out InstantConditionSimOutput output, bool clear, bool reset_stall = false ) { output = new InstantConditionSimOutput(); double area = 0; double MAC = 0; double b_2 = 0; Vector3d forward = Vector3.forward; Vector3d up = Vector3.up; Vector3d right = Vector3.right; Vector3d CoM = Vector3d.zero; if (EditorDriver.editorFacility == EditorFacility.VAB) { forward = Vector3.up; up = -Vector3.forward; } double mass = 0; 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; } CoM /= mass; // Rodhern: The original reference directions (velocity, liftVector, sideways) did not form an orthonormal // basis. That in turn produced some counterintuitive calculation results, such as coupled yaw and pitch // derivatives. A more thorough discussion of the topic can be found on the KSP forums: // https://forum.kerbalspaceprogram.com/index.php?/topic/19321-131-ferram-aerospace-research-v01591-liepmann-4218/&do=findComment&comment=2781270 // The reference directions have been replaced by new ones that are orthonormal by construction. // In dkavolis branch Vector3.Cross() and Vector3d.Normalize() are used explicitly. There is no apparent // benefit to this other than possibly improved readability. double sinAlpha = Math.Sin(input.alpha * Math.PI / 180); double cosAlpha = Math.Sqrt(Math.Max(1 - sinAlpha * sinAlpha, 0)); double sinBeta = Math.Sin(input.beta * Math.PI / 180); double cosBeta = Math.Sqrt(Math.Max(1 - sinBeta * sinBeta, 0)); double sinPhi = Math.Sin(input.phi * Math.PI / 180); double cosPhi = Math.Sqrt(Math.Max(1 - sinPhi * sinPhi, 0)); double alphaDot = input.alphaDot * Math.PI / 180; double betaDot = input.betaDot * Math.PI / 180; double phiDot = input.phiDot * Math.PI / 180; Vector3d velocity = forward * cosAlpha * cosBeta; velocity += right * (sinPhi * sinAlpha * cosBeta + cosPhi * sinBeta); velocity += -up * (cosPhi * sinAlpha * cosBeta - sinPhi * sinBeta); velocity.Normalize(); Vector3d liftDown = -forward * sinAlpha; liftDown += right * sinPhi * cosAlpha; liftDown += -up * cosPhi * cosAlpha; liftDown.Normalize(); Vector3d sideways = Vector3.Cross(velocity, liftDown); sideways.Normalize(); Vector3d angVel = forward * (phiDot - sinAlpha * betaDot); angVel += right * (cosPhi * alphaDot + cosAlpha * sinPhi * betaDot); angVel += up * (sinPhi * alphaDot - cosAlpha * cosPhi * betaDot); foreach (FARWingAerodynamicModel w in _wingAerodynamicModel) { if (!(w && w.part)) { continue; } w.ComputeForceEditor(velocity, input.machNumber, 2); if (clear) { w.EditorClClear(reset_stall); } Vector3d relPos = w.GetAerodynamicCenter() - CoM; Vector3d vel = velocity + Vector3d.Cross(angVel, relPos); if (w is FARControllableSurface controllableSurface) { controllableSurface.SetControlStateEditor(CoM, vel, (float)input.pitchValue, 0, 0, input.flaps, input.spoilers); } else if (w.isShielded) { continue; } Vector3d force = w.ComputeForceEditor(vel.normalized, input.machNumber, 2) * 1000; output.Cl += -Vector3d.Dot(force, liftDown); output.Cy += Vector3d.Dot(force, sideways); output.Cd += -Vector3d.Dot(force, velocity); Vector3d moment = -Vector3d.Cross(relPos, force); output.Cm += Vector3d.Dot(moment, sideways); output.Cn += Vector3d.Dot(moment, liftDown); output.C_roll += Vector3d.Dot(moment, velocity); area += w.S; MAC += w.GetMAC() * w.S; b_2 += w.Getb_2() * w.S; } var center = new FARCenterQuery(); foreach (FARAeroSection aeroSection in _currentAeroSections) { aeroSection.PredictionCalculateAeroForces(2, (float)input.machNumber, 10000, 0, 0.005f, velocity.normalized, center); } Vector3d centerForce = center.force * 1000; output.Cl += -Vector3d.Dot(centerForce, liftDown); output.Cy += Vector3d.Dot(centerForce, sideways); output.Cd += -Vector3d.Dot(centerForce, velocity); Vector3d centerMoment = -center.TorqueAt(CoM) * 1000; output.Cm += Vector3d.Dot(centerMoment, sideways); output.Cn += Vector3d.Dot(centerMoment, liftDown); output.C_roll += Vector3d.Dot(centerMoment, velocity); if (area.NearlyEqual(0)) { area = _maxCrossSectionFromBody; b_2 = 1; MAC = _bodyLength; } double recipArea = 1 / area; MAC *= recipArea; b_2 *= recipArea; output.Cl *= recipArea; output.Cd *= recipArea; output.Cm *= recipArea / MAC; output.Cy *= recipArea; output.Cn *= recipArea / b_2; output.C_roll *= recipArea / b_2; }
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 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 void ResetAndGetClCdCmSteady(InstantConditionSimInput input, out InstantConditionSimOutput output) { parent.ResetClCdCmSteady(CoM, input); parent.GetClCdCmSteady(input, out output, true, false); }
public InstantConditionSimIterationResult IterateForAlphaAndPitch(out InstantConditionSimInput resultinput, out InstantConditionSimOutput resultoutput) { // reset 'old' calculations before first iteration parent.ResetClCdCmSteady(CoM, iterationInput); // level flight with yoke at neutral double alpha0 = FindAlphaForPitch(0); // stable attitude flight (at alpha0) double pitch0 = FindPitchForAlpha(alpha0); // level flight with deflected control surfaces double alpha1 = FindAlphaForPitch(pitch0); // updated stable attitude deflection double pitch1 = FindPitchForAlpha(alpha1); // iteration const int iterlim = 50; const double tolscale = 1 / 8; // scaled tolerance is stricter on the variable (alpha or pitch) to converge first int iterstep = 0; // in relation to when to exit two-dimensional iteration; ideally both variables while (iterstep < iterlim && // have converged within tol_linear at that point. Math.Abs(pitch1 - pitch0) > pitchtol.tol_linear * tolscale && Math.Abs(alpha1 - alpha0) > alphatol.tol_linear * tolscale) { double alpha2; double pitch2; IterateOnce(alpha0, pitch0, alpha1, pitch1, out alpha2, out pitch2); ++iterstep; alpha0 = alpha1; pitch0 = pitch1; alpha1 = alpha2; pitch1 = pitch2; } if (Math.Abs(pitch1 - pitch0) < pitchtol.tol_linear && Math.Abs(alpha1 - alpha0) < alphatol.tol_linear) { // ok, use alpha1 and pitch1 (i.e. the result of the last iteration) } else if (Math.Abs(pitch1 - pitch0) < pitchtol.tol_linear) { // we think we roughly know the stable attitude yoke position Debug.Log("[Rodhern][FAR] pitch determined (solve for alpha)."); alpha1 = FindAlphaForPitch(pitch1); } else if (Math.Abs(alpha1 - alpha0) < alphatol.tol_linear) { // accept partial optimization Debug.Log("[Rodhern][FAR] partial solution (alpha ~= " + alpha1 + ")."); pitch1 = FindPitchForAlpha(alpha1); alpha1 = FindAlphaForPitch(pitch1); } else { // level (but unstable) flight with yoke at neutral Debug.Log("[Rodhern][FAR] fix pitch at zero (last alpha was " + alpha1 + ")."); pitch1 = 0; alpha1 = FindAlphaForPitch(pitch1); } if (Double.IsNaN(alpha1) || Double.IsNaN(pitch1)) { alpha1 = 0; pitch1 = 0; } iterationInput.alpha = alpha1; iterationInput.pitchValue = pitch1; ResetAndGetClCdCmSteady(iterationInput, out iterationOutput); string AoAState; if (Math.Abs((iterationOutput.Cl - neededCl) / neededCl) < 0.01) { if (Math.Abs(iterationOutput.Cm) < 0.01) { AoAState = ""; } else { AoAState = (iterationOutput.Cm > 0) ? "\\" : "/"; } } else { AoAState = (iterationOutput.Cl > neededCl) ? "<" : ">"; } Debug.Log("[Rodhern][FAR] Cl needed: " + neededCl + ", AoAState: '" + AoAState + "'," + " AoA: " + alpha1 + ", pitch: " + pitch1 + ", Cl: " + iterationOutput.Cl + ", Cd: " + iterationOutput.Cd + ", Cm: " + iterationOutput.Cm + "."); resultinput = iterationInput.Clone(); // clone so that we do not give away our private variable reference resultoutput = iterationOutput; // new iteration output values are made at each calculation, so we can do a simple struct assignment copy return(new InstantConditionSimIterationResult(iterationOutput.Cl, iterationOutput.Cd, iterationOutput.Cm, pitch1, alpha1, AoAState)); }
public void GetClCdCmSteady(InstantConditionSimInput input, out InstantConditionSimOutput output, bool clear, bool reset_stall) { Vector3d CoM; double mass, area, MAC, b; // mass not actually used in these calculations GetCoMAndSize(out CoM, out mass, out area, out MAC, out b); FARCenterQuery center; ResetClCdCmSteady(CoM, input, out center, false, clear, reset_stall); Vector3d velocity, liftDown, sideways, angVel, velVector; GetAxisVectors(CoM, input, out velocity, out liftDown, out sideways, out angVel); velVector = input.fltenv.VelocityVector(velocity); output = new InstantConditionSimOutput(); for (int i = 0; i < _wingAerodynamicModel.Count; i++) { FARWingAerodynamicModel w = _wingAerodynamicModel[i]; if (!(w && w.part) || w.isShielded) { continue; } Vector3d relPos = w.GetAerodynamicCenter() - CoM; Vector3d vel = velVector + Vector3d.Cross(angVel, relPos); Vector3d force = w.ComputeForceEditor(vel, input.fltenv); output.Cl += -Vector3d.Dot(force, liftDown); output.Cd += -Vector3d.Dot(force, velocity); output.Cy += Vector3d.Dot(force, sideways); Vector3d moment = -Vector3d.Cross(relPos, force); output.Cm += Vector3d.Dot(moment, sideways); output.Cn += Vector3d.Dot(moment, liftDown); output.C_roll += Vector3d.Dot(moment, velocity); } Vector3d centerForce = center.force; output.Cl += -Vector3d.Dot(centerForce, liftDown); output.Cd += -Vector3d.Dot(centerForce, velocity); output.Cy += Vector3d.Dot(centerForce, sideways); Vector3d centerMoment = -center.TorqueAt(CoM); output.Cm += Vector3d.Dot(centerMoment, sideways); output.Cn += Vector3d.Dot(centerMoment, liftDown); output.C_roll += Vector3d.Dot(centerMoment, velocity); double q = input.fltenv.DynamicPressure(); double recip = 1 / (q * area); // reciprocal value to area and dynamic pressure output.Cl *= recip; output.Cd *= recip; output.Cy *= recip; output.Cm *= recip / MAC; output.Cn *= recip / b; output.C_roll *= recip / b; }