From: Bertrand Coconnier Date: Sat, 16 Apr 2016 12:49:12 +0000 (+0200) Subject: JSBSim: - Fixed the forces/hold-down feature. The velocities are now zeroed when... X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=43f2daa7aedf358ef652c6e1ffb85870705b5dde;p=flightgear.git JSBSim: - Fixed the forces/hold-down feature. The velocities are now zeroed when the property is set which should prevent aircraft from drifting as has been reported by G. Agostinho. - Fixed a bug that was skipping the initialization. --- diff --git a/src/FDM/JSBSim/FGFDMExec.cpp b/src/FDM/JSBSim/FGFDMExec.cpp index 2aaf597dd..8829b6787 100644 --- a/src/FDM/JSBSim/FGFDMExec.cpp +++ b/src/FDM/JSBSim/FGFDMExec.cpp @@ -72,7 +72,7 @@ using namespace std; namespace JSBSim { -IDENT(IdSrc,"$Id: FGFDMExec.cpp,v 1.187 2016/01/31 11:12:59 bcoconni Exp $"); +IDENT(IdSrc,"$Id: FGFDMExec.cpp,v 1.189 2016/04/16 12:24:39 bcoconni Exp $"); IDENT(IdHdr,ID_FDMEXEC); /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -100,6 +100,7 @@ FGFDMExec::FGFDMExec(FGPropertyManager* root, unsigned int* fdmctr) : Root(root) StandAlone = false; ResetMode = 0; RandomSeed = 0; + HoldDown = false; IncrementThenHolding = false; // increment then hold is off by default TimeStepsUntilHold = -1; @@ -173,6 +174,7 @@ FGFDMExec::FGFDMExec(FGPropertyManager* root, unsigned int* fdmctr) : Root(root) instance->Tie("simulation/jsbsim-debug", this, &FGFDMExec::GetDebugLevel, &FGFDMExec::SetDebugLevel); instance->Tie("simulation/frame", (int *)&Frame, false); instance->Tie("simulation/trim-completed", (int *)&trim_completed, false); + instance->Tie("forces/hold-down", this, &FGFDMExec::GetHoldDown, &FGFDMExec::SetHoldDown); Constructing = false; } @@ -345,7 +347,6 @@ void FGFDMExec::LoadInputs(unsigned int idx) switch(idx) { case ePropagate: Propagate->in.vPQRidot = Accelerations->GetPQRidot(); - Propagate->in.vQtrndot = Accelerations->GetQuaterniondot(); Propagate->in.vUVWidot = Accelerations->GetUVWidot(); Propagate->in.DeltaT = dT; break; @@ -497,7 +498,6 @@ void FGFDMExec::LoadInputs(unsigned int idx) Accelerations->in.Tb2i = Propagate->GetTb2i(); Accelerations->in.Tec2b = Propagate->GetTec2b(); Accelerations->in.Tec2i = Propagate->GetTec2i(); - Accelerations->in.qAttitudeECI = Propagate->GetQuaternionECI(); Accelerations->in.Moment = Aircraft->GetMoments(); Accelerations->in.GroundMoment = GroundReactions->GetMoments(); Accelerations->in.Force = Aircraft->GetForces(); @@ -622,6 +622,19 @@ void FGFDMExec::ResetToInitialConditions(int mode) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +void FGFDMExec::SetHoldDown(bool hd) +{ + HoldDown = hd; + Accelerations->SetHoldDown(hd); + if (hd) { + Propagate->in.vPQRidot = Accelerations->GetPQRidot(); + Propagate->in.vUVWidot = Accelerations->GetUVWidot(); + } + Propagate->SetHoldDown(hd); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + vector FGFDMExec::EnumerateFDMs(void) { vector FDMList; diff --git a/src/FDM/JSBSim/FGFDMExec.h b/src/FDM/JSBSim/FGFDMExec.h index d68a0f069..03e3ae799 100644 --- a/src/FDM/JSBSim/FGFDMExec.h +++ b/src/FDM/JSBSim/FGFDMExec.h @@ -54,7 +54,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_FDMEXEC "$Id: FGFDMExec.h,v 1.104 2015/12/13 07:54:48 bcoconni Exp $" +#define ID_FDMEXEC "$Id: FGFDMExec.h,v 1.105 2016/04/16 12:24:39 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -178,7 +178,7 @@ CLASS DOCUMENTATION property actually maps toa function call of DoTrim(). @author Jon S. Berndt - @version $Revision: 1.104 $ + @version $Revision: 1.105 $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -578,6 +578,17 @@ public: @param FGIC The initial conditions that will be passed to the simulation. */ void Initialize(FGInitialCondition *FGIC); + /** Sets the property forces/hold-down. This allows to do hard 'hold-down' + such as for rockets on a launch pad with engines ignited. + @param hd enables the 'hold-down' function if non-zero + */ + void SetHoldDown(bool hd); + + /** Gets the value of the property forces/hold-down. + @result zero if the 'hold-down' function is disabled, non-zero otherwise. + */ + bool GetHoldDown(void) const {return HoldDown;} + private: int Error; unsigned int Frame; @@ -632,7 +643,9 @@ private: FGPropertyManager* Root; bool StandAlone; FGPropertyManager* instance; - + + bool HoldDown; + // The FDM counter is used to give each child FDM an unique ID. The root FDM has the ID 0 unsigned int* FDMctr; diff --git a/src/FDM/JSBSim/models/FGAccelerations.cpp b/src/FDM/JSBSim/models/FGAccelerations.cpp index 011333062..bfb4a6a81 100644 --- a/src/FDM/JSBSim/models/FGAccelerations.cpp +++ b/src/FDM/JSBSim/models/FGAccelerations.cpp @@ -60,7 +60,7 @@ using namespace std; namespace JSBSim { -IDENT(IdSrc,"$Id: FGAccelerations.cpp,v 1.26 2016/01/31 11:13:00 bcoconni Exp $"); +IDENT(IdSrc,"$Id: FGAccelerations.cpp,v 1.28 2016/04/16 12:24:39 bcoconni Exp $"); IDENT(IdHdr,ID_ACCELERATIONS); /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -74,14 +74,12 @@ FGAccelerations::FGAccelerations(FGFDMExec* fdmex) Name = "FGAccelerations"; gravType = gtWGS84; gravTorque = false; - HoldDown = 0; vPQRidot.InitMatrix(); vUVWidot.InitMatrix(); vUVWdot.InitMatrix(); vGravAccel.InitMatrix(); vBodyAccel.InitMatrix(); - vQtrndot = FGQuaternion(0,0,0); bind(); Debug(0); @@ -105,7 +103,6 @@ bool FGAccelerations::InitModel(void) vUVWdot.InitMatrix(); vGravAccel.InitMatrix(); vBodyAccel.InitMatrix(); - vQtrndot = FGQuaternion(0,0,0); return true; } @@ -122,9 +119,9 @@ bool FGAccelerations::Run(bool Holding) CalculatePQRdot(); // Angular rate derivative CalculateUVWdot(); // Translational rate derivative - CalculateQuatdot(); // Angular orientation derivative - ResolveFrictionForces(in.DeltaT * rate); // Update rate derivatives with friction forces + if (!FDMExec->GetHoldDown()) + ResolveFrictionForces(in.DeltaT * rate); // Update rate derivatives with friction forces Debug(2); return false; @@ -160,7 +157,7 @@ void FGAccelerations::CalculatePQRdot(void) // moments and the total inertial angular velocity expressed in the body // frame. // if (HoldDown && !FDMExec->GetTrimStatus()) { - if (HoldDown) { + if (FDMExec->GetHoldDown()) { // The rotational acceleration in ECI is calculated so that the rotational // acceleration is zero in the body frame. vPQRdot.InitMatrix(); @@ -172,19 +169,6 @@ void FGAccelerations::CalculatePQRdot(void) } } -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// 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 FGAccelerations::CalculateQuatdot(void) -{ - // Compute quaternion orientation derivative on current body rates - vQtrndot = in.qAttitudeECI.GetQDot(in.vPQRi); -} - //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // This set of calculations results in the body and inertial frame accelerations // being computed. @@ -204,7 +188,7 @@ void FGAccelerations::CalculateQuatdot(void) void FGAccelerations::CalculateUVWdot(void) { - if (HoldDown && !FDMExec->GetTrimStatus()) + if (FDMExec->GetHoldDown() && !FDMExec->GetTrimStatus()) vBodyAccel.InitMatrix(); else vBodyAccel = in.Force / in.Mass; @@ -216,21 +200,21 @@ void FGAccelerations::CalculateUVWdot(void) // Include Gravitation accel switch (gravType) { - case gtStandard: - { - double radius = in.vInertialPosition.Magnitude(); - vGravAccel = -(in.GAccel / radius) * in.vInertialPosition; - } - break; - case gtWGS84: - vGravAccel = in.Tec2i * in.J2Grav; - break; + case gtStandard: + { + double radius = in.vInertialPosition.Magnitude(); + vGravAccel = -(in.GAccel / radius) * in.vInertialPosition; + } + break; + case gtWGS84: + vGravAccel = in.Tec2i * in.J2Grav; + break; } - if (HoldDown) { + if (FDMExec->GetHoldDown()) { // The acceleration in ECI is calculated so that the acceleration is zero // in the body frame. - vUVWidot = -1.0 * (in.Tb2i * vUVWdot); + vUVWidot = in.vOmegaPlanet * (in.vOmegaPlanet * in.vInertialPosition); vUVWdot.InitMatrix(); } else { @@ -239,6 +223,18 @@ void FGAccelerations::CalculateUVWdot(void) } } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGAccelerations::SetHoldDown(bool hd) +{ + if (hd) { + vUVWidot = in.vOmegaPlanet * (in.vOmegaPlanet * in.vInertialPosition); + vUVWdot.InitMatrix(); + vPQRidot = in.vPQRi * (in.Ti2b * in.vOmegaPlanet); + vPQRdot.InitMatrix(); + } +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // Resolves the contact forces just before integrating the EOM. // This routine is using Lagrange multipliers and the projected Gauss-Seidel @@ -351,7 +347,6 @@ void FGAccelerations::InitializeDerivatives(void) // Make an initial run and set past values CalculatePQRdot(); // Angular rate derivative CalculateUVWdot(); // Translational rate derivative - CalculateQuatdot(); // Angular orientation derivative ResolveFrictionForces(0.); // Update rate derivatives with friction forces } @@ -386,8 +381,6 @@ void FGAccelerations::bind(void) PropertyManager->Tie("forces/fbx-gear-lbs", this, eX, (PMF)&FGAccelerations::GetGroundForces); PropertyManager->Tie("forces/fby-gear-lbs", this, eY, (PMF)&FGAccelerations::GetGroundForces); PropertyManager->Tie("forces/fbz-gear-lbs", this, eZ, (PMF)&FGAccelerations::GetGroundForces); - - PropertyManager->Tie("forces/hold-down", this, &FGAccelerations::GetHoldDown, &FGAccelerations::SetHoldDown); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/models/FGAccelerations.h b/src/FDM/JSBSim/models/FGAccelerations.h index 2fd4386b3..684e62663 100644 --- a/src/FDM/JSBSim/models/FGAccelerations.h +++ b/src/FDM/JSBSim/models/FGAccelerations.h @@ -44,13 +44,12 @@ INCLUDES #include "math/FGColumnVector3.h" #include "math/LagrangeMultiplier.h" #include "math/FGMatrix33.h" -#include "math/FGQuaternion.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_ACCELERATIONS "$Id: FGAccelerations.h,v 1.15 2013/11/29 18:56:30 jberndt Exp $" +#define ID_ACCELERATIONS "$Id: FGAccelerations.h,v 1.19 2016/04/16 12:24:39 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -97,7 +96,7 @@ CLASS DOCUMENTATION NASA SP-8024, May 1969 @author Jon S. Berndt, Mathias Froehlich, Bertrand Coconnier - @version $Id: FGAccelerations.h,v 1.15 2013/11/29 18:56:30 jberndt Exp $ + @version $Id: FGAccelerations.h,v 1.19 2016/04/16 12:24:39 bcoconni Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -135,17 +134,6 @@ public: @return false if no error */ bool Run(bool Holding); - /** Retrieves the time derivative of the body orientation quaternion. - Retrieves the time derivative of the body orientation quaternion based on - the rate of change of the orientation between the body and the ECI frame. - The quaternion returned is represented by an FGQuaternion reference. The - quaternion is 1-based, so that the first element can be retrieved using - the "()" operator. - units rad/sec^2 - @return The time derivative of the body orientation quaternion. - */ - const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;} - /** Retrieves the body axis acceleration. Retrieves the computed body axis accelerations based on the applied forces and accounting for a rotating body frame. @@ -327,12 +315,8 @@ public: /** Sets the property forces/hold-down. This allows to do hard 'hold-down' such as for rockets on a launch pad with engines ignited. @param hd enables the 'hold-down' function if non-zero - */ - void SetHoldDown(int hd) {HoldDown = hd;} - /** Gets the value of the property forces/hold-down. - @result zero if the 'hold-down' function is disabled, non-zero otherwise. - */ - int GetHoldDown(void) const {return HoldDown;} + */ + void SetHoldDown(bool hd); struct Inputs { /// The body inertia matrix expressed in the body frame @@ -347,8 +331,6 @@ public: FGMatrix33 Tec2b; /// Transformation matrix from the ECEF to the ECI frame FGMatrix33 Tec2i; - /// Orientation quaternion of the body with respect to the ECI frame - FGQuaternion qAttitudeECI; /// Total moments applied to the body except friction and gravity (expressed in the body frame) FGColumnVector3 Moment; /// Moments generated by the ground normal reactions expressed in the body frame. Does not account for friction. @@ -387,7 +369,6 @@ private: FGColumnVector3 vPQRdot, vPQRidot; FGColumnVector3 vUVWdot, vUVWidot; - FGQuaternion vQtrndot; FGColumnVector3 vBodyAccel; FGColumnVector3 vGravAccel; FGColumnVector3 vFrictionForces; @@ -395,10 +376,8 @@ private: int gravType; bool gravTorque; - int HoldDown; void CalculatePQRdot(void); - void CalculateQuatdot(void); void CalculateUVWdot(void); void ResolveFrictionForces(double dt); diff --git a/src/FDM/JSBSim/models/FGFCSChannel.h b/src/FDM/JSBSim/models/FGFCSChannel.h index 154760972..7c9c667c0 100755 --- a/src/FDM/JSBSim/models/FGFCSChannel.h +++ b/src/FDM/JSBSim/models/FGFCSChannel.h @@ -44,7 +44,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_FCSCHANNEL "$Id: FGFCSChannel.h,v 1.9 2016/04/03 17:06:24 bcoconni Exp $" +#define ID_FCSCHANNEL "$Id: FGFCSChannel.h,v 1.10 2016/04/16 11:00:11 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -127,8 +127,13 @@ public: // and do not execute the channel. if (OnOffNode && !OnOffNode->getBoolValue()) return; - if (fcs->GetDt() != 0.0) + if (fcs->GetDt() != 0.0) { + if (ExecFrameCountSinceLastRun >= ExecRate) { + ExecFrameCountSinceLastRun = 0; + } + ++ExecFrameCountSinceLastRun; + } // channel will be run at rate 1 if trimming, or when the next execrate // frame is reached @@ -136,9 +141,6 @@ public: for (unsigned int i=0; iRun(); } - - if (ExecFrameCountSinceLastRun >= ExecRate) - ExecFrameCountSinceLastRun = 0; } /// Get the channel rate int GetRate(void) const { return ExecRate; } diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index 8ccdda3bd..6b356b6b3 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -79,7 +79,7 @@ using namespace std; namespace JSBSim { -IDENT(IdSrc,"$Id: FGPropagate.cpp,v 1.128 2016/01/23 10:48:11 bcoconni Exp $"); +IDENT(IdSrc,"$Id: FGPropagate.cpp,v 1.130 2016/04/16 12:24:39 bcoconni Exp $"); IDENT(IdHdr,ID_PROPAGATE); /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -179,6 +179,7 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet; CalculateInertialVelocity(); // Translational position derivative + CalculateQuatdot(); // Angular orientation derivative } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -189,7 +190,7 @@ void FGPropagate::InitializeDerivatives() VState.dqPQRidot.assign(5, in.vPQRidot); VState.dqUVWidot.assign(5, in.vUVWidot); VState.dqInertialVelocity.assign(5, VState.vInertialVelocity); - VState.dqQtrndot.assign(5, in.vQtrndot); + VState.dqQtrndot.assign(5, VState.vQtrndot); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -221,7 +222,7 @@ bool FGPropagate::Run(bool Holding) // Propagate rotational / translational velocity, angular /translational position, respectively. - Integrate(VState.qAttitudeECI, in.vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position); + Integrate(VState.qAttitudeECI, VState.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, in.vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate); @@ -255,6 +256,9 @@ bool FGPropagate::Run(bool Holding) VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet; + // Angular orientation derivative + CalculateQuatdot(); + VState.qAttitudeLocal = Tl2b.GetQuaternion(); // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame. @@ -264,6 +268,33 @@ bool FGPropagate::Run(bool Holding) return false; } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPropagate::SetHoldDown(bool hd) +{ + if (hd) { + VState.vUVW.InitMatrix(); + CalculateInertialVelocity(); + VState.vPQR.InitMatrix(); + VState.vPQRi = Ti2b * in.vOmegaPlanet; + CalculateQuatdot(); + InitializeDerivatives(); + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// 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 + VState.vQtrndot = VState.qAttitudeECI.GetQDot(VState.vPQRi); +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // 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 @@ -457,6 +488,7 @@ void FGPropagate::SetInertialOrientation(const FGQuaternion& Qi) VState.qAttitudeECI.Normalize(); UpdateBodyMatrices(); VState.qAttitudeLocal = Tl2b.GetQuaternion(); + CalculateQuatdot(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -472,6 +504,7 @@ void FGPropagate::SetInertialVelocity(const FGColumnVector3& Vi) { void FGPropagate::SetInertialRates(const FGColumnVector3& vRates) { VState.vPQRi = Ti2b * vRates; VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet; + CalculateQuatdot(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -553,6 +586,7 @@ void FGPropagate::SetVState(const VehicleState& vstate) VState.vPQR = vstate.vPQR; VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet; VState.vInertialPosition = vstate.vInertialPosition; + CalculateQuatdot(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/models/FGPropagate.h b/src/FDM/JSBSim/models/FGPropagate.h index b2ee9703b..fd3275973 100644 --- a/src/FDM/JSBSim/models/FGPropagate.h +++ b/src/FDM/JSBSim/models/FGPropagate.h @@ -49,7 +49,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.83 2016/01/23 10:48:11 bcoconni Exp $" +#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.85 2016/04/16 12:24:39 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -92,7 +92,7 @@ CLASS DOCUMENTATION @endcode @author Jon S. Berndt, Mathias Froehlich, Bertrand Coconnier - @version $Id: FGPropagate.h,v 1.83 2016/01/23 10:48:11 bcoconni Exp $ + @version $Id: FGPropagate.h,v 1.85 2016/04/16 12:24:39 bcoconni Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -133,6 +133,8 @@ public: body frame relative to the inertial (ECI) frame. */ FGQuaternion qAttitudeECI; + FGQuaternion vQtrndot; + FGColumnVector3 vInertialVelocity; FGColumnVector3 vInertialPosition; @@ -229,6 +231,17 @@ public: */ const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;} + /** Retrieves the time derivative of the body orientation quaternion. + Retrieves the time derivative of the body orientation quaternion based on + the rate of change of the orientation between the body and the ECI frame. + The quaternion returned is represented by an FGQuaternion reference. The + quaternion is 1-based, so that the first element can be retrieved using + the "()" operator. + units rad/sec^2 + @return The time derivative of the body orientation quaternion. + */ + const FGQuaternion& GetQuaterniondot(void) const {return VState.vQtrndot;} + /** Retrieves the Euler angles that define the vehicle orientation. Extracts the Euler angles from the quaternion that stores the orientation in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The @@ -583,11 +596,16 @@ public: VState.vLocation -= Tb2ec*deltaLoc; } + /** Sets the property forces/hold-down. This allows to do hard 'hold-down' + such as for rockets on a launch pad with engines ignited. + @param hd enables the 'hold-down' function if non-zero + */ + void SetHoldDown(bool hd); + void DumpState(void); struct Inputs { FGColumnVector3 vPQRidot; - FGQuaternion vQtrndot; FGColumnVector3 vUVWidot; FGColumnVector3 vOmegaPlanet; double SemiMajor; @@ -626,6 +644,7 @@ private: void CalculateInertialVelocity(void); void CalculateUVW(void); + void CalculateQuatdot(void); void Integrate( FGColumnVector3& Integrand, FGColumnVector3& Val,