/// <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); }