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 <system> initialization.
This commit is contained in:
Bertrand Coconnier
parent
bbd15eb370
commit
43f2daa7ae
7 changed files with 130 additions and 77 deletions
|
|
@ -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 <string> FGFDMExec::EnumerateFDMs(void)
|
||||
{
|
||||
vector <string> FDMList;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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; i<FCSComponents.size(); i++)
|
||||
FCSComponents[i]->Run();
|
||||
}
|
||||
|
||||
if (ExecFrameCountSinceLastRun >= ExecRate)
|
||||
ExecFrameCountSinceLastRun = 0;
|
||||
}
|
||||
/// Get the channel rate
|
||||
int GetRate(void) const { return ExecRate; }
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue