-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-// 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 inertial 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
-// VState.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)
-
-void FGPropagate::CalculatePQRdot(void)
-{
- const FGColumnVector3& vMoments = FDMExec->GetAircraft()->GetMoments(); // current moments
- const FGMatrix33& J = FDMExec->GetMassBalance()->GetJ(); // inertia matrix
- const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv(); // inertia matrix inverse
-
- // Compute body frame rotational accelerations based on the current body
- // moments and the total inertial angular velocity expressed in the body
- // frame.
-
- vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
- vPQRidot = Tb2i * vPQRdot;
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-// Compute the quaternion orientation derivative
-//
-// vQtrndot is the quaternion derivative.
-// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
-// Second edition (2004), eqn 1.5-16b (page 50)
-
-void FGPropagate::CalculateQuatdot(void)
-{
- // Compute quaternion orientation derivative on current body rates
- vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-// 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.
-// vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
-// so it has to be transformed to the body frame. More completely,
-// vOmegaEarth is the rate of the ECEF frame relative to the Inertial
-// frame (ECI), expressed in the Inertial frame.
-// vForces is the total force on the vehicle in the body frame.
-// VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
-// in the body frame.
-// VState.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)
-
-void FGPropagate::CalculateUVWdot(void)
-{
- double mass = FDMExec->GetMassBalance()->GetMass(); // mass
- const FGColumnVector3& vForces = FDMExec->GetAircraft()->GetForces(); // current forces
-
- vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
-
- // Include Centripetal acceleration.
- vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
-
- // Include Gravitation accel
- switch (gravType) {
- case gtStandard:
- vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->GetGAccel(VehicleRadius) );
- break;
- case gtWGS84:
- vGravAccel = Tec2b * FDMExec->GetInertial()->GetGravityJ2(VState.vLocation);
- break;
- }
-
- vUVWdot += vGravAccel;
- vUVWidot = Tb2i * (vForces/mass + vGravAccel);
-}
-