FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
{
Debug(0);
Name = "FGPropagate";
last2_vPQRdot.InitMatrix();
last_vPQRdot.InitMatrix();
vPQRdot.InitMatrix();
last2_vQtrndot = FGQuaternion(0,0,0);
last_vQtrndot = FGQuaternion(0,0,0);
vQtrndot = FGQuaternion(0,0,0);
last2_vUVWdot.InitMatrix();
last_vUVWdot.InitMatrix();
vUVWdot.InitMatrix();
last2_vLocationDot.InitMatrix();
last_vLocationDot.InitMatrix();
vLocationDot.InitMatrix();
vOmegaLocal.InitMatrix();
integrator_rotational_rate = eAdamsBashforth2;
integrator_translational_rate = eTrapezoidal;
integrator_rotational_position = eAdamsBashforth2;
integrator_translational_position = eTrapezoidal;
bind();
Debug(0);
}
/** Returns the derivative of the quaternion corresponding to the
angular velocities PQR.
See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
Equation 1.3-36.
Also see Jack Kuipers, "Quaternions and Rotation Sequences", Equation 11.12.
*/
FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR) const
{
return FGQuaternion(
-0.5*( data[1]*PQR(eP) + data[2]*PQR(eQ) + data[3]*PQR(eR)),
0.5*( data[0]*PQR(eP) - data[3]*PQR(eQ) + data[2]*PQR(eR)),
0.5*( data[3]*PQR(eP) + data[0]*PQR(eQ) - data[1]*PQR(eR)),
0.5*(-data[2]*PQR(eP) + data[1]*PQR(eQ) + data[0]*PQR(eR))
);
}
bool FGAccelerations::InitModel(void)
{
if (!FGModel::InitModel()) return false;
vPQRidot.InitMatrix();
vUVWidot.InitMatrix();
vGravAccel.InitMatrix();
vBodyAccel.InitMatrix();
vQtrndot = FGQuaternion(0,0,0);
return true;
}
bool FGPropagate::InitModel(void)
{
if (!FGModel::InitModel()) return false;
// For initialization ONLY:
SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius();
VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
vOmega = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
last2_vPQRdot.InitMatrix();
last_vPQRdot.InitMatrix();
vPQRdot.InitMatrix();
last2_vQtrndot = FGQuaternion(0,0,0);
last_vQtrndot = FGQuaternion(0,0,0);
vQtrndot = FGQuaternion(0,0,0);
last2_vUVWdot.InitMatrix();
last_vUVWdot.InitMatrix();
vUVWdot.InitMatrix();
last2_vLocationDot.InitMatrix();
last_vLocationDot.InitMatrix();
vLocationDot.InitMatrix();
vOmegaLocal.InitMatrix();
integrator_rotational_rate = eAdamsBashforth2;
integrator_translational_rate = eTrapezoidal;
integrator_rotational_position = eAdamsBashforth2;
integrator_translational_position = eTrapezoidal;
return true;
}
FGAccelerations::FGAccelerations(FGFDMExec* fdmex)
: FGModel(fdmex)
{
Debug(0);
Name = "FGAccelerations";
gravType = gtWGS84;
gravTorque = false;
HoldDown = 0;
vPQRidot.InitMatrix();
vUVWidot.InitMatrix();
vGravAccel.InitMatrix();
vBodyAccel.InitMatrix();
vQtrndot = FGQuaternion(0,0,0);
bind();
Debug(0);
}
FGPropagate::FGPropagate(FGFDMExec* fdmex)
: FGModel(fdmex),
VehicleRadius(0)
{
Debug(0);
Name = "FGPropagate";
/// 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(5, FGColumnVector3(0.0,0.0,0.0));
VState.dqUVWidot.resize(5, FGColumnVector3(0.0,0.0,0.0));
VState.dqInertialVelocity.resize(5, FGColumnVector3(0.0,0.0,0.0));
VState.dqQtrndot.resize(5, FGQuaternion(0.0,0.0,0.0));
bind();
Debug(0);
}
void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
{
SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
// Set the position lat/lon/radius
VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
FGIC->GetLatitudeRadIC(),
FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
VehicleRadius = GetRadius();
radInv = 1.0/VehicleRadius;
// Set the Orientation from the euler angles
VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
FGIC->GetThetaRadIC(),
FGIC->GetPsiRadIC() );
// Set the velocities in the instantaneus body frame
VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
FGIC->GetVBodyFpsIC(),
FGIC->GetWBodyFpsIC() );
// Set the angular velocities in the instantaneus body frame.
VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
FGIC->GetQRadpsIC(),
FGIC->GetRRadpsIC() );
// Compute the local frame ECEF velocity
vVel = GetTb2l()*VState.vUVW;
// Finally, make sure that the quaternion stays normalized.
VState.vQtrn.Normalize();
// Recompute the LocalTerrainRadius.
RecomputeLocalTerrainRadius();
}
