void FGAccelerations::CalculatePQRdot(void) { if (gravTorque) { // Compute the gravitational torque // Reference: See Harris and Lyle "Spacecraft Gravitational Torques", // NASA SP-8024 (1969) eqn (2) (page 7) FGColumnVector3 R = in.Ti2b * in.vInertialPosition; double invRadius = 1.0 / R.Magnitude(); R *= invRadius; in.Moment += (3.0 * in.GAccel * invRadius) * (R * (in.J * R)); } // Compute body frame rotational accelerations based on the current body // moments and the total inertial angular velocity expressed in the body // frame. // if (HoldDown && !FDMExec->GetTrimStatus()) { if (FDMExec->GetHoldDown()) { // The rotational acceleration in ECI is calculated so that the rotational // acceleration is zero in the body frame. vPQRdot.InitMatrix(); vPQRidot = in.vPQRi * (in.Ti2b * in.vOmegaPlanet); } else { vPQRidot = in.Jinv * (in.Moment - in.vPQRi * (in.J * in.vPQRi)); vPQRdot = vPQRidot - in.vPQRi * (in.Ti2b * in.vOmegaPlanet); } }
FGTrim::RotationParameters FGTrim::calcRotation(vector<ContactPoints>& contacts, const FGColumnVector3& u, const FGColumnVector3& GM0) { RotationParameters rParam; vector<ContactPoints>::iterator iter; rParam.angleMin = 3.0 * M_PI; for (iter = contacts.begin(); iter != contacts.end(); iter++) { // Below the processed contact point is named 'M' // Construct an orthonormal basis (u, v, t). The ground normal is obtained // from iter->normal. FGColumnVector3 t = u * iter->normal; double length = t.Magnitude(); t /= length; // Normalize the tangent FGColumnVector3 v = t * u; FGColumnVector3 MM0 = GM0 - iter->location; // d0 is the distance from the circle center 'C' to the reference point 'M0' double d0 = DotProduct(MM0, u); // Compute the square of the circle radius i.e. the square of the distance // between 'C' and 'M'. double sqrRadius = DotProduct(MM0, MM0) - d0 * d0; // Compute the distance from the circle center 'C' to the line made by the // intersection between the ground and the plane that contains the circle. double DistPlane = d0 * DotProduct(u, iter->normal) / length; // The coordinate of the point of intersection 'P' between the circle and // the ground is (0, DistPlane, alpha) in the basis (u, v, t) double alpha = sqrt(sqrRadius - DistPlane * DistPlane); FGColumnVector3 CP = alpha * t + DistPlane * v; // The transformation is now constructed: we can extract the angle using the // classical formulas (cosine is obtained from the dot product and sine from // the cross product). double cosine = -DotProduct(MM0, CP) / sqrRadius; double sine = DotProduct(MM0 * u, CP) / sqrRadius; double angle = atan2(sine, cosine); if (angle < 0.0) angle += 2.0 * M_PI; if (angle < rParam.angleMin) { rParam.angleMin = angle; rParam.contactRef = iter; } } return rParam; }
FGColumnVector3 FGInertial::GetGravityJ2(FGColumnVector3 position) const { FGColumnVector3 J2Gravity; // Gravitation accel double r = position.Magnitude(); double lat = Propagate->GetLatitude(); double sinLat = sin(lat); double preCommon = 1.5*J2*(a/r)*(a/r); double xy = 1.0 - 5.0*(sinLat*sinLat); double z = 3.0 - 5.0*(sinLat*sinLat); double GMOverr2 = GM/(r*r); J2Gravity(1) = -GMOverr2 * ((1.0 + (preCommon * xy)) * position(eX)/r); J2Gravity(2) = -GMOverr2 * ((1.0 + (preCommon * xy)) * position(eY)/r); J2Gravity(3) = -GMOverr2 * ((1.0 + (preCommon * z)) * position(eZ)/r); return J2Gravity; }
double FGPropeller::Calculate(double EnginePower) { FGColumnVector3 localAeroVel = Transform().Transposed() * in.AeroUVW; double omega, PowerAvailable; double Vel = localAeroVel(eU); double rho = in.Density; double RPS = RPM/60.0; // Calculate helical tip Mach double Area = 0.25*Diameter*Diameter*M_PI; double Vtip = RPS * Diameter * M_PI; HelicalTipMach = sqrt(Vtip*Vtip + Vel*Vel) / in.Soundspeed; PowerAvailable = EnginePower - GetPowerRequired(); if (RPS > 0.0) J = Vel / (Diameter * RPS); // Calculate J normally else J = Vel / Diameter; if (MaxPitch == MinPitch) { // Fixed pitch prop ThrustCoeff = cThrust->GetValue(J); } else { // Variable pitch prop ThrustCoeff = cThrust->GetValue(J, Pitch); } // Apply optional scaling factor to Ct (default value = 1) ThrustCoeff *= CtFactor; // Apply optional Mach effects from CT_MACH table if (CtMach) ThrustCoeff *= CtMach->GetValue(HelicalTipMach); Thrust = ThrustCoeff*RPS*RPS*D4*rho; // Induced velocity in the propeller disk area. This formula is obtained // from momentum theory - see B. W. McCormick, "Aerodynamics, Aeronautics, // and Flight Mechanics" 1st edition, eqn. 6.15 (propeller analysis chapter). // Since Thrust and Vel can both be negative we need to adjust this formula // To handle sign (direction) separately from magnitude. double Vel2sum = Vel*abs(Vel) + 2.0*Thrust/(rho*Area); if( Vel2sum > 0.0) Vinduced = 0.5 * (-Vel + sqrt(Vel2sum)); else Vinduced = 0.5 * (-Vel - sqrt(-Vel2sum)); // We need to drop the case where the downstream velocity is opposite in // direction to the aircraft velocity. For example, in such a case, the // direction of the airflow on the tail would be opposite to the airflow on // the wing tips. When such complicated airflows occur, the momentum theory // breaks down and the formulas above are no longer applicable // (see H. Glauert, "The Elements of Airfoil and Airscrew Theory", // 2nd edition, §16.3, pp. 219-221) if ((Vel+2.0*Vinduced)*Vel < 0.0) { // The momentum theory is no longer applicable so let's assume the induced // saturates to -0.5*Vel so that the total velocity Vel+2*Vinduced equals 0. Vinduced = -0.5*Vel; } // P-factor is simulated by a shift of the acting location of the thrust. // The shift is a multiple of the angle between the propeller shaft axis // and the relative wind that goes through the propeller disk. if (P_Factor > 0.0001) { double tangentialVel = localAeroVel.Magnitude(eV, eW); if (tangentialVel > 0.0001) { double angle = atan2(tangentialVel, localAeroVel(eU)); double factor = Sense * P_Factor * angle / tangentialVel; SetActingLocationY( GetLocationY() + factor * localAeroVel(eW)); SetActingLocationZ( GetLocationZ() + factor * localAeroVel(eV)); } } omega = RPS*2.0*M_PI; vFn(eX) = Thrust; // The Ixx value and rotation speed given below are for rotation about the // natural axis of the engine. The transform takes place in the base class // FGForce::GetBodyForces() function. vH(eX) = Ixx*omega*Sense; vH(eY) = 0.0; vH(eZ) = 0.0; if (omega > 0.0) ExcessTorque = PowerAvailable / omega; else ExcessTorque = PowerAvailable / 1.0; RPM = (RPS + ((ExcessTorque / Ixx) / (2.0 * M_PI)) * deltaT) * 60.0; if (RPM < 0.0) RPM = 0.0; // Engine won't turn backwards // Transform Torque and momentum first, as PQR is used in this // equation and cannot be transformed itself. vMn = in.PQR*(Transform()*vH) + Transform()*vTorque; return Thrust; // return thrust in pounds }
double FGPropeller::Calculate(double EnginePower) { FGColumnVector3 localAeroVel = Transform().Transposed() * in.AeroUVW; double omega, PowerAvailable; double Vel = localAeroVel(eU); double rho = in.Density; double RPS = RPM/60.0; // Calculate helical tip Mach double Area = 0.25*Diameter*Diameter*M_PI; double Vtip = RPS * Diameter * M_PI; HelicalTipMach = sqrt(Vtip*Vtip + Vel*Vel) / in.Soundspeed; if (RPS > 0.0) J = Vel / (Diameter * RPS); // Calculate J normally else J = Vel / Diameter; PowerAvailable = EnginePower - GetPowerRequired(); if (MaxPitch == MinPitch) { // Fixed pitch prop ThrustCoeff = cThrust->GetValue(J); } else { // Variable pitch prop ThrustCoeff = cThrust->GetValue(J, Pitch); } // Apply optional scaling factor to Ct (default value = 1) ThrustCoeff *= CtFactor; // Apply optional Mach effects from CT_MACH table if (CtMach) ThrustCoeff *= CtMach->GetValue(HelicalTipMach); Thrust = ThrustCoeff*RPS*RPS*D4*rho; // Induced velocity in the propeller disk area. This formula is obtained // from momentum theory - see B. W. McCormick, "Aerodynamics, Aeronautics, // and Flight Mechanics" 1st edition, eqn. 6.15 (propeller analysis chapter). // Since Thrust and Vel can both be negative we need to adjust this formula // To handle sign (direction) separately from magnitude. double Vel2sum = Vel*abs(Vel) + 2.0*Thrust/(rho*Area); if( Vel2sum > 0.0) Vinduced = 0.5 * (-Vel + sqrt(Vel2sum)); else Vinduced = 0.5 * (-Vel - sqrt(-Vel2sum)); // P-factor is simulated by a shift of the acting location of the thrust. // The shift is a multiple of the angle between the propeller shaft axis // and the relative wind that goes through the propeller disk. if (P_Factor > 0.0001) { double tangentialVel = localAeroVel.Magnitude(eV, eW); if (tangentialVel > 0.0001) { // The angle made locally by the air flow with respect to the propeller // axis is influenced by the induced velocity. This attenuates the // influence of a string cross wind and gives a more realistic behavior. double angle = atan2(tangentialVel, Vel+Vinduced); double factor = Sense * P_Factor * angle / tangentialVel; SetActingLocationY( GetLocationY() + factor * localAeroVel(eW)); SetActingLocationZ( GetLocationZ() + factor * localAeroVel(eV)); } } omega = RPS*2.0*M_PI; vFn(eX) = Thrust; // The Ixx value and rotation speed given below are for rotation about the // natural axis of the engine. The transform takes place in the base class // FGForce::GetBodyForces() function. FGColumnVector3 vH(Ixx*omega*Sense*Sense_multiplier, 0.0, 0.0); if (omega > 0.0) ExcessTorque = PowerAvailable / omega; else ExcessTorque = PowerAvailable / 1.0; RPM = (RPS + ((ExcessTorque / Ixx) / (2.0 * M_PI)) * in.TotalDeltaT) * 60.0; if (RPM < 0.0) RPM = 0.0; // Engine won't turn backwards // Transform Torque and momentum first, as PQR is used in this // equation and cannot be transformed itself. vMn = in.PQRi*(Transform()*vH) + Transform()*vTorque; return Thrust; // return thrust in pounds }