private void UpdateUpstreamValuesFromWingModules(List <FARWingAerodynamicModel> wingModules, List <double> associatedInfluences, double directionalInfluence, double thisWingAoA)
{
for (int i = 0; i < wingModules.Count; i++)
{
FARWingAerodynamicModel wingModule = wingModules[i];
double wingInfluenceFactor = associatedInfluences[i] * directionalInfluence;
double tmp = Vector3.Dot(wingModule.transform.forward, parentWingModule.transform.forward);
effectiveUpstreamMAC += wingModule.GetMAC() * wingInfluenceFactor;
effectiveUpstreamb_2 += wingModule.Getb_2() * wingInfluenceFactor;
effectiveUpstreamArea += wingModule.S * wingInfluenceFactor;
effectiveUpstreamLiftSlope += wingModule.GetLiftSlope() * wingInfluenceFactor;
effectiveUpstreamStall += wingModule.GetStall() * wingInfluenceFactor;
effectiveUpstreamCosSweepAngle += wingModule.GetCosSweepAngle() * wingInfluenceFactor;
effectiveUpstreamAoAMax += wingModule.AoAmax * wingInfluenceFactor;
effectiveUpstreamCd0 += wingModule.GetCd0() * wingInfluenceFactor;
effectiveUpstreamInfluence += wingInfluenceFactor;
double wAoA = wingModule.CalculateAoA(wingModule.GetVelocity()) * Math.Sign(tmp);
tmp = (thisWingAoA - wAoA) * wingInfluenceFactor; //First, make sure that the AoA are wrt the same direction; then account for any strange angling of the part that shouldn't be there
effectiveUpstreamAoA += tmp;
}
}
public void GetCoMAndSize(out Vector3d CoM, out double mass, out double area, out double MAC, out double b)
{
CoM = Vector3d.zero;
mass = 0; area = 0; MAC = 0; b = 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;
FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>();
if (w != null && !w.isShielded)
{
area += w.S;
MAC += w.GetMAC() * w.S;
b += w.Getb_2() * w.S;
}
}
if (area > 0)
{
MAC /= area;
b /= area;
}
else
{
area = _maxCrossSectionFromBody;
MAC = _bodyLength;
b = 1;
}
CoM /= mass;
mass *= 1000;
}
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;
}
/// <summary>
/// Accounts for increments in lift due to camber changes from upstream wings, and returns changes for this wing part; returns true if there are wings in front of it
/// </summary>
/// <param name="thisWingAoA">AoA of this wing in rad</param>
/// <param name="thisWingMachNumber">Mach Number of this wing in rad</param>
/// <param name="ACWeight">Weighting value for applying ACshift</param>
/// <param name="ACShift">Value used to shift the wing AC due to interactive effects</param>
/// <param name="ClIncrementFromRear">Increase in Cl due to this</param>
/// <returns></returns>
public void CalculateEffectsOfUpstreamWing(double thisWingAoA, double thisWingMachNumber, Vector3d parallelInPlaneLocal,
ref double ACweight, ref double ACshift, ref double ClIncrementFromRear)
{
double thisWingMAC, thisWingb_2;
thisWingMAC = parentWingModule.GetMAC();
thisWingb_2 = parentWingModule.Getb_2();
effectiveUpstreamMAC = 0;
effectiveUpstreamb_2 = 0;
effectiveUpstreamArea = 0;
effectiveUpstreamLiftSlope = 0;
effectiveUpstreamStall = 0;
effectiveUpstreamCosSweepAngle = 0;
effectiveUpstreamAoAMax = 0;
effectiveUpstreamAoA = 0;
effectiveUpstreamCd0 = 0;
effectiveUpstreamInfluence = 0;
double wingForwardDir = parallelInPlaneLocal.y;
double wingRightwardDir = parallelInPlaneLocal.x * srfAttachFlipped;
if (wingForwardDir > 0)
{
wingForwardDir *= wingForwardDir;
UpdateUpstreamValuesFromWingModules(nearbyWingModulesForwardList, nearbyWingModulesForwardInfluence, wingForwardDir, thisWingAoA);
}
else
{
wingForwardDir *= wingForwardDir;
UpdateUpstreamValuesFromWingModules(nearbyWingModulesBackwardList, nearbyWingModulesBackwardInfluence, wingForwardDir, thisWingAoA);
}
if (wingRightwardDir > 0)
{
wingRightwardDir *= wingRightwardDir;
UpdateUpstreamValuesFromWingModules(nearbyWingModulesRightwardList, nearbyWingModulesRightwardInfluence, wingRightwardDir, thisWingAoA);
}
else
{
wingRightwardDir *= wingRightwardDir;
UpdateUpstreamValuesFromWingModules(nearbyWingModulesLeftwardList, nearbyWingModulesLeftwardInfluence, wingRightwardDir, thisWingAoA);
}
double MachCoeff = FARMathUtil.Clamp(1 - thisWingMachNumber * thisWingMachNumber, 0, 1);
if (MachCoeff != 0)
{
double flapRatio = FARMathUtil.Clamp(thisWingMAC / (thisWingMAC + effectiveUpstreamMAC), 0, 1);
float flt_flapRatio = (float)flapRatio;
double flapFactor = wingCamberFactor.Evaluate(flt_flapRatio); //Flap Effectiveness Factor
double dCm_dCl = wingCamberMoment.Evaluate(flt_flapRatio); //Change in moment due to change in lift from flap
//This accounts for the wing possibly having a longer span than the flap
double WingFraction = FARMathUtil.Clamp(thisWingb_2 / effectiveUpstreamb_2, 0, 1);
//This accounts for the flap possibly having a longer span than the wing it's attached to
double FlapFraction = FARMathUtil.Clamp(effectiveUpstreamb_2 / thisWingb_2, 0, 1);
double ClIncrement = flapFactor * effectiveUpstreamLiftSlope * effectiveUpstreamAoA; //Lift created by the flap interaction
ClIncrement *= (parentWingModule.S * FlapFraction + effectiveUpstreamArea * WingFraction) / parentWingModule.S; //Increase the Cl so that even though we're working with the flap's area, it accounts for the added lift across the entire object
ACweight = ClIncrement * MachCoeff; // Total flap Cl for the purpose of applying ACshift, including the bit subtracted below
ClIncrement -= FlapFraction * effectiveUpstreamLiftSlope * effectiveUpstreamAoA; //Removing additional angle so that lift of the flap is calculated as lift at wing angle + lift due to flap interaction rather than being greater
ACshift = (dCm_dCl + 0.75 * (1 - flapRatio)) * (thisWingMAC + effectiveUpstreamMAC); //Change in Cm with change in Cl
ClIncrementFromRear = ClIncrement * MachCoeff;
}
}
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(Vector3d CoM, double alpha, double beta, double phi, double alphaDot, double betaDot, double phiDot, double M, double pitch, out double Cl, out double Cd, out double Cm, out double Cy, out double Cn, out double C_roll, bool clear, bool reset_stall = false, int flap_setting = 0, bool spoilersDeployed = false, bool vehicleFueled = true)
{
Cl = 0;
Cd = 0;
Cm = 0;
Cy = 0;
Cn = 0;
C_roll = 0;
double area = 0;
double MAC = 0;
double b_2 = 0;
alpha *= FARMathUtil.deg2rad;
beta *= FARMathUtil.deg2rad;
phi *= FARMathUtil.deg2rad;
Vector3d forward = Vector3.forward;
Vector3d up = Vector3.up;
Vector3d right = Vector3.right;
if (EditorDriver.editorFacility == EditorFacility.VAB)
{
forward = Vector3.up;
up = -Vector3.forward;
}
Vector3d AngVel = (phiDot - Math.Sin(alpha) * betaDot) * forward + (Math.Cos(phi) * alphaDot + Math.Cos(alpha) * Math.Sin(phi) * betaDot) * right + (Math.Sin(phi) * alphaDot - Math.Cos(alpha) * Math.Cos(phi) * betaDot) * up;
Vector3d velocity = forward * Math.Cos(alpha) * Math.Cos(beta) + right * (Math.Sin(phi) * Math.Sin(alpha) * Math.Cos(beta) - Math.Cos(phi) * Math.Sin(beta)) - up * (Math.Cos(phi) * Math.Sin(alpha) * Math.Cos(beta) - Math.Cos(phi) * Math.Sin(beta));
velocity.Normalize();
Vector3d liftVector = -forward *Math.Sin(alpha) + right * Math.Sin(phi) * Math.Cos(alpha) - up * Math.Cos(phi) * Math.Cos(alpha);
Vector3d sideways = Vector3.Cross(velocity, liftVector);
for (int i = 0; i < FARAeroUtil.CurEditorWings.Count; i++)
{
FARWingAerodynamicModel w = FARAeroUtil.CurEditorWings[i];
if (w.isShielded)
{
continue;
}
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)pitch, 0, 0, flap_setting, spoilersDeployed);
}
w.ComputeClCdEditor(vel, M);
double tmpCl = w.GetCl() * w.S;
Cl += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), liftVector);
Cy += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), sideways);
double tmpCd = w.GetCd() * w.S;
Cd += tmpCd;
Cm += tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), liftVector) + tmpCd * -Vector3d.Dot((relPos), liftVector);
Cn += tmpCd * Vector3d.Dot((relPos), sideways) + tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), sideways);
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;
}
for (int i = 0; i < FARAeroUtil.CurEditorOtherDrag.Count; i++)
{
FARBasicDragModel d = FARAeroUtil.CurEditorOtherDrag[i];
if (d.isShielded)
{
continue;
}
Vector3d relPos = d.part.transform.position - CoM;
Vector3d vel = velocity + Vector3d.Cross(AngVel, relPos);
double tmpCd = d.GetDragEditor(vel, M);
Cd += tmpCd;
double tmpCl = d.GetLiftEditor();
Cl += tmpCl * -Vector3d.Dot(d.GetLiftDirection(), liftVector);
Cy += tmpCl * -Vector3d.Dot(d.GetLiftDirection(), sideways);
relPos = d.GetCoDWithoutMomentShift() - CoM;
Cm += d.GetMomentEditor() + tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(d.GetLiftDirection(), liftVector) + tmpCd * -Vector3d.Dot((relPos), liftVector);
Cn += tmpCd * Vector3d.Dot((relPos), sideways) + tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(d.GetLiftDirection(), sideways);
C_roll += tmpCl * Vector3d.Dot((relPos), sideways) * -Vector3d.Dot(d.GetLiftDirection(), liftVector);
}
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 (vehicleFueled && p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
double stock_drag = partMass * p.maximum_drag * FlightGlobals.DragMultiplier * 1000;
Cd += stock_drag;
Cm += stock_drag * -Vector3d.Dot(part_pos, liftVector);
Cn += stock_drag * Vector3d.Dot(part_pos, sideways);
}
if (area == 0)
{
area = 1;
b_2 = 1;
MAC = 1;
}
double recipArea = 1 / area;
MAC *= recipArea;
b_2 *= recipArea;
Cl *= recipArea;
Cd *= recipArea;
Cm *= recipArea / MAC;
Cy *= recipArea;
Cn *= recipArea / b_2;
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;
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;
// 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 = -forward * cosAlpha * sinBeta;
//sideways += right * (cosPhi * cosBeta - sinPhi * sinAlpha * sinBeta);
//sideways += up * (cosPhi * sinAlpha * sinBeta + sinPhi * cosBeta);
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);
for (int i = 0; i < _wingAerodynamicModel.Count; i++)
{
FARWingAerodynamicModel w = _wingAerodynamicModel[i];
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)
{
(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, 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);
//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, 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);
/*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.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(
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);
}