X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2Fmodels%2FFGPropagate.cpp;h=51bc472d22fbdd7ed4d49f72ff9129fd9b7a6c44;hb=a302cdc1cbb3c147e7c862b484cdd5d86f30a29c;hp=9bd50289882d378cae31575ecb69f13b59e308a2;hpb=50c1fe2d3e8bbfdb54873b213eb3a148cfa1f00d;p=flightgear.git diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index 9bd502898..51bc472d2 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -48,6 +48,7 @@ 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 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES @@ -56,10 +57,11 @@ INCLUDES #include #include #include +#include #include "FGPropagate.h" +#include "FGGroundReactions.h" #include "FGFDMExec.h" -#include "FGState.h" #include "FGAircraft.h" #include "FGMassBalance.h" #include "FGInertial.h" @@ -69,40 +71,40 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id$"; +static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.88 2011/05/20 03:18:36 jberndt 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"; - - last2_vPQRdot.InitMatrix(); - last_vPQRdot.InitMatrix(); - vPQRdot.InitMatrix(); - - last2_vQtrndot = FGQuaternion(0,0,0); - last_vQtrndot = FGQuaternion(0,0,0); + gravType = gtWGS84; + + vPQRidot.InitMatrix(); vQtrndot = FGQuaternion(0,0,0); + vUVWidot.InitMatrix(); + vInertialVelocity.InitMatrix(); - last2_vUVWdot.InitMatrix(); - last_vUVWdot.InitMatrix(); - vUVWdot.InitMatrix(); - - last2_vLocationDot.InitMatrix(); - last_vLocationDot.InitMatrix(); - vLocationDot.InitMatrix(); + /// These define the indices use to select the various integrators. + // eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4}; - vOmegaLocal.InitMatrix(); + integrator_rotational_rate = eRectEuler; + integrator_translational_rate = eAdamsBashforth2; + integrator_rotational_position = eRectEuler; + integrator_translational_position = eAdamsBashforth3; - integrator_rotational_rate = eAdamsBashforth2; - integrator_translational_rate = eTrapezoidal; - integrator_rotational_position = eAdamsBashforth2; - integrator_translational_position = eTrapezoidal; + 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)); bind(); Debug(0); @@ -119,37 +121,27 @@ FGPropagate::~FGPropagate(void) 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()); - vOmega = 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 - last2_vPQRdot.InitMatrix(); - last_vPQRdot.InitMatrix(); - vPQRdot.InitMatrix(); - - last2_vQtrndot = FGQuaternion(0,0,0); - last_vQtrndot = FGQuaternion(0,0,0); + vPQRidot.InitMatrix(); vQtrndot = FGQuaternion(0,0,0); + vUVWidot.InitMatrix(); + vInertialVelocity.InitMatrix(); - last2_vUVWdot.InitMatrix(); - last_vUVWdot.InitMatrix(); - vUVWdot.InitMatrix(); - - last2_vLocationDot.InitMatrix(); - last_vLocationDot.InitMatrix(); - vLocationDot.InitMatrix(); - - vOmegaLocal.InitMatrix(); + 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)); - 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; } @@ -161,51 +153,55 @@ 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() ); + FGIC->GetLatitudeRadIC(), + FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() ); - VehicleRadius = GetRadius(); - radInv = 1.0/VehicleRadius; + VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); + + Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform + Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform + + VState.vInertialPosition = Tec2i * VState.vLocation; + + UpdateLocationMatrices(); - // Set the Orientation from the euler angles - VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(), - FGIC->GetThetaRadIC(), - FGIC->GetPsiRadIC() ); + // 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.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal; + UpdateBodyMatrices(); // 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() ); + FGIC->GetVBodyFpsIC(), + FGIC->GetWBodyFpsIC() ); // Compute the local frame ECEF velocity - vVel = GetTb2l()*VState.vUVW; - - // Finally, make sure that the quaternion stays normalized. - VState.vQtrn.Normalize(); + vVel = Tb2l * VState.vUVW; // Recompute the LocalTerrainRadius. RecomputeLocalTerrainRadius(); - // These local copies of the transformation matrices are for use for - // initial conditions 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 + VehicleRadius = GetRadius(); + + // Set the angular velocities of the body frame relative to the ECEF frame, + // expressed in the body frame. + VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(), + FGIC->GetQRadpsIC(), + FGIC->GetRRadpsIC() ); + + VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth; + + // Make an initial run and set past values + InitializeDerivatives(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -228,125 +224,68 @@ 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; + if (FGModel::Run(Holding)) return true; // Fast return if we have nothing to do ... + if (Holding) return false; + + double dt = FDMExec->GetDeltaT()*rate; // The 'stepsize' RunPreFunctions(); - RecomputeLocalTerrainRadius(); + // Calculate state derivatives + CalculatePQRdot(); // Angular rate derivative + CalculateUVWdot(); // Translational rate derivative + ResolveFrictionForces(dt); // Update rate derivatives with friction forces + CalculateQuatdot(); // Angular orientation derivative - // Calculate current aircraft radius from center of planet + // Propagate rotational / translational velocity, angular /translational position, respectively. - 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 + Integrate(VState.vPQRi, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate); + 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); - // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame. - vVel = Tb2l * VState.vUVW; + // CAUTION : the order of the operations below is very important to get transformation + // matrices that are consistent with the new state of the vehicle - // Inertial angular velocity measured in the body frame. - vPQRi = VState.vPQR + Tec2b*vOmega; + // 1. Update the Earth position angle (EPA) + VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); - // Calculate state derivatives - CalculatePQRdot(); // Angular rate derivative - CalculateUVWdot(); // Translational rate derivative - CalculateQuatdot(); // Angular orientation derivative - CalculateLocationdot(); // Translational position derivative + // 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 - // Integrate to propagate the state + // 3. Update the location from the updated Ti2ec and inertial position + VState.vLocation = Ti2ec*VState.vInertialPosition; - double dt = State->Getdt()*rate; // The 'stepsize' + // 4. Update the other "Location-based" transformation matrices from the updated + // vLocation vector. + UpdateLocationMatrices(); - // Propagate rotational velocity + // 5. Normalize the ECI Attitude quaternion + VState.qAttitudeECI.Normalize(); - 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 + // 6. Update the "Orientation-based" transformation matrices from the updated + // orientation quaternion and vLocation vector. + UpdateBodyMatrices(); - 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; - } + // Translational position derivative (velocities are integrated in the inertial frame) + CalculateUVW(); - // Propagate angular position + // Set auxilliary state variables + RecomputeLocalTerrainRadius(); - 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; - } + VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet - // Propagate translational position + VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth; - 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; + VState.qAttitudeLocal = Tl2b.GetQuaternion(); - last2_vLocationDot = last_vLocationDot; - last_vLocationDot = vLocationDot; + // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame. + vVel = Tb2l * VState.vUVW; - RunPreFunctions(); + RunPostFunctions(); Debug(2); return false; @@ -361,22 +300,23 @@ bool FGPropagate::Run(void) // J is the inertia matrix // Jinv is the inverse inertia matrix // vMoments is the moment vector in the body frame -// vPQRi is the total inertial angular velocity of the vehicle +// 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 = 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 - vPQRi*(J*vPQRi)); + vPQRidot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi)); + vPQRdot = vPQRidot - VState.vPQRi * (Ti2b * vOmegaEarth); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -388,72 +328,370 @@ void FGPropagate::CalculatePQRdot(void) void FGPropagate::CalculateQuatdot(void) { - vOmegaLocal.InitMatrix( radInv*vVel(eEast), - -radInv*vVel(eNorth), - -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() ); - // Compute quaternion orientation derivative on current body rates - vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*vOmegaLocal); + vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// This set of calculations results in the body frame accelerations being -// computed. +// 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), eqn 1.5-16d (page 50) +// Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50) 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; - const FGColumnVector3 vGravAccel( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) ); + // Include Centripetal acceleration. + vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition)); - // Begin to compute body frame accelerations based on the current body forces - vUVWdot = vForces/mass - VState.vPQR * VState.vUVW; + // 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; + } - // Include Coriolis acceleration. - vUVWdot -= 2.0 * (Ti2b *vOmega) * VState.vUVW; + vUVWdot += vGravAccel; + vUVWidot = Tb2i * (vForces/mass + vGravAccel); +} - // Include Centrifugal acceleration. - if (!GroundReactions->GetWOW()) { - vUVWdot -= Ti2b*(vOmega*(vOmega*(Tec2i*VState.vLocation))); +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + // 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", + // Second edition (2004), eqn 1.5-16c (page 50) + +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, + FGColumnVector3& Val, + deque & ValDot, + double dt, + eIntegrateType integration_type) +{ + ValDot.push_front(Val); + ValDot.pop_back(); + + switch(integration_type) { + case eRectEuler: Integrand += dt*ValDot[0]; + break; + case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]); + break; + case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]); + break; + case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]); + 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 eNone: // do nothing, freeze translational rate + break; } +} - // Include Gravitation accel - FGColumnVector3 gravAccel = Tl2b*vGravAccel; - vUVWdot += gravAccel; +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPropagate::Integrate( FGQuaternion& Integrand, + FGQuaternion& Val, + deque & ValDot, + double dt, + eIntegrateType integration_type) +{ + ValDot.push_front(Val); + ValDot.pop_back(); + + switch(integration_type) { + case eRectEuler: Integrand += dt*ValDot[0]; + break; + case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]); + break; + case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]); + break; + case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]); + 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 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: + break; + } +} -void FGPropagate::CalculateLocationdot(void) +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// 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) { - // Transform the vehicle velocity relative to the ECEF frame, expressed - // in the body frame, to be expressed in the ECEF frame. - vLocationDot = Tb2ec * VState.vUVW; + 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); + } - // Now, 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. The above velocity is only - // relative to the rotating ECEF frame. - // Reference: See Stevens and Lewis, "Aircraft Control and Simulation", - // Second edition (2004), eqn 1.5-16c (page 50) + // If no gears are in contact with the ground then return + if (!n) return; - vInertialVelocity = Tec2i * vLocationDot + (vOmega * (Tec2i * VState.vLocation)); + 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 '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]; + + 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 += 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(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropagate::RecomputeLocalTerrainRadius(void) +void FGPropagate::UpdateLocationMatrices(void) { - double t = State->Getsim_time(); + 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 +} - // Get the LocalTerrain radius. +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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 = Ti2b * Tec2i; // 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 = Ti2b * vRates; + 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(vUVWidot); + VState.dqInertialVelocity.push_front(VState.vInertialVelocity); + VState.dqQtrndot.push_front(vQtrndot); + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPropagate::RecomputeLocalTerrainRadius(void) +{ FGLocation contactloc; FGColumnVector3 dv; - FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv); - LocalTerrainRadius = contactloc.GetRadius(); + double t = FDMExec->GetSimTime(); + + // Get the LocalTerrain radius. + FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, + LocalTerrainVelocity, LocalTerrainAngularVelocity); + LocalTerrainRadius = contactloc.GetRadius(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -473,44 +711,89 @@ double FGPropagate::GetTerrainElevation(void) const //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -const FGMatrix33& FGPropagate::GetTi2ec(void) +double FGPropagate::GetDistanceAGL(void) const { - return VState.vLocation.GetTi2ec(Inertial->GetEarthPositionAngle()); + return VState.vLocation.GetRadius() - LocalTerrainRadius; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -const FGMatrix33& FGPropagate::GetTec2i(void) +void FGPropagate::SetVState(const VehicleState& vstate) { - return VState.vLocation.GetTec2i(Inertial->GetEarthPositionAngle()); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + 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.vInertialPosition = vstate.vInertialPosition; -void FGPropagate::SetAltitudeASL(double altASL) -{ - VState.vLocation.SetRadius( altASL + SeaLevelRadius ); + 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; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -518,7 +801,7 @@ void FGPropagate::SetDistanceAGL(double tt) void FGPropagate::bind(void) { typedef double (FGPropagate::*PMF)(int) const; -// typedef double (FGPropagate::*dPMF)() const; + PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot); PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel); @@ -572,10 +855,11 @@ void FGPropagate::bind(void) 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", &integrator_rotational_rate); - PropertyManager->Tie("simulation/integrator/rate/translational", &integrator_translational_rate); - PropertyManager->Tie("simulation/integrator/position/rotational", &integrator_rotational_position); - PropertyManager->Tie("simulation/integrator/position/translational", &integrator_translational_position); + 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); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -612,7 +896,96 @@ void FGPropagate::Debug(int from) } if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects } - if (debug_lvl & 8 ) { // Runtime state variables + if (debug_lvl & 8 && from == 2) { // Runtime state variables + 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; + 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; + cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl; + cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl; + 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 << endl; + cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): " + << reset << endl << Tec2b.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):" + << reset << endl << Tb2ec.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):" + << reset << endl << Tl2b.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):" + << reset << endl << Tb2l.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):" + << reset << endl << Tl2ec.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):" + << reset << endl << Tec2l.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):" + << reset << endl << Tec2i.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):" + << reset << endl << Ti2ec.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):" + << reset << endl << Ti2b.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):" + << reset << endl << Tb2i.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):" + << reset << endl << Ti2l.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):" + << reset << endl << Tl2i.Dump("\t", " ") << endl; + cout << highint << " Associated Euler angles (deg): " << setw(8) + << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg) + << endl << endl; + + cout << setprecision(6); // reset the output stream } if (debug_lvl & 16) { // Sanity checking if (from == 2) { // State sanity checking