X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2Fmodels%2FFGPropagate.cpp;h=3c77bca282af8e5df2c38680b08efdd9924d064a;hb=024ef128e3395e8c0e32b360abe19b4d345e4f80;hp=d917420d80e0d1cb0728587b35f34a1cdf14d751;hpb=53e8fbfcbb3ddac0dc261043fea7bd24dfa6e62d;p=flightgear.git diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index d917420d8..3c77bca28 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -48,7 +48,16 @@ COMMENTS, REFERENCES, and NOTES Wiley & Sons, 1979 ISBN 0-471-03032-5 [5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons, 1982 ISBN 0-471-08936-2 -[6] Erin Catto, "Iterative Dynamics with Temporal Coherence", February 22, 2005 +[6] S. Buss, "Accurate and Efficient Simulation of Rigid Body Rotations", + Technical Report, Department of Mathematics, University of California, + San Diego, 1999 +[7] Barker L.E., Bowles R.L. and Williams L.H., "Development and Application of + a Local Linearization Algorithm for the Integration of Quaternion Rate + Equations in Real-Time Flight Simulation Problems", NASA TN D-7347, + December 1973 +[8] Phillips W.F, Hailey C.E and Gebert G.A, "Review of Attitude Representations + Used for Aircraft Kinematics", Journal Of Aircraft Vol. 38, No. 4, + July-August 2001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES @@ -62,38 +71,35 @@ INCLUDES #include "FGPropagate.h" #include "FGGroundReactions.h" #include "FGFDMExec.h" -#include "FGAircraft.h" -#include "FGMassBalance.h" -#include "FGInertial.h" #include "input_output/FGPropertyManager.h" using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.74 2010/11/28 13:02:43 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.105 2012/03/26 21:26:11 bcoconni Exp $"; 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), + VehicleRadius(0) { Debug(0); Name = "FGPropagate"; - gravType = gtWGS84; - - vPQRdot.InitMatrix(); - vQtrndot = FGQuaternion(0,0,0); - vUVWdot.InitMatrix(); + vInertialVelocity.InitMatrix(); - 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)); @@ -115,18 +121,10 @@ FGPropagate::~FGPropagate(void) bool FGPropagate::InitModel(void) { - if (!FGModel::InitModel()) return false; - // For initialization ONLY: - SeaLevelRadius = LocalTerrainRadius = FDMExec->GetInertial()->GetRefRadius(); - - VState.vLocation.SetRadius( LocalTerrainRadius + 4.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(in.SemiMajor, in.SemiMinor); + VState.vLocation.SetAltitudeAGL(4.0, FDMExec->GetSimTime()); - vPQRdot.InitMatrix(); - vQtrndot = FGQuaternion(0,0,0); - vUVWdot.InitMatrix(); vInertialVelocity.InitMatrix(); VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0)); @@ -134,10 +132,10 @@ bool FGPropagate::InitModel(void) VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0)); VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0)); - integrator_rotational_rate = eAdamsBashforth2; - integrator_translational_rate = eTrapezoidal; - integrator_rotational_position = eAdamsBashforth2; - integrator_translational_position = eTrapezoidal; + integrator_rotational_rate = eRectEuler; + integrator_translational_rate = eAdamsBashforth2; + integrator_rotational_position = eRectEuler; + integrator_translational_position = eAdamsBashforth3; return true; } @@ -146,20 +144,13 @@ bool FGPropagate::InitModel(void) void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) { - SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC()); - SetTerrainElevation(FGIC->GetTerrainElevationFtIC()); - // Initialize the State Vector elements and the transformation matrices // Set the position lat/lon/radius - VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(), - FGIC->GetLatitudeRadIC(), - FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() ); - - VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); + VState.vLocation = FGIC->GetPosition(); - Ti2ec = GetTi2ec(); // ECI to ECEF transform - Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform + Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform + Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform VState.vInertialPosition = Tec2i * VState.vLocation; @@ -168,47 +159,41 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) // Set the orientation from the euler angles (is normalized within the // constructor). The Euler angles represent the orientation of the body // frame relative to the local frame. - VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(), - FGIC->GetThetaRadIC(), - FGIC->GetPsiRadIC() ); + VState.qAttitudeLocal = FGIC->GetOrientation(); VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal; UpdateBodyMatrices(); // Set the velocities in the instantaneus body frame - VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(), - FGIC->GetVBodyFpsIC(), - FGIC->GetWBodyFpsIC() ); + VState.vUVW = FGIC->GetUVWFpsIC(); // Compute the local frame ECEF velocity vVel = Tb2l * VState.vUVW; - // Recompute the LocalTerrainRadius. - RecomputeLocalTerrainRadius(); - + // Compute local terrain velocity + RecomputeLocalTerrainVelocity(); VehicleRadius = GetRadius(); - double radInv = 1.0/VehicleRadius; - // Refer to Stevens and Lewis, 1.5-14a, pg. 49. - // This is the rotation rate of the "Local" frame, expressed in the local frame. + // Set the angular velocities of the body frame relative to the ECEF frame, + // expressed in the body frame. + VState.vPQR = FGIC->GetPQRRadpsIC(); - FGColumnVector3 vOmegaLocal = FGColumnVector3( - radInv*vVel(eEast), - -radInv*vVel(eNorth), - -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() ); + VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet; - // Set the angular velocities of the body frame relative to the ECEF frame, - // expressed in the body frame. Effectively, this is: - // w_b/e = w_b/l + w_l/e - VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(), - FGIC->GetQRadpsIC(), - FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal; + CalculateInertialVelocity(); // Translational position derivative +} - VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth; - VState.vPQRi_i = Tb2i * VState.vPQRi; +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// Initialize the past value deques - // Make an initial run and set past values - InitializeDerivatives(); +void FGPropagate::InitializeDerivatives() +{ + for (int i=0; i<4; i++) { + VState.dqPQRidot[i] = in.vPQRidot; + VState.dqUVWidot[i] = in.vUVWidot; + VState.dqInertialVelocity[i] = VState.vInertialVelocity; + VState.dqQtrndot[i] = in.vQtrndot; + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -224,45 +209,36 @@ This propagation is done using the current state values and current derivatives. Based on these values we compute an approximation to the state values for (now + dt). -In the code below, variables named beginning with a small "v" refer to a +In the code below, variables named beginning with a small "v" refer to a a column vector, variables named beginning with a "T" refer to a transformation matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered Inertial. */ -bool FGPropagate::Run(void) +bool FGPropagate::Run(bool Holding) { - if (FGModel::Run()) return true; // Fast return if we have nothing to do ... - if (FDMExec->Holding()) return false; - - double dt = FDMExec->GetDeltaT()*rate; // The 'stepsize' + if (FGModel::Run(Holding)) return true; // Fast return if we have nothing to do ... + if (Holding) return false; - RunPreFunctions(); - - // Calculate state derivatives - CalculatePQRdot(); // Angular rate derivative - CalculateUVWdot(); // Translational rate derivative - ResolveFrictionForces(dt); // Update rate derivatives with friction forces - CalculateQuatdot(); // Angular orientation derivative - CalculateUVW(); // Translational position derivative (velocities are integrated in the inertial frame) + double dt = in.DeltaT * rate; // The 'stepsize' // Propagate rotational / translational velocity, angular /translational position, respectively. - Integrate(VState.vPQRi_i, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate); // ECI integration - Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position); + Integrate(VState.qAttitudeECI, in.vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position); + Integrate(VState.vPQRi, in.vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate); Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position); - Integrate(VState.vInertialVelocity, vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate); + Integrate(VState.vInertialVelocity, in.vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate); // CAUTION : the order of the operations below is very important to get transformation // matrices that are consistent with the new state of the vehicle // 1. Update the Earth position angle (EPA) - VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); + VState.vLocation.IncrementEarthPositionAngle(in.vOmegaPlanet(eZ)*(in.DeltaT*rate)); // 2. Update the Ti2ec and Tec2i transforms from the updated EPA - Ti2ec = GetTi2ec(); // ECI to ECEF transform - Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform + Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform + Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform // 3. Update the location from the updated Ti2ec and inertial position VState.vLocation = Ti2ec*VState.vInertialPosition; @@ -271,124 +247,38 @@ bool FGPropagate::Run(void) // vLocation vector. UpdateLocationMatrices(); - // 5. Normalize the ECI Attitude quaternion - VState.qAttitudeECI.Normalize(); - - // 6. Update the "Orientation-based" transformation matrices from the updated + // 5. Update the "Orientation-based" transformation matrices from the updated // orientation quaternion and vLocation vector. UpdateBodyMatrices(); - // Set auxililary state variables - RecomputeLocalTerrainRadius(); + // Translational position derivative (velocities are integrated in the inertial frame) + CalculateUVW(); + // Set auxilliary state variables + RecomputeLocalTerrainVelocity(); VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet - VState.vPQRi = Ti2b * VState.vPQRi_i; - VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth; + VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet; VState.qAttitudeLocal = Tl2b.GetQuaternion(); // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame. vVel = Tb2l * VState.vUVW; - RunPostFunctions(); - Debug(2); return false; } -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// 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); -} - //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // Transform the velocity vector of the body relative to the origin (Earth // center) to be expressed in the inertial frame, and add the vehicle velocity // contribution due to the rotation of the planet. - // Reference: See Stevens and Lewis, "Aircraft Control and Simulation", + // Reference: See Stevens and Lewis, "Aircraft Control and Simulation", // Second edition (2004), eqn 1.5-16c (page 50) void FGPropagate::CalculateInertialVelocity(void) { - VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition); + VState.vInertialVelocity = Tb2i * VState.vUVW + (in.vOmegaPlanet * VState.vInertialPosition); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -398,7 +288,7 @@ void FGPropagate::CalculateInertialVelocity(void) void FGPropagate::CalculateUVW(void) { - VState.vUVW = Ti2b * (VState.vInertialVelocity - (vOmegaEarth * VState.vInertialPosition)); + VState.vUVW = Ti2b * (VState.vInertialVelocity - (in.vOmegaPlanet * VState.vInertialPosition)); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -425,6 +315,8 @@ void FGPropagate::Integrate( FGColumnVector3& Integrand, break; case eNone: // do nothing, freeze translational rate break; + default: + break; } } @@ -450,197 +342,111 @@ void FGPropagate::Integrate( FGQuaternion& Integrand, break; case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]); break; + case eBuss1: + { + // This is the first order method as described in Samuel R. Buss paper[6]. + // The formula from Buss' paper is transposed below to quaternions and is + // actually the exact solution of the quaternion differential equation + // qdot = 1/2*w*q when w is constant. + Integrand = Integrand * QExp(0.5 * dt * VState.vPQRi); + } + return; // No need to normalize since the quaternion exponential is always normal + case eBuss2: + { + // This is the 'augmented second-order method' from S.R. Buss paper [6]. + // Unlike Runge-Kutta or Adams-Bashforth, it is a one-pass second-order + // method (see reference [6]). + FGColumnVector3 wi = VState.vPQRi; + FGColumnVector3 wdoti = in.vPQRidot; + FGColumnVector3 omega = wi + 0.5*dt*wdoti + dt*dt/12.*wdoti*wi; + Integrand = Integrand * QExp(0.5 * dt * omega); + } + return; // No need to normalize since the quaternion exponential is always normal + case eLocalLinearization: + { + // This is the local linearization algorithm of Barker et al. (see ref. [7]) + // It is also a one-pass second-order method. The code below is based on the + // more compact formulation issued from equation (107) of ref. [8]. The + // constants C1, C2, C3 and C4 have the same value than those in ref. [7] pp. 11 + FGColumnVector3 wi = 0.5 * VState.vPQRi; + FGColumnVector3 wdoti = 0.5 * in.vPQRidot; + double omegak2 = DotProduct(VState.vPQRi, VState.vPQRi); + double omegak = omegak2 > 1E-6 ? sqrt(omegak2) : 1E-6; + double rhok = 0.5 * dt * omegak; + double C1 = cos(rhok); + double C2 = 2.0 * sin(rhok) / omegak; + double C3 = 4.0 * (1.0 - C1) / (omegak*omegak); + double C4 = 4.0 * (dt - C2) / (omegak*omegak); + FGColumnVector3 Omega = C2*wi + C3*wdoti + C4*wi*wdoti; + FGQuaternion q; + + q(1) = C1 - C4*DotProduct(wi, wdoti); + q(2) = Omega(eP); + q(3) = Omega(eQ); + q(4) = Omega(eR); + + Integrand = Integrand * q; + + /* Cross check with ref. [7] pp.11-12 formulas and code pp. 20 + double pk = VState.vPQRi(eP); + double qk = VState.vPQRi(eQ); + double rk = VState.vPQRi(eR); + double pdotk = in.vPQRidot(eP); + double qdotk = in.vPQRidot(eQ); + double rdotk = in.vPQRidot(eR); + double Ap = -0.25 * (pk*pdotk + qk*qdotk + rk*rdotk); + double Bp = 0.25 * (pk*qdotk - qk*pdotk); + double Cp = 0.25 * (pdotk*rk - pk*rdotk); + double Dp = 0.25 * (qk*rdotk - qdotk*rk); + double C2p = sin(rhok) / omegak; + double C3p = 2.0 * (1.0 - cos(rhok)) / (omegak*omegak); + double H = C1 + C4 * Ap; + double G = -C2p*rk - C3p*rdotk + C4*Bp; + double J = C2p*qk + C3p*qdotk - C4*Cp; + double K = C2p*pk + C3p*pdotk - C4*Dp; + + cout << "q: " << q << endl; + + // Warning! In the paper of Barker et al. the quaternion components are not + // ordered the same way as in JSBSim (see equations (2) and (3) of ref. [7] + // as well as the comment just below equation (3)) + cout << "FORTRAN: " << H << " , " << K << " , " << J << " , " << -G << endl;*/ + } + break; // The quaternion q is not normal so the normalization needs to be done. case eNone: // do nothing, freeze rotational rate break; - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// Evaluates the rates (translation or rotation) that the friction forces have -// to resist to. This includes the external forces and moments as well as the -// relative movement between the aircraft and the ground. -// Erin Catto's paper (see ref [6]) only supports Euler integration scheme and -// this algorithm has been adapted to handle the multistep algorithms that -// JSBSim supports (i.e. Trapezoidal, Adams-Bashforth 2, 3 and 4). The capacity -// to handle the multistep integration schemes adds some complexity but it -// significantly helps stabilizing the friction forces. - -void FGPropagate::EvaluateRateToResistTo(FGColumnVector3& vdot, - const FGColumnVector3& Val, - const FGColumnVector3& ValDot, - const FGColumnVector3& LocalTerrainVal, - deque & dqValDot, - const double dt, - const eIntegrateType integration_type) -{ - switch(integration_type) { - case eAdamsBashforth4: - vdot = ValDot + Ti2b * (-59.*dqValDot[0]+37.*dqValDot[1]-9.*dqValDot[2])/55.; - if (dt > 0.) // Zeroes out the relative movement between aircraft and ground - vdot += 24.*(Val - Tec2b * LocalTerrainVal) / (55.*dt); - break; - case eAdamsBashforth3: - vdot = ValDot + Ti2b * (-16.*dqValDot[0]+5.*dqValDot[1])/23.; - if (dt > 0.) // Zeroes out the relative movement between aircraft and ground - vdot += 12.*(Val - Tec2b * LocalTerrainVal) / (23.*dt); - break; - case eAdamsBashforth2: - vdot = ValDot - Ti2b * dqValDot[0]/3.; - if (dt > 0.) // Zeroes out the relative movement between aircraft and ground - vdot += 2.*(Val - Tec2b * LocalTerrainVal) / (3.*dt); - break; - case eTrapezoidal: - vdot = ValDot + Ti2b * dqValDot[0]; - if (dt > 0.) // Zeroes out the relative movement between aircraft and ground - vdot += 2.*(Val - Tec2b * LocalTerrainVal) / dt; - break; - case eRectEuler: - vdot = ValDot; - if (dt > 0.) // Zeroes out the relative movement between aircraft and ground - vdot += (Val - Tec2b * LocalTerrainVal) / dt; - break; - case eNone: + default: break; } -} -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// Resolves the contact forces just before integrating the EOM. -// This routine is using Lagrange multipliers and the projected Gauss-Seidel -// (PGS) method. -// Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence", -// February 22, 2005 -// In JSBSim there is only one rigid body (the aircraft) and there can be -// multiple points of contact between the aircraft and the ground. As a -// consequence our matrix J*M^-1*J^T is not sparse and the algorithm described -// in Catto's paper has been adapted accordingly. -// The friction forces are resolved in the body frame relative to the origin -// (Earth center). - -void FGPropagate::ResolveFrictionForces(double dt) -{ - const double invMass = 1.0 / FDMExec->GetMassBalance()->GetMass(); - const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv(); - vector JacF, JacM; - vector lambda, lambdaMin, lambdaMax; - FGColumnVector3 vdot, wdot; - FGColumnVector3 Fc, Mc; - int n = 0; - - // Compiles data from the ground reactions to build up the jacobian matrix - for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it, n++) { - JacF.push_back((*it)->ForceJacobian); - JacM.push_back((*it)->MomentJacobian); - lambda.push_back((*it)->value); - lambdaMax.push_back((*it)->Max); - lambdaMin.push_back((*it)->Min); - } - - // If no gears are in contact with the ground then return - if (!n) return; - - vector a(n*n); // Will contain J*M^-1*J^T - vector rhs(n); - - // Assemble the linear system of equations - for (int i=0; i < n; i++) { - for (int j=0; j < i; j++) - a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J - for (int j=i; j < n; j++) - a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]); - } - - // Assemble the RHS member - - // Translation - EvaluateRateToResistTo(vdot, VState.vUVW, vUVWdot, LocalTerrainVelocity, - VState.dqUVWidot, dt, integrator_translational_rate); - - // Rotation - EvaluateRateToResistTo(wdot, VState.vPQR, vPQRdot, LocalTerrainAngularVelocity, - VState.dqPQRidot, dt, integrator_rotational_rate); - - // Prepare the linear system for the Gauss-Seidel algorithm : - // 1. Compute the right hand side member 'rhs' - // 2. Divide every line of 'a' and 'lhs' by a[i,i]. This is in order to save - // a division computation at each iteration of Gauss-Seidel. - for (int i=0; i < n; i++) { - double d = 1.0 / a[i*n+i]; - - rhs[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d; - for (int j=0; j < n; j++) - a[i*n+j] *= d; - } - - // Resolve the Lagrange multipliers with the projected Gauss-Seidel method - for (int iter=0; iter < 50; iter++) { - double norm = 0.; - - for (int i=0; i < n; i++) { - double lambda0 = lambda[i]; - double dlambda = rhs[i]; - - for (int j=0; j < n; j++) - dlambda -= a[i*n+j]*lambda[j]; - - lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]); - dlambda = lambda[i] - lambda0; - - norm += fabs(dlambda); - } - - if (norm < 1E-5) break; - } - - // Calculate the total friction forces and moments - - Fc.InitMatrix(); - Mc.InitMatrix(); - - for (int i=0; i< n; i++) { - Fc += lambda[i]*JacF[i]; - Mc += lambda[i]*JacM[i]; - } - - vUVWdot += invMass * Fc; - vUVWidot += invMass * Tb2i * Fc; - vPQRdot += Jinv * Mc; - vPQRidot += Tb2i* Jinv * Mc; - - // Save the value of the Lagrange multipliers to accelerate the convergence - // of the Gauss-Seidel algorithm at next iteration. - int i = 0; - for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it) - (*it)->value = lambda[i++]; - - FDMExec->GetGroundReactions()->UpdateForcesAndMoments(); + Integrand.Normalize(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGPropagate::UpdateLocationMatrices(void) { - Tl2ec = GetTl2ec(); // local to ECEF transform - Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform - Ti2l = GetTi2l(); - Tl2i = Ti2l.Transposed(); + Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform + Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform + Ti2l = VState.vLocation.GetTi2l(); // ECI to local frame transform + Tl2i = Ti2l.Transposed(); // local to ECI transform } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGPropagate::UpdateBodyMatrices(void) { - Ti2b = GetTi2b(); // ECI to body frame transform - Tb2i = Ti2b.Transposed(); // body to ECI frame transform - Tl2b = Ti2b*Tl2i; // local to body frame transform - Tb2l = Tl2b.Transposed(); // body to local frame transform - Tec2b = Tl2b * Tec2l; // ECEF to body frame transform - Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform + Ti2b = VState.qAttitudeECI.GetT(); // ECI to body frame transform + Tb2i = Ti2b.Transposed(); // body to ECI frame transform + Tl2b = Ti2b * Tl2i; // local to body frame transform + Tb2l = Tl2b.Transposed(); // body to local frame transform + Tec2b = Ti2b * Tec2i; // ECEF to body frame transform + Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropagate::SetInertialOrientation(FGQuaternion Qi) { +void FGPropagate::SetInertialOrientation(const FGQuaternion& Qi) { VState.qAttitudeECI = Qi; VState.qAttitudeECI.Normalize(); UpdateBodyMatrices(); @@ -649,115 +455,107 @@ void FGPropagate::SetInertialOrientation(FGQuaternion Qi) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) { +void FGPropagate::SetInertialVelocity(const FGColumnVector3& Vi) { VState.vInertialVelocity = Vi; CalculateUVW(); - vVel = GetTb2l() * VState.vUVW; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGPropagate::SetInertialRates(FGColumnVector3 vRates) { - VState.vPQRi_i = vRates; - VState.vPQRi = Ti2b * VState.vPQRi_i; - VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth; + vVel = Tb2l * VState.vUVW; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropagate::InitializeDerivatives(void) -{ - // Make an initial run and set past values - CalculatePQRdot(); // Angular rate derivative - CalculateUVWdot(); // Translational rate derivative - ResolveFrictionForces(0.); // Update rate derivatives with friction forces - CalculateQuatdot(); // Angular orientation derivative - CalculateInertialVelocity(); // Translational position derivative - - // Initialize past values deques - VState.dqPQRidot.clear(); - VState.dqUVWidot.clear(); - VState.dqInertialVelocity.clear(); - VState.dqQtrndot.clear(); - for (int i=0; i<4; i++) { - VState.dqPQRidot.push_front(vPQRidot); - VState.dqUVWidot.push_front(vUVWdot); - VState.dqInertialVelocity.push_front(VState.vInertialVelocity); - VState.dqQtrndot.push_front(vQtrndot); - } +void FGPropagate::SetInertialRates(const FGColumnVector3& vRates) { + VState.vPQRi = Ti2b * vRates; + VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropagate::RecomputeLocalTerrainRadius(void) +void FGPropagate::RecomputeLocalTerrainVelocity() { - FGLocation contactloc; - FGColumnVector3 dv; - double t = FDMExec->GetSimTime(); - - // Get the LocalTerrain radius. - FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, - LocalTerrainVelocity, LocalTerrainAngularVelocity); - LocalTerrainRadius = contactloc.GetRadius(); + FGLocation contact; + FGColumnVector3 normal; + VState.vLocation.GetContactPoint(FDMExec->GetSimTime(), contact, normal, + LocalTerrainVelocity, LocalTerrainAngularVelocity); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGPropagate::SetTerrainElevation(double terrainElev) { - LocalTerrainRadius = terrainElev + SeaLevelRadius; - FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius); + double radius = terrainElev + VState.vLocation.GetSeaLevelRadius(); + FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(radius); } + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGPropagate::GetTerrainElevation(void) const +void FGPropagate::SetSeaLevelRadius(double tt) { - return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius; + FDMExec->GetGroundCallback()->SetSeaLevelRadius(tt); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -//Todo: when should this be called - when should the new EPA be used to -// calculate the transformation matrix, so that the matrix is not a step -// ahead of the sim and the associated calculations? -const FGMatrix33& FGPropagate::GetTi2ec(void) + +double FGPropagate::GetLocalTerrainRadius(void) const { - return VState.vLocation.GetTi2ec(); + return VState.vLocation.GetTerrainRadius(FDMExec->GetSimTime()); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -const FGMatrix33& FGPropagate::GetTec2i(void) +double FGPropagate::GetDistanceAGL(void) const { - return VState.vLocation.GetTec2i(); + return VState.vLocation.GetAltitudeAGL(FDMExec->GetSimTime()); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropagate::SetAltitudeASL(double altASL) +void FGPropagate::SetDistanceAGL(double tt) { - VState.vLocation.SetRadius( altASL + SeaLevelRadius ); + VState.vLocation.SetAltitudeAGL(tt, FDMExec->GetSimTime()); + UpdateVehicleState(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGPropagate::GetLocalTerrainRadius(void) const +void FGPropagate::SetVState(const VehicleState& vstate) { - return LocalTerrainRadius; + //ToDo: Shouldn't all of these be set from the vstate vector passed in? + VState.vLocation = vstate.vLocation; + Ti2ec = VState.vLocation.GetTi2ec(); // useless ? + Tec2i = Ti2ec.Transposed(); + UpdateLocationMatrices(); + SetInertialOrientation(vstate.qAttitudeECI); + RecomputeLocalTerrainVelocity(); + VehicleRadius = GetRadius(); + VState.vUVW = vstate.vUVW; + vVel = Tb2l * VState.vUVW; + VState.vPQR = vstate.vPQR; + VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet; + VState.vInertialPosition = vstate.vInertialPosition; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGPropagate::GetDistanceAGL(void) const +void FGPropagate::UpdateVehicleState(void) { - return VState.vLocation.GetRadius() - LocalTerrainRadius; + RecomputeLocalTerrainVelocity(); + VehicleRadius = GetRadius(); + VState.vInertialPosition = Tec2i * VState.vLocation; + UpdateLocationMatrices(); + UpdateBodyMatrices(); + vVel = Tb2l * VState.vUVW; + VState.qAttitudeLocal = Tl2b.GetQuaternion(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropagate::SetDistanceAGL(double tt) +void FGPropagate::SetLocation(const FGLocation& l) { - VState.vLocation.SetRadius( tt + LocalTerrainRadius ); + VState.vLocation = l; + Ti2ec = VState.vLocation.GetTi2ec(); // useless ? + Tec2i = Ti2ec.Transposed(); + UpdateVehicleState(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -785,7 +583,7 @@ void FGPropagate::DumpState(void) cout << endl << " " << underon << "Velocity" << underoff << endl; cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl; - cout << " ECEF: " << (GetTb2ec() * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl; + cout << " ECEF: " << (Tb2ec * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl; cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl; cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl; @@ -821,20 +619,12 @@ void FGPropagate::bind(void) PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude); - PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot); - PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot); - PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot); - - PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot); - PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot); - PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot); - PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true); PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true); - PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude); - PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude); - PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg); - PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg); + PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude, false); + PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude, false); + PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg, false); + PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg, false); PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad); PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg); PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude); @@ -844,6 +634,7 @@ void FGPropagate::bind(void) &FGPropagate::GetTerrainElevation, &FGPropagate::SetTerrainElevation, false); + PropertyManager->Tie("position/epa-rad", this, &FGPropagate::GetEarthPositionAngle); PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius); PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler); @@ -853,12 +644,11 @@ void FGPropagate::bind(void) PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler); PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler); PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler); - + PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate); PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate); PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position); PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position); - PropertyManager->Tie("simulation/gravity-model", &gravType); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -896,12 +686,12 @@ void FGPropagate::Debug(int from) if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects } if (debug_lvl & 8 && from == 2) { // Runtime state variables - cout << endl << fgblue << highint << left + cout << endl << fgblue << highint << left << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds" << reset << endl; cout << endl; cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset - << FDMExec->GetInertial()->GetEarthPositionAngleDeg() << endl; + << GetEarthPositionAngleDeg() << endl; cout << endl; cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl; cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl; @@ -910,7 +700,7 @@ void FGPropagate::Debug(int from) cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl; cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl; cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl; - cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl; +// cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl; cout << endl; cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): " << reset << endl << Tec2b.Dump("\t", " ") << endl;