/// <summary>
// Compute body frame rotational accelerations based on the current body moments
///
/// vPQRdot is the derivative of the absolute angular velocity of the vehicle
/// (body rate with respect to the ECEF frame), expressed in the body frame,
/// where the derivative is taken in the body frame.
/// J is the inertia matrix
/// Jinv is the inverse inertia matrix
/// vMoments is the moment vector in the body frame
/// in.vPQRi is the total inertial angular velocity of the vehicle
/// expressed in the body frame.
/// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
/// Second edition (2004), eqn 1.5-16e (page 50)
/// </summary>
private void CalculatePQRdot()
{
if (gravTorque)
{
// Compute the gravitational torque
// Reference: See Harris and Lyle "Spacecraft Gravitational Torques",
// NASA SP-8024 (1969) eqn (2) (page 7)
Vector3D R = inputs.Ti2b * inputs.vInertialPosition;
double invRadius = 1.0 / R.Magnitude();
R *= invRadius;
inputs.Moment += (3.0 * inputs.vGravAccel.Magnitude() * invRadius) * (R * (inputs.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 = Vector3D.Zero;
vPQRidot = inputs.vPQRi * (inputs.Ti2b * inputs.vOmegaPlanet);
}
else
{
vPQRidot = inputs.Jinv * (inputs.Moment - inputs.vPQRi * (inputs.J * inputs.vPQRi));
vPQRdot = vPQRidot - inputs.vPQRi * (inputs.Ti2b * inputs.vOmegaPlanet);
}
}
/// <summary>
/// This set of calculations results in the body and inertial frame accelerations
/// being computed.
/// Compute body and inertial frames accelerations based on the current body
/// forces including centripetal and Coriolis accelerations for the former.
/// in.vOmegaPlanet is the Earth angular rate - expressed in the inertial frame -
/// so it has to be transformed to the body frame. More completely,
/// in.vOmegaPlanet is the rate of the ECEF frame relative to the Inertial
/// frame (ECI), expressed in the Inertial frame.
/// in.Force is the total force on the vehicle in the body frame.
/// in.vPQR is the vehicle body rate relative to the ECEF frame, expressed
/// in the body frame.
/// in.vUVW is the vehicle velocity relative to the ECEF frame, expressed
/// in the body frame.
/// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
/// Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
/// </summary>
private void CalculateUVWdot()
{
if (FDMExec.GetHoldDown() && !FDMExec.GetTrimStatus())
{
vBodyAccel = Vector3D.Zero;
}
else
{
vBodyAccel = inputs.Force / inputs.Mass;
}
vUVWdot = vBodyAccel - (inputs.vPQR + 2.0 * (inputs.Ti2b * inputs.vOmegaPlanet)) * inputs.vUVW;
// Include Centripetal acceleration.
vUVWdot -= inputs.Ti2b * (inputs.vOmegaPlanet * (inputs.vOmegaPlanet * inputs.vInertialPosition));
if (FDMExec.GetHoldDown())
{
// The acceleration in ECI is calculated so that the acceleration is zero
// in the body frame.
vUVWidot = inputs.vOmegaPlanet * (inputs.vOmegaPlanet * inputs.vInertialPosition);
vUVWdot = Vector3D.Zero;
}
else
{
vUVWdot += inputs.Tec2b * inputs.vGravAccel;
vUVWidot = inputs.Tb2i * vBodyAccel + inputs.Tec2i * inputs.vGravAccel;
}
}
/// <summary>
/// Runs the state propagation model; called by the Executive
/// Can pass in a value indicating if the executive is directing the simulation to Hold.
/// </summary>
/// <param name="Holding">if true, the executive has been directed to hold the sim from
/// advancing time.Some models may ignore this flag, such as the Input
/// model, which may need to be active to listen on a socket for the
/// "Resume" command to be given.</param>
/// <returns>false if no error</returns>
public override bool Run(bool Holding)
{
if (base.Run(Holding))
{
return(true); // Fast return if we have nothing to do ...
}
if (Holding)
{
return(false);
}
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
if (!FDMExec.GetHoldDown())
{
CalculateFrictionForces(inputs.DeltaT * rate); // Update rate derivatives with friction forces
}
Debug(2);
return(false);
}