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
}
