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
}
void FGTrim::trimOnGround(void)
{
FGGroundReactions* GroundReactions = fdmex->GetGroundReactions();
FGPropagate* Propagate = fdmex->GetPropagate();
FGMassBalance* MassBalance = fdmex->GetMassBalance();
FGAccelerations* Accelerations = fdmex->GetAccelerations();
vector<ContactPoints> contacts;
FGLocation CGLocation = Propagate->GetLocation();
FGMatrix33 Tec2b = Propagate->GetTec2b();
FGMatrix33 Tl2b = Propagate->GetTl2b();
double hmin = 1E+10;
int contactRef = -1;
// Build the list of the aircraft contact points and take opportunity of the
// loop to find which one is closer to (or deeper into) the ground.
for (int i = 0; i < GroundReactions->GetNumGearUnits(); i++) {
ContactPoints c;
FGLGear* gear = GroundReactions->GetGearUnit(i);
c.location = gear->GetLocalGear();
FGLocation gearLoc = CGLocation.LocalToLocation(c.location);
c.location = Tl2b * c.location;
FGColumnVector3 normal, vDummy;
FGLocation lDummy;
double height = gearLoc.GetContactPoint(lDummy, normal, vDummy, vDummy);
c.normal = Tec2b * normal;
contacts.push_back(c);
if (height < hmin) {
hmin = height;
contactRef = i;
}
}
// Remove the contact point that is closest to the ground from the list:
// the rotation axis will be going thru this point so we need to remove it
// to avoid divisions by zero that could result from the computation of
// the rotations.
FGColumnVector3 contact0 = contacts[contactRef].location;
contacts.erase(contacts.begin() + contactRef);
// Update the initial conditions: this should remove the forces generated
// by overcompressed landing gears
fgic.SetAltitudeASLFtIC(fgic.GetAltitudeASLFtIC() - hmin);
fdmex->Initialize(&fgic);
fdmex->Run();
// Compute the rotation axis: it is obtained from the direction of the
// moment measured at the contact point 'contact0'
FGColumnVector3 force = MassBalance->GetMass() * Accelerations->GetUVWdot();
FGColumnVector3 moment = MassBalance->GetJ() * Accelerations->GetPQRdot()
+ force * contact0;
FGColumnVector3 rotationAxis = moment.Normalize();
// Compute the rotation parameters: angle and the first point to come into
// contact with the ground when the rotation is applied.
RotationParameters rParam = calcRotation(contacts, rotationAxis, contact0);
FGQuaternion q0(rParam.angleMin, rotationAxis);
// Apply the computed rotation to all the contact points
FGMatrix33 rot = q0.GetTInv();
vector<ContactPoints>::iterator iter;
for (iter = contacts.begin(); iter != contacts.end(); iter++)
iter->location = contact0 + rot * (iter->location - contact0);
// Remove the second point to come in contact with the ground from the list.
// The reason is the same than above: avoid divisions by zero when the next
// rotation will be computed.
FGColumnVector3 contact1 = rParam.contactRef->location;
contacts.erase(rParam.contactRef);
// Compute the rotation axis: now there are 2 points in contact with the
// ground so the only option for the aircraft is to rotate around the axis
// generated by these 2 points.
rotationAxis = contact1 - contact0;
// Make sure that the rotation orientation is consistent with the moment.
if (DotProduct(rotationAxis, moment) < 0.0)
rotationAxis = contact0 - contact1;
rotationAxis.Normalize();
// Compute the rotation parameters
rParam = calcRotation(contacts, rotationAxis, contact0);
FGQuaternion q1(rParam.angleMin, rotationAxis);
// Update the aircraft orientation
FGColumnVector3 euler = (q0 * q1 * fgic.GetOrientation()).GetEuler();
fgic.SetPhiRadIC(euler(1));
fgic.SetThetaRadIC(euler(2));
fgic.SetPsiRadIC(euler(3));
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double TestInterface::EasyGetValue(const string prop)
{
if (prop == "set-running")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-Get: engine(s) running = %i\n",fdmExec->GetPropulsion()->GetEngine(0)->GetRunning());
return fdmExec->GetPropulsion()->GetEngine(0)->GetRunning();
}
else if (prop == "u-fps")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: propagate->GetUVW(1);= %f\n", propagate->GetUVW(1));
return propagate->GetUVW(1);;
}
else if (prop == "v-fps")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: propagate->GetUVW(2);= %f\n", propagate->GetUVW(2));
return propagate->GetUVW(2);;
}
else if (prop == "w-fps")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: propagate->GetUVW(3);= %f\n", propagate->GetUVW(3));
return propagate->GetUVW(3);;
}
else if (prop == "p-rad_sec")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: roll rate (rad/s) = %f\n",propagate->GetPQR(1));
return propagate->GetPQR(1);;
}
else if (prop == "q-rad_sec")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: pitch rate (rad/s) = %f\n",propagate->GetPQR(2));
return propagate->GetPQR(2);
}
else if (prop == "r-rad_sec")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: yaw rate (rad/s) = %f\n",propagate->GetPQR(3));
return propagate->GetPQR(3);
}
else if (prop == "h-sl-ft")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: altitude over sea level (mt) = %f\n",propagate->GetAltitudeASLmeters());
return propagate->GetAltitudeASLmeters();
}
else if (prop == "long-gc-deg")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: geocentric longitude (deg) = %f\n",propagate->GetLongitudeDeg());
return propagate->GetLongitudeDeg();
}
else if (prop == "lat-gc-deg")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: geocentric latitude (deg) = %f\n",propagate->GetLatitudeDeg());
return propagate->GetLatitudeDeg();
}
else if (prop == "phi-rad")
{
FGColumnVector3 euler = propagate->GetVState().qAttitudeLocal.GetEuler();
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: phi-rad = %f\n",euler.Entry(1));
return euler.Entry(1);
}
else if (prop == "theta-rad")
{
FGColumnVector3 euler = propagate->GetVState().qAttitudeLocal.GetEuler();
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: theta-rad = %f\n",euler.Entry(2));
return euler.Entry(2);
}
else if (prop == "psi-rad")
{
FGColumnVector3 euler = propagate->GetVState().qAttitudeLocal.GetEuler();
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: psi-rad = %f\n",euler.Entry(3));
return euler.Entry(3);
}
else if (prop == "elevator-pos-rad")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: elevator pos (rad) = %f\n",fdmExec->GetFCS()->GetDePos());
return fdmExec->GetFCS()->GetDePos();
}
else if (prop == "aileron-pos-rad")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-get: right aileron pos (rad) = %f\n",fdmExec->GetFCS()->GetDaRPos());
return fdmExec->GetFCS()->GetDaRPos();
}
else if (prop == "rudder-pos-rad")
{
if ( verbosityLevel == eVeryVerbose )
mexPrintf("\tEasy-set: rudder pos (deg) = %f\n",fdmExec->GetFCS()->GetDrPos());
return fdmExec->GetFCS()->GetDrPos();
}
return 0;
}
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
}