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