static const char *IdHdr = ID_PROPAGATE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
static const char *IdHdr = ID_PROPAGATE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex),
-LocalTerrainRadius(0), SeaLevelRadius(0), VehicleRadius(0)
+FGPropagate::FGPropagate(FGFDMExec* fdmex)
+ : FGModel(fdmex),
+ LocalTerrainRadius(0),
+ SeaLevelRadius(0),
+ VehicleRadius(0)
- integrator_rotational_rate = eAdamsBashforth2;
- integrator_translational_rate = eTrapezoidal;
- integrator_rotational_position = eAdamsBashforth2;
- integrator_translational_position = eTrapezoidal;
+ /// These define the indices use to select the various integrators.
+ // eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
+
+ integrator_rotational_rate = eRectEuler;
+ integrator_translational_rate = eAdamsBashforth2;
+ integrator_rotational_position = eRectEuler;
+ integrator_translational_position = eAdamsBashforth3;
VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.vLocation.SetEllipse(FDMExec->GetInertial()->GetSemimajor(), FDMExec->GetInertial()->GetSemiminor());
vOmegaEarth = FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->omega() ); // Earth rotation vector
VState.vLocation.SetEllipse(FDMExec->GetInertial()->GetSemimajor(), FDMExec->GetInertial()->GetSemiminor());
vOmegaEarth = FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->omega() ); // Earth rotation vector
vInertialVelocity.InitMatrix();
VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
vInertialVelocity.InitMatrix();
VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
- // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
- // This is the rotation rate of the "Local" frame, expressed in the local frame.
-
- FGColumnVector3 vOmegaLocal = FGColumnVector3(
- radInv*vVel(eEast),
- -radInv*vVel(eNorth),
- -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
-
CalculateUVWdot(); // Translational rate derivative
ResolveFrictionForces(dt); // Update rate derivatives with friction forces
CalculateQuatdot(); // Angular orientation derivative
CalculateUVWdot(); // Translational rate derivative
ResolveFrictionForces(dt); // Update rate derivatives with friction forces
CalculateQuatdot(); // Angular orientation derivative
- Integrate(VState.vPQRi_i, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate); // ECI integration
+ Integrate(VState.vPQRi, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate);
Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
Integrate(VState.vInertialVelocity, vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
Integrate(VState.vInertialVelocity, vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
// Set auxililary state variables
RecomputeLocalTerrainRadius();
VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
// Set auxililary state variables
RecomputeLocalTerrainRadius();
VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
- vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
- vPQRidot = Tb2i * vPQRdot;
+ vPQRidot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
+ vPQRdot = vPQRidot - VState.vPQRi * (Ti2b * vOmegaEarth);
// Save the value of the Lagrange multipliers to accelerate the convergence
// of the Gauss-Seidel algorithm at next iteration.
// Save the value of the Lagrange multipliers to accelerate the convergence
// of the Gauss-Seidel algorithm at next iteration.
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::SetInertialRates(FGColumnVector3 vRates) {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::SetInertialRates(FGColumnVector3 vRates) {
VState.dqQtrndot.clear();
for (int i=0; i<4; i++) {
VState.dqPQRidot.push_front(vPQRidot);
VState.dqQtrndot.clear();
for (int i=0; i<4; i++) {
VState.dqPQRidot.push_front(vPQRidot);
VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
VState.dqQtrndot.push_front(vQtrndot);
}
VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
VState.dqQtrndot.push_front(vQtrndot);
}
vVel = Tb2l * VState.vUVW;
VState.vPQR = vstate.vPQR;
VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
vVel = Tb2l * VState.vUVW;
VState.vPQR = vstate.vPQR;
VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;