X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2Fmodels%2FFGPropagate.cpp;h=51bc472d22fbdd7ed4d49f72ff9129fd9b7a6c44;hb=a302cdc1cbb3c147e7c862b484cdd5d86f30a29c;hp=5405c863983c2241b03487527ded86bec28e22ff;hpb=674a295896a1e56d605f39874262d6f146a586a3;p=flightgear.git diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index 5405c8639..51bc472d2 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -71,28 +71,35 @@ 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.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"; gravType = gtWGS84; - vPQRdot.InitMatrix(); + vPQRidot.InitMatrix(); vQtrndot = FGQuaternion(0,0,0); - vUVWdot.InitMatrix(); + vUVWidot.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)); @@ -114,8 +121,6 @@ FGPropagate::~FGPropagate(void) bool FGPropagate::InitModel(void) { - if (!FGModel::InitModel()) return false; - // For initialization ONLY: SeaLevelRadius = LocalTerrainRadius = FDMExec->GetInertial()->GetRefRadius(); @@ -123,9 +128,9 @@ bool FGPropagate::InitModel(void) VState.vLocation.SetEllipse(FDMExec->GetInertial()->GetSemimajor(), FDMExec->GetInertial()->GetSemiminor()); vOmegaEarth = FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->omega() ); // Earth rotation vector - vPQRdot.InitMatrix(); + vPQRidot.InitMatrix(); vQtrndot = FGQuaternion(0,0,0); - vUVWdot.InitMatrix(); + vUVWidot.InitMatrix(); vInertialVelocity.InitMatrix(); VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0)); @@ -133,10 +138,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; } @@ -157,8 +162,8 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); - 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; @@ -186,25 +191,14 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) 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. - - FGColumnVector3 vOmegaLocal = FGColumnVector3( - radInv*vVel(eEast), - -radInv*vVel(eNorth), - -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() ); // 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 + // expressed in the body frame. VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(), FGIC->GetQRadpsIC(), - FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal; + FGIC->GetRRadpsIC() ); VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth; - VState.vPQRi_i = Tb2i * VState.vPQRi; // Make an initial run and set past values InitializeDerivatives(); @@ -230,10 +224,10 @@ 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' @@ -244,11 +238,10 @@ bool FGPropagate::Run(void) 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) // Propagate rotational / translational velocity, angular /translational position, respectively. - Integrate(VState.vPQRi_i, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate); // ECI integration + 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); @@ -260,8 +253,8 @@ bool FGPropagate::Run(void) VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); // 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; @@ -277,12 +270,14 @@ bool FGPropagate::Run(void) // orientation quaternion and vLocation vector. UpdateBodyMatrices(); - // Set auxililary state variables + // Translational position derivative (velocities are integrated in the inertial frame) + CalculateUVW(); + + // Set auxilliary state variables RecomputeLocalTerrainRadius(); VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet - VState.vPQRi = Ti2b * VState.vPQRi_i; VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth; VState.qAttitudeLocal = Tl2b.GetQuaternion(); @@ -320,8 +315,8 @@ void FGPropagate::CalculatePQRdot(void) // moments and the total inertial angular velocity expressed in the body // frame. - vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi)); - vPQRidot = Tb2i * vPQRdot; + vPQRidot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi)); + vPQRdot = vPQRidot - VState.vPQRi * (Ti2b * vOmegaEarth); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -561,7 +556,7 @@ void FGPropagate::ResolveFrictionForces(double dt) // 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 + // 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]; @@ -604,7 +599,7 @@ void FGPropagate::ResolveFrictionForces(double dt) vUVWdot += invMass * Fc; vUVWidot += invMass * Tb2i * Fc; vPQRdot += Jinv * Mc; - vPQRidot += Tb2i* Jinv * Mc; + vPQRidot += Jinv * Mc; // Save the value of the Lagrange multipliers to accelerate the convergence // of the Gauss-Seidel algorithm at next iteration. @@ -619,22 +614,22 @@ void FGPropagate::ResolveFrictionForces(double dt) 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 } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -651,14 +646,13 @@ void FGPropagate::SetInertialOrientation(FGQuaternion Qi) { void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) { VState.vInertialVelocity = Vi; CalculateUVW(); - vVel = GetTb2l() * VState.vUVW; + vVel = Tb2l * VState.vUVW; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGPropagate::SetInertialRates(FGColumnVector3 vRates) { - VState.vPQRi_i = vRates; - VState.vPQRi = Ti2b * VState.vPQRi_i; + VState.vPQRi = Ti2b * vRates; VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth; } @@ -680,7 +674,7 @@ void FGPropagate::InitializeDerivatives(void) VState.dqQtrndot.clear(); for (int i=0; i<4; i++) { VState.dqPQRidot.push_front(vPQRidot); - VState.dqUVWidot.push_front(vUVWdot); + VState.dqUVWidot.push_front(vUVWidot); VState.dqInertialVelocity.push_front(VState.vInertialVelocity); VState.dqQtrndot.push_front(vQtrndot); } @@ -716,47 +710,55 @@ double FGPropagate::GetTerrainElevation(void) const } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -//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.vInertialPosition = vstate.vInertialPosition; -double FGPropagate::GetLocalTerrainRadius(void) const -{ - return LocalTerrainRadius; + InitializeDerivatives(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGPropagate::GetDistanceAGL(void) const +void FGPropagate::UpdateVehicleState(void) { - return VState.vLocation.GetRadius() - LocalTerrainRadius; + RecomputeLocalTerrainRadius(); + 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; + VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); + Ti2ec = VState.vLocation.GetTi2ec(); // useless ? + Tec2i = Ti2ec.Transposed(); + UpdateVehicleState(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -784,7 +786,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;