void FGAccelerations::InitializeDerivatives(void)
{
// Make an initial run and set past values
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
CalculateFrictionForces(0.); // Update rate derivatives with friction forces
}
bool FGAccelerations::Run(bool Holding)
{
if (FGModel::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(in.DeltaT * rate); // Update rate derivatives with friction forces
Debug(2);
return false;
}
bool FGPropagate::Run(void)
{
if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
if (FDMExec->Holding()) return false;
RecomputeLocalTerrainRadius();
// Calculate current aircraft radius from center of planet
VehicleRadius = GetRadius();
radInv = 1.0/VehicleRadius;
// These local copies of the transformation matrices are for use this
// pass through Run() only.
Tl2b = GetTl2b(); // local to body frame transform
Tb2l = Tl2b.Transposed(); // body to local frame transform
Tl2ec = GetTl2ec(); // local to ECEF transform
Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
Ti2ec = GetTi2ec(); // ECI to ECEF transform
Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
Ti2b = Tec2b*Ti2ec; // ECI to body frame transform
Tb2i = Ti2b.Transposed(); // body to ECI frame transform
// Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
vVel = Tb2l * VState.vUVW;
// Inertial angular velocity measured in the body frame.
vPQRi = VState.vPQR + Tec2b*vOmega;
// Calculate state derivatives
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
CalculateQuatdot(); // Angular orientation derivative
CalculateLocationdot(); // Translational position derivative
// Integrate to propagate the state
double dt = State->Getdt()*rate; // The 'stepsize'
// Propagate rotational velocity
switch(integrator_rotational_rate) {
case eRectEuler: VState.vPQR += dt*vPQRdot;
break;
case eTrapezoidal: VState.vPQR += 0.5*dt*(vPQRdot + last_vPQRdot);
break;
case eAdamsBashforth2: VState.vPQR += dt*(1.5*vPQRdot - 0.5*last_vPQRdot);
break;
case eAdamsBashforth3: VState.vPQR += (1/12.0)*dt*(23.0*vPQRdot - 16.0*last_vPQRdot + 5.0*last2_vPQRdot);
break;
case eNone: // do nothing, freeze angular rate
break;
}
// Propagate translational velocity
switch(integrator_translational_rate) {
case eRectEuler: VState.vUVW += dt*vUVWdot;
break;
case eTrapezoidal: VState.vUVW += 0.5*dt*(vUVWdot + last_vUVWdot);
break;
case eAdamsBashforth2: VState.vUVW += dt*(1.5*vUVWdot - 0.5*last_vUVWdot);
break;
case eAdamsBashforth3: VState.vUVW += (1/12.0)*dt*(23.0*vUVWdot - 16.0*last_vUVWdot + 5.0*last2_vUVWdot);
break;
case eNone: // do nothing, freeze translational rate
break;
}
// Propagate angular position
switch(integrator_rotational_position) {
case eRectEuler: VState.vQtrn += dt*vQtrndot;
break;
case eTrapezoidal: VState.vQtrn += 0.5*dt*(vQtrndot + last_vQtrndot);
break;
case eAdamsBashforth2: VState.vQtrn += dt*(1.5*vQtrndot - 0.5*last_vQtrndot);
break;
case eAdamsBashforth3: VState.vQtrn += (1/12.0)*dt*(23.0*vQtrndot - 16.0*last_vQtrndot + 5.0*last2_vQtrndot);
break;
case eNone: // do nothing, freeze angular position
break;
}
// Propagate translational position
switch(integrator_translational_position) {
case eRectEuler: VState.vLocation += dt*vLocationDot;
break;
case eTrapezoidal: VState.vLocation += 0.5*dt*(vLocationDot + last_vLocationDot);
break;
case eAdamsBashforth2: VState.vLocation += dt*(1.5*vLocationDot - 0.5*last_vLocationDot);
break;
case eAdamsBashforth3: VState.vLocation += (1/12.0)*dt*(23.0*vLocationDot - 16.0*last_vLocationDot + 5.0*last2_vLocationDot);
break;
case eNone: // do nothing, freeze translational position
break;
}
// Set past values
last2_vPQRdot = last_vPQRdot;
last_vPQRdot = vPQRdot;
last2_vUVWdot = last_vUVWdot;
last_vUVWdot = vUVWdot;
last2_vQtrndot = last_vQtrndot;
last_vQtrndot = vQtrndot;
last2_vLocationDot = last_vLocationDot;
last_vLocationDot = vLocationDot;
Debug(2);
return false;
}
