X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2Fmodels%2FFGPropagate.cpp;h=4d1ef44a858a60d532d336d532d30f10673ced8c;hb=4f364af6d178d947eae1a5a751e3a9542b270069;hp=ebb83211b4d010c48f24aae19e04011525f2e0a1;hpb=63b3f802ec9412b47c089d7c82f740a6c41f3e3e;p=flightgear.git diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index ebb83211b..4d1ef44a8 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -71,14 +71,15 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.60 2010/08/12 19:11:22 andgi Exp $"; +static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.76 2011/01/16 16:10:59 bcoconni Exp $"; static const char *IdHdr = ID_PROPAGATE; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex) +FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex), +LocalTerrainRadius(0), SeaLevelRadius(0), VehicleRadius(0) { Debug(0); Name = "FGPropagate"; @@ -94,8 +95,8 @@ FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex) integrator_rotational_position = eAdamsBashforth2; integrator_translational_position = eTrapezoidal; - VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0)); - VState.dqUVWdot.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.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0)); VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0)); @@ -117,19 +118,19 @@ bool FGPropagate::InitModel(void) if (!FGModel::InitModel()) return false; // For initialization ONLY: - SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius(); + SeaLevelRadius = LocalTerrainRadius = FDMExec->GetInertial()->GetRefRadius(); VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 ); - VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor()); - vOmegaEarth = FGColumnVector3( 0.0, 0.0, Inertial->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 vPQRdot.InitMatrix(); vQtrndot = FGQuaternion(0,0,0); vUVWdot.InitMatrix(); vInertialVelocity.InitMatrix(); - VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0)); - VState.dqUVWdot.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.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0)); VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0)); @@ -148,9 +149,6 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC()); SetTerrainElevation(FGIC->GetTerrainElevationFtIC()); - VehicleRadius = GetRadius(); - radInv = 1.0/VehicleRadius; - // Initialize the State Vector elements and the transformation matrices // Set the position lat/lon/radius @@ -158,16 +156,14 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) FGIC->GetLatitudeRadIC(), FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() ); - VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle()); + VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); - Ti2ec = GetTi2ec(); // ECI to ECEF transform - Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform - - Tl2ec = GetTl2ec(); // local to ECEF transform - Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform + Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform + Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform + + VState.vInertialPosition = Tec2i * VState.vLocation; - Ti2l = GetTi2l(); - Tl2i = Ti2l.Transposed(); + UpdateLocationMatrices(); // Set the orientation from the euler angles (is normalized within the // constructor). The Euler angles represent the orientation of the body @@ -177,26 +173,22 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) FGIC->GetPsiRadIC() ); VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal; - - Ti2b = GetTi2b(); // ECI to body frame transform - Tb2i = Ti2b.Transposed(); // body to ECI frame transform - - Tl2b = VState.qAttitudeLocal; // 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 + UpdateBodyMatrices(); // Set the velocities in the instantaneus body frame VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(), FGIC->GetVBodyFpsIC(), FGIC->GetWBodyFpsIC() ); - VState.vInertialPosition = Tec2i * VState.vLocation; - // Compute the local frame ECEF velocity vVel = Tb2l * VState.vUVW; + // Recompute the LocalTerrainRadius. + RecomputeLocalTerrainRadius(); + + 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. @@ -213,28 +205,10 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal; VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth; + VState.vPQRi_i = Tb2i * VState.vPQRi; // 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.dqPQRdot.clear(); - VState.dqUVWdot.clear(); - VState.dqInertialVelocity.clear(); - VState.dqQtrndot.clear(); - for (int i=0; i<4; i++) { - VState.dqPQRdot.push_front(vPQRdot); - VState.dqUVWdot.push_front(vUVWdot); - VState.dqInertialVelocity.push_front(VState.vInertialVelocity); - VState.dqQtrndot.push_front(vQtrndot); - } - - // Recompute the LocalTerrainRadius. - RecomputeLocalTerrainRadius(); + InitializeDerivatives(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -271,43 +245,45 @@ bool FGPropagate::Run(void) CalculateUVWdot(); // Translational rate derivative ResolveFrictionForces(dt); // Update rate derivatives with friction forces CalculateQuatdot(); // Angular orientation derivative - CalculateInertialVelocity(); // Translational position derivative + CalculateUVW(); // Translational position derivative (velocities are integrated in the inertial frame) // Propagate rotational / translational velocity, angular /translational position, respectively. - Integrate(VState.vPQRi, vPQRdot, VState.dqPQRdot, dt, integrator_rotational_rate); - Integrate(VState.vUVW, vUVWdot, VState.dqUVWdot, dt, integrator_translational_rate); + + 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.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position); + Integrate(VState.vInertialVelocity, vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate); - VState.qAttitudeECI.Normalize(); // Normalize the ECI Attitude quaternion + // CAUTION : the order of the operations below is very important to get transformation + // matrices that are consistent with the new state of the vehicle - VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle()); // Update the Earth position angle (EPA) + // 1. Update the Earth position angle (EPA) + VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); - // Update the "Location-based" transformation matrices from the vLocation vector. + // 2. Update the Ti2ec and Tec2i transforms from the updated EPA + Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform + Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform - Ti2ec = GetTi2ec(); // ECI to ECEF transform - Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform - Tl2ec = GetTl2ec(); // local to ECEF transform - Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform - Ti2l = GetTi2l(); - Tl2i = Ti2l.Transposed(); + // 3. Update the location from the updated Ti2ec and inertial position + VState.vLocation = Ti2ec*VState.vInertialPosition; + + // 4. Update the other "Location-based" transformation matrices from the updated + // vLocation vector. + UpdateLocationMatrices(); - // Update the "Orientation-based" transformation matrices from the orientation quaternion + // 5. Normalize the ECI Attitude quaternion + VState.qAttitudeECI.Normalize(); - 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 + // 6. Update the "Orientation-based" transformation matrices from the updated + // orientation quaternion and vLocation vector. + UpdateBodyMatrices(); // Set auxililary state variables - VState.vLocation = Ti2ec*VState.vInertialPosition; RecomputeLocalTerrainRadius(); VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet - radInv = 1.0/VehicleRadius; + VState.vPQRi = Ti2b * VState.vPQRi_i; VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth; VState.qAttitudeLocal = Tl2b.GetQuaternion(); @@ -337,15 +313,16 @@ bool FGPropagate::Run(void) void FGPropagate::CalculatePQRdot(void) { - const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments - const FGMatrix33& J = MassBalance->GetJ(); // inertia matrix - const FGMatrix33& Jinv = MassBalance->GetJinv(); // inertia matrix inverse + 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; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -362,10 +339,10 @@ void FGPropagate::CalculateQuatdot(void) } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// This set of calculations results in the body frame accelerations being -// computed. -// Compute body frame accelerations based on the current body forces. -// Include centripetal and coriolis accelerations. +// 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 @@ -380,8 +357,8 @@ void FGPropagate::CalculateQuatdot(void) void FGPropagate::CalculateUVWdot(void) { - double mass = MassBalance->GetMass(); // mass - const FGColumnVector3& vForces = Aircraft->GetForces(); // current forces + 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; @@ -391,14 +368,15 @@ void FGPropagate::CalculateUVWdot(void) // Include Gravitation accel switch (gravType) { case gtStandard: - vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) ); + vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->GetGAccel(VehicleRadius) ); break; case gtWGS84: - vGravAccel = Tec2b * Inertial->GetGravityJ2(VState.vLocation); + vGravAccel = Tec2b * FDMExec->GetInertial()->GetGravityJ2(VState.vLocation); break; } vUVWdot += vGravAccel; + vUVWidot = Tb2i * (vForces/mass + vGravAccel); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -413,6 +391,16 @@ void FGPropagate::CalculateInertialVelocity(void) VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition); } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + // Transform the velocity vector of the inertial frame to be expressed in the + // body frame relative to the origin (Earth center), and substract the vehicle + // velocity contribution due to the rotation of the planet. + +void FGPropagate::CalculateUVW(void) +{ + VState.vUVW = Ti2b * (VState.vInertialVelocity - (vOmegaEarth * VState.vInertialPosition)); +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGPropagate::Integrate( FGColumnVector3& Integrand, @@ -467,6 +455,55 @@ void FGPropagate::Integrate( FGQuaternion& Integrand, } } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// 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: + break; + } +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // Resolves the contact forces just before integrating the EOM. // This routine is using Lagrange multipliers and the projected Gauss-Seidel @@ -477,68 +514,60 @@ void FGPropagate::Integrate( FGQuaternion& Integrand, // 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 / MassBalance->GetMass(); - const FGMatrix33& Jinv = MassBalance->GetJinv(); + 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, i; + int n = 0; // Compiles data from the ground reactions to build up the jacobian matrix - for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, n++) { + 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 eta(n); - vector lambda(n); - vector lambdaMin(n); - vector lambdaMax(n); - - // Initializes the Lagrange multipliers - i = 0; - for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, i++) { - lambda[i] = (*it)->value; - lambdaMax[i] = (*it)->Max; - lambdaMin[i] = (*it)->Min; - } - - vdot = vUVWdot; - wdot = vPQRdot; - - if (dt > 0.) { - // First compute the ground velocity below the aircraft center of gravity - FGLocation contact; - FGColumnVector3 normal, cvel; - - // Instruct the algorithm to zero out the relative movement between the - // aircraft and the ground. - vdot += (VState.vUVW - Tec2b * cvel) / dt; - wdot += VState.vPQR / dt; - } + vector rhs(n); // Assemble the linear system of equations - for (i=0; i < n; i++) { + 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 : - // divide every line of 'a' and eta by a[i,i]. This is in order to save - // a division computation at each iteration of Gauss-Seidel. - for (i=0; i < n; i++) { + // 1. Compute the right hand side member 'rhs' + // 2. Divide every line of 'a' and 'rhs' 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]; - eta[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d; + rhs[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d; for (int j=0; j < n; j++) a[i*n+j] *= d; } @@ -547,9 +576,9 @@ void FGPropagate::ResolveFrictionForces(double dt) for (int iter=0; iter < 50; iter++) { double norm = 0.; - for (i=0; i < n; i++) { + for (int i=0; i < n; i++) { double lambda0 = lambda[i]; - double dlambda = eta[i]; + double dlambda = rhs[i]; for (int j=0; j < n; j++) dlambda -= a[i*n+j]*lambda[j]; @@ -568,43 +597,107 @@ void FGPropagate::ResolveFrictionForces(double dt) Fc.InitMatrix(); Mc.InitMatrix(); - for (i=0; i< n; i++) { + 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. - i = 0; - for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it) + int i = 0; + for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it) (*it)->value = lambda[i++]; - GroundReactions->UpdateForcesAndMoments(); + FDMExec->GetGroundReactions()->UpdateForcesAndMoments(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPropagate::UpdateLocationMatrices(void) +{ + 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 = 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 = Tl2b * Tec2l; // ECEF to body frame transform + Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGPropagate::SetInertialOrientation(FGQuaternion Qi) { VState.qAttitudeECI = Qi; + VState.qAttitudeECI.Normalize(); + UpdateBodyMatrices(); + VState.qAttitudeLocal = Tl2b.GetQuaternion(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) { VState.vInertialVelocity = Vi; + CalculateUVW(); + vVel = Tb2l * VState.vUVW; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPropagate::SetInertialRates(FGColumnVector3 vRates) { + VState.vPQRi_i = vRates; + VState.vPQRi = Ti2b * VState.vPQRi_i; + VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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::RecomputeLocalTerrainRadius(void) { + FGLocation contactloc; + FGColumnVector3 dv; double t = FDMExec->GetSimTime(); // Get the LocalTerrain radius. - FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv); + FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, + LocalTerrainVelocity, LocalTerrainAngularVelocity); LocalTerrainRadius = contactloc.GetRadius(); } @@ -623,48 +716,92 @@ double FGPropagate::GetTerrainElevation(void) const return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius; } -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -//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) -{ - return VState.vLocation.GetTi2ec(); -} - //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -const FGMatrix33& FGPropagate::GetTec2i(void) +double FGPropagate::GetDistanceAGL(void) const { - return VState.vLocation.GetTec2i(); + return VState.vLocation.GetRadius() - LocalTerrainRadius; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropagate::SetAltitudeASL(double altASL) +void FGPropagate::SetVState(const VehicleState& vstate) { - VState.vLocation.SetRadius( altASL + SeaLevelRadius ); + VState.vLocation = vstate.vLocation; + VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); + Ti2ec = VState.vLocation.GetTi2ec(); // useless ? + Tec2i = Ti2ec.Transposed(); + UpdateLocationMatrices(); + SetInertialOrientation(vstate.qAttitudeECI); + RecomputeLocalTerrainRadius(); + VehicleRadius = GetRadius(); + VState.vUVW = vstate.vUVW; + vVel = Tb2l * VState.vUVW; + VState.vPQR = vstate.vPQR; + VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth; + VState.vPQRi_i = Tb2i * VState.vPQRi; + VState.vInertialPosition = vstate.vInertialPosition; + + InitializeDerivatives(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGPropagate::GetLocalTerrainRadius(void) const +void FGPropagate::UpdateVehicleState(void) { - return LocalTerrainRadius; + RecomputeLocalTerrainRadius(); + VehicleRadius = GetRadius(); + VState.vInertialPosition = Tec2i * VState.vLocation; + UpdateLocationMatrices(); + UpdateBodyMatrices(); + vVel = Tb2l * VState.vUVW; + VState.qAttitudeLocal = Tl2b.GetQuaternion(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGPropagate::GetDistanceAGL(void) const +void FGPropagate::SetLocation(const FGLocation& l) { - return VState.vLocation.GetRadius() - LocalTerrainRadius; + VState.vLocation = l; + VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); + Ti2ec = VState.vLocation.GetTi2ec(); // useless ? + Tec2i = Ti2ec.Transposed(); + UpdateVehicleState(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropagate::SetDistanceAGL(double tt) +void FGPropagate::DumpState(void) { - VState.vLocation.SetRadius( tt + LocalTerrainRadius ); + cout << endl; + cout << fgblue + << "------------------------------------------------------------------" << reset << endl; + cout << highint + << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl; + cout << " " << underon + << "Position" << underoff << endl; + cout << " ECI: " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl; + cout << " ECEF: " << VState.vLocation << " (x,y,z, in ft)" << endl; + cout << " Local: " << VState.vLocation.GetLatitudeDeg() + << ", " << VState.vLocation.GetLongitudeDeg() + << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl; + + cout << endl << " " << underon + << "Orientation" << underoff << endl; + cout << " ECI: " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl; + cout << " Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl; + + cout << endl << " " << underon + << "Velocity" << underoff << endl; + cout << " ECI: " << VState.vInertialVelocity.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; + + cout << endl << " " << underon + << "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl; + cout << " ECI: " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl; + cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -773,7 +910,7 @@ void FGPropagate::Debug(int from) << reset << endl; cout << endl; cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset - << Inertial->GetEarthPositionAngleDeg() << endl; + << FDMExec->GetInertial()->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;