Sync. with JSBSim CVS.
This commit is contained in:
Erik Hofman
parent
61a81e855d
commit
0d0751e37c
11 changed files with 413 additions and 189 deletions
|
|
@ -71,7 +71,7 @@ using namespace std;
|
|||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id: FGFDMExec.cpp,v 1.78 2010/04/12 12:25:19 jberndt Exp $";
|
||||
static const char *IdSrc = "$Id: FGFDMExec.cpp,v 1.79 2010/07/25 13:52:20 jberndt Exp $";
|
||||
static const char *IdHdr = ID_FDMEXEC;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
|
@ -665,10 +665,10 @@ bool FGFDMExec::LoadModel(string model, bool addModelToPath)
|
|||
|
||||
modelLoaded = true;
|
||||
|
||||
MassBalance->Run(); // Update all mass properties for the report.
|
||||
MassBalance->GetMassPropertiesReport();
|
||||
|
||||
if (debug_lvl > 0) {
|
||||
MassBalance->Run(); // Update all mass properties for the report.
|
||||
MassBalance->GetMassPropertiesReport();
|
||||
|
||||
cout << endl << fgblue << highint
|
||||
<< "End of vehicle configuration loading." << endl
|
||||
<< "-------------------------------------------------------------------------------"
|
||||
|
|
|
|||
|
|
@ -245,6 +245,13 @@ public:
|
|||
is equal to zero it is left untouched. */
|
||||
FGColumnVector3& Normalize(void);
|
||||
|
||||
/** Dot product of two vectors
|
||||
Compute and return the euclidean dot (or scalar) product of two vectors
|
||||
v1 and v2 */
|
||||
friend inline double DotProduct(const FGColumnVector3& v1, const FGColumnVector3& v2) {
|
||||
return v1(1)*v2(1) + v1(2)*v2(2) + v1(3)*v2(3);
|
||||
}
|
||||
|
||||
private:
|
||||
double data[3];
|
||||
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@ INCLUDES
|
|||
DEFINITIONS
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#define ID_LOCATION "$Id: FGLocation.h,v 1.21 2010/07/09 04:11:45 jberndt Exp $"
|
||||
#define ID_LOCATION "$Id: FGLocation.h,v 1.22 2010/07/25 22:15:57 jberndt Exp $"
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
FORWARD DECLARATIONS
|
||||
|
|
@ -142,7 +142,7 @@ CLASS DOCUMENTATION
|
|||
@see W. C. Durham "Aircraft Dynamics & Control", section 2.2
|
||||
|
||||
@author Mathias Froehlich
|
||||
@version $Id: FGLocation.h,v 1.21 2010/07/09 04:11:45 jberndt Exp $
|
||||
@version $Id: FGLocation.h,v 1.22 2010/07/25 22:15:57 jberndt Exp $
|
||||
*/
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
|
@ -326,7 +326,8 @@ public:
|
|||
@return the distance of the location represented with this class
|
||||
instance to the center of the earth in ft. The radius value is
|
||||
always positive. */
|
||||
double GetRadius() const { return mECLoc.Magnitude(); }
|
||||
//double GetRadius() const { return mECLoc.Magnitude(); } // may not work with FlightGear
|
||||
double GetRadius() const { ComputeDerived(); return mRadius; }
|
||||
|
||||
/** Transform matrix from local horizontal to earth centered frame.
|
||||
Returns a const reference to the rotation matrix of the transform from
|
||||
|
|
|
|||
|
|
@ -68,7 +68,7 @@ DEFINITIONS
|
|||
GLOBAL DATA
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
static const char *IdSrc = "$Id: FGAircraft.cpp,v 1.26 2010/02/15 03:28:24 jberndt Exp $";
|
||||
static const char *IdSrc = "$Id: FGAircraft.cpp,v 1.27 2010/07/27 23:18:19 jberndt Exp $";
|
||||
static const char *IdHdr = ID_AIRCRAFT;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
|
@ -139,10 +139,10 @@ bool FGAircraft::Run(void)
|
|||
|
||||
vBodyAccel = vForces/MassBalance->GetMass();
|
||||
|
||||
vNcg = vBodyAccel/Inertial->gravity();
|
||||
vNcg = vBodyAccel/Inertial->SLgravity();
|
||||
|
||||
vNwcg = Aerodynamics->GetTb2w() * vNcg;
|
||||
vNwcg(3) = -1*vNwcg(3) + 1;
|
||||
vNwcg(3) = 1.0 - vNwcg(3);
|
||||
|
||||
RunPostFunctions();
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
Author: Tony Peden, Jon Berndt
|
||||
Date started: 01/26/99
|
||||
Purpose: Calculates additional parameters needed by the visual system, etc.
|
||||
Called by: FGSimExec
|
||||
Called by: FGFDMExec
|
||||
|
||||
------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) -------------
|
||||
|
||||
|
|
@ -59,7 +59,7 @@ using namespace std;
|
|||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id: FGAuxiliary.cpp,v 1.39 2010/07/09 12:06:11 jberndt Exp $";
|
||||
static const char *IdSrc = "$Id: FGAuxiliary.cpp,v 1.42 2010/07/27 23:18:19 jberndt Exp $";
|
||||
static const char *IdHdr = ID_AUXILIARY;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
|
@ -167,22 +167,10 @@ bool FGAuxiliary::Run()
|
|||
vEulerRates(ePhi) = vPQR(eP) + vEulerRates(ePsi)*sTht;
|
||||
}
|
||||
|
||||
// 12/16/2005, JSB: For ground handling purposes, at this time, let's ramp
|
||||
// in the effects of wind from 10 fps to 30 fps when there is weight on the
|
||||
// landing gear wheels.
|
||||
|
||||
if (GroundReactions->GetWOW() && vUVW(eU) < 10) {
|
||||
vAeroPQR = vPQR;
|
||||
vAeroUVW = vUVW;
|
||||
} else if (GroundReactions->GetWOW() && vUVW(eU) < 30) {
|
||||
double factor = (vUVW(eU) - 10.0)/20.0;
|
||||
vAeroPQR = vPQR - factor*Atmosphere->GetTurbPQR();
|
||||
vAeroUVW = vUVW - factor*Propagate->GetTl2b()*Atmosphere->GetTotalWindNED();
|
||||
} else {
|
||||
FGColumnVector3 wind = Propagate->GetTl2b()*Atmosphere->GetTotalWindNED();
|
||||
vAeroPQR = vPQR - Atmosphere->GetTurbPQR();
|
||||
vAeroUVW = vUVW - wind;
|
||||
}
|
||||
// Combine the wind speed with aircraft speed to obtain wind relative speed
|
||||
FGColumnVector3 wind = Propagate->GetTl2b()*Atmosphere->GetTotalWindNED();
|
||||
vAeroPQR = vPQR - Atmosphere->GetTurbPQR();
|
||||
vAeroUVW = vUVW - wind;
|
||||
|
||||
Vt = vAeroUVW.Magnitude();
|
||||
if ( Vt > 0.05) {
|
||||
|
|
@ -258,7 +246,7 @@ bool FGAuxiliary::Run()
|
|||
|
||||
vAircraftAccel /= MassBalance->GetMass();
|
||||
// Nz is Acceleration in "g's", along normal axis (-Z body axis)
|
||||
Nz = -vAircraftAccel(eZ)/Inertial->gravity();
|
||||
Nz = -vAircraftAccel(eZ)/Inertial->SLgravity();
|
||||
vToEyePt = MassBalance->StructuralToBody(Aircraft->GetXYZep());
|
||||
vPilotAccel = vAircraftAccel + Propagate->GetPQRdot() * vToEyePt;
|
||||
vPilotAccel += vPQR * (vPQR * vToEyePt);
|
||||
|
|
@ -269,11 +257,11 @@ bool FGAuxiliary::Run()
|
|||
// any jitter that could be introduced by the landing gear. Theoretically,
|
||||
// this branch could be eliminated, with a penalty of having a short
|
||||
// transient at startup (lasting only a fraction of a second).
|
||||
vPilotAccel = Propagate->GetTl2b() * FGColumnVector3( 0.0, 0.0, -Inertial->gravity() );
|
||||
Nz = -vPilotAccel(eZ)/Inertial->gravity();
|
||||
vPilotAccel = Propagate->GetTl2b() * FGColumnVector3( 0.0, 0.0, -Inertial->SLgravity() );
|
||||
Nz = -vPilotAccel(eZ)/Inertial->SLgravity();
|
||||
}
|
||||
|
||||
vPilotAccelN = vPilotAccel/Inertial->gravity();
|
||||
vPilotAccelN = vPilotAccel/Inertial->SLgravity();
|
||||
|
||||
// VRP computation
|
||||
const FGLocation& vLocation = Propagate->GetLocation();
|
||||
|
|
|
|||
|
|
@ -46,14 +46,53 @@ using namespace std;
|
|||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id: FGGroundReactions.cpp,v 1.26 2009/11/12 13:08:11 jberndt Exp $";
|
||||
static const char *IdSrc = "$Id: FGGroundReactions.cpp,v 1.29 2010/07/30 11:50:01 jberndt Exp $";
|
||||
static const char *IdHdr = ID_GROUNDREACTIONS;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION for MultiplierIterator (See below for FGGroundReactions)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
MultiplierIterator::MultiplierIterator(FGGroundReactions* GndReactions)
|
||||
: GroundReactions(GndReactions),
|
||||
multiplier(NULL),
|
||||
gearNum(0),
|
||||
entry(0)
|
||||
{
|
||||
for (int i=0; i < GroundReactions->GetNumGearUnits(); i++) {
|
||||
FGLGear* gear = GroundReactions->GetGearUnit(i);
|
||||
|
||||
if (!gear->GetWOW()) continue;
|
||||
|
||||
gearNum = i;
|
||||
multiplier = gear->GetMultiplierEntry(0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
MultiplierIterator& MultiplierIterator::operator++()
|
||||
{
|
||||
for (int i=gearNum; i < GroundReactions->GetNumGearUnits(); i++) {
|
||||
FGLGear* gear = GroundReactions->GetGearUnit(i);
|
||||
|
||||
if (!gear->GetWOW()) continue;
|
||||
|
||||
multiplier = gear->GetMultiplierEntry(++entry);
|
||||
if (multiplier) {
|
||||
gearNum = i;
|
||||
break;
|
||||
}
|
||||
else
|
||||
entry = -1;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
|
||||
FGGroundReactions::FGGroundReactions(FGFDMExec* fgex) : FGModel(fgex)
|
||||
{
|
||||
Name = "FGGroundReactions";
|
||||
|
|
@ -123,6 +162,20 @@ bool FGGroundReactions::GetWOW(void)
|
|||
return result;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// This function must be called after friction forces are resolved in order to
|
||||
// include them in the ground reactions total force and moment.
|
||||
void FGGroundReactions::UpdateForcesAndMoments(void)
|
||||
{
|
||||
vForces.InitMatrix();
|
||||
vMoments.InitMatrix();
|
||||
|
||||
for (unsigned int i=0; i<lGear.size(); i++) {
|
||||
vForces += lGear[i]->UpdateForces();
|
||||
vMoments += lGear[i]->GetMoments();
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
bool FGGroundReactions::Load(Element* el)
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@ INCLUDES
|
|||
#include "math/FGColumnVector3.h"
|
||||
#include "input_output/FGXMLElement.h"
|
||||
|
||||
#define ID_GROUNDREACTIONS "$Id: FGGroundReactions.h,v 1.15 2009/10/02 10:30:09 jberndt Exp $"
|
||||
#define ID_GROUNDREACTIONS "$Id: FGGroundReactions.h,v 1.17 2010/07/30 11:50:01 jberndt Exp $"
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
FORWARD DECLARATIONS
|
||||
|
|
@ -78,6 +78,19 @@ CLASS DOCUMENTATION
|
|||
CLASS DECLARATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
class MultiplierIterator
|
||||
{
|
||||
public:
|
||||
MultiplierIterator(FGGroundReactions* GndReactions);
|
||||
MultiplierIterator& operator++();
|
||||
FGPropagate::LagrangeMultiplier* operator*() { return multiplier; }
|
||||
private:
|
||||
FGGroundReactions* GroundReactions;
|
||||
FGPropagate::LagrangeMultiplier* multiplier;
|
||||
int gearNum;
|
||||
int entry;
|
||||
};
|
||||
|
||||
class FGGroundReactions : public FGModel
|
||||
{
|
||||
public:
|
||||
|
|
@ -94,6 +107,7 @@ public:
|
|||
string GetGroundReactionStrings(string delimeter);
|
||||
string GetGroundReactionValues(string delimeter);
|
||||
bool GetWOW(void);
|
||||
void UpdateForcesAndMoments(void);
|
||||
|
||||
int GetNumGearUnits(void) const { return (int)lGear.size(); }
|
||||
|
||||
|
|
|
|||
|
|
@ -61,7 +61,7 @@ DEFINITIONS
|
|||
GLOBAL DATA
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
static const char *IdSrc = "$Id: FGLGear.cpp,v 1.74 2010/05/18 10:54:14 jberndt Exp $";
|
||||
static const char *IdSrc = "$Id: FGLGear.cpp,v 1.76 2010/07/30 11:50:01 jberndt Exp $";
|
||||
static const char *IdHdr = ID_LGEAR;
|
||||
|
||||
// Body To Structural (body frame is rotated 180 deg about Y and lengths are given in
|
||||
|
|
@ -210,49 +210,11 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) :
|
|||
<< sSteerType << " is undefined." << endl;
|
||||
}
|
||||
|
||||
RFRV = 0.7; // Rolling force relaxation velocity, default value
|
||||
SFRV = 0.7; // Side force relaxation velocity, default value
|
||||
|
||||
Element* relax_vel = el->FindElement("relaxation_velocity");
|
||||
if (relax_vel) {
|
||||
if (relax_vel->FindElement("rolling")) {
|
||||
RFRV = relax_vel->FindElementValueAsNumberConvertTo("rolling", "FT/SEC");
|
||||
}
|
||||
if (relax_vel->FindElement("side")) {
|
||||
SFRV = relax_vel->FindElementValueAsNumberConvertTo("side", "FT/SEC");
|
||||
}
|
||||
}
|
||||
|
||||
Aircraft = fdmex->GetAircraft();
|
||||
Propagate = fdmex->GetPropagate();
|
||||
Auxiliary = fdmex->GetAuxiliary();
|
||||
FCS = fdmex->GetFCS();
|
||||
MassBalance = fdmex->GetMassBalance();
|
||||
|
||||
LongForceLagFilterCoeff = 1/fdmex->GetDeltaT(); // default longitudinal force filter coefficient
|
||||
LatForceLagFilterCoeff = 1/fdmex->GetDeltaT(); // default lateral force filter coefficient
|
||||
|
||||
Element* force_lag_filter_elem = el->FindElement("force_lag_filter");
|
||||
if (force_lag_filter_elem) {
|
||||
if (force_lag_filter_elem->FindElement("rolling")) {
|
||||
LongForceLagFilterCoeff = force_lag_filter_elem->FindElementValueAsNumber("rolling");
|
||||
}
|
||||
if (force_lag_filter_elem->FindElement("side")) {
|
||||
LatForceLagFilterCoeff = force_lag_filter_elem->FindElementValueAsNumber("side");
|
||||
}
|
||||
}
|
||||
|
||||
LongForceFilter = Filter(LongForceLagFilterCoeff, fdmex->GetDeltaT());
|
||||
LatForceFilter = Filter(LatForceLagFilterCoeff, fdmex->GetDeltaT());
|
||||
|
||||
WheelSlipLagFilterCoeff = 1/fdmex->GetDeltaT();
|
||||
|
||||
Element *wheel_slip_angle_lag_elem = el->FindElement("wheel_slip_filter");
|
||||
if (wheel_slip_angle_lag_elem) {
|
||||
WheelSlipLagFilterCoeff = wheel_slip_angle_lag_elem->GetDataAsNumber();
|
||||
}
|
||||
|
||||
WheelSlipFilter = Filter(WheelSlipLagFilterCoeff, fdmex->GetDeltaT());
|
||||
Auxiliary = fdmex->GetAuxiliary();
|
||||
Propagate = fdmex->GetPropagate();
|
||||
FCS = fdmex->GetFCS();
|
||||
MassBalance = fdmex->GetMassBalance();
|
||||
GroundReactions = fdmex->GetGroundReactions();
|
||||
|
||||
GearUp = false;
|
||||
GearDown = true;
|
||||
|
|
@ -291,6 +253,11 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) :
|
|||
Peak = staticFCoeff;
|
||||
Curvature = 1.03;
|
||||
|
||||
// Initialize Lagrange multipliers
|
||||
LMultiplier[ftRoll].value = 0.;
|
||||
LMultiplier[ftSide].value = 0.;
|
||||
LMultiplier[ftRoll].value = 0.;
|
||||
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
|
|
@ -307,35 +274,35 @@ FGLGear::~FGLGear()
|
|||
FGColumnVector3& FGLGear::GetBodyForces(void)
|
||||
{
|
||||
double t = fdmex->GetSimTime();
|
||||
dT = fdmex->GetDeltaT()*fdmex->GetGroundReactions()->GetRate();
|
||||
dT = fdmex->GetDeltaT()*GroundReactions->GetRate();
|
||||
|
||||
vFn.InitMatrix();
|
||||
|
||||
if (isRetractable) ComputeRetractionState();
|
||||
|
||||
if (GearDown) {
|
||||
double verticalZProj = 0.;
|
||||
|
||||
vWhlBodyVec = MassBalance->StructuralToBody(vXYZn); // Get wheel in body frame
|
||||
vLocalGear = Propagate->GetTb2l() * vWhlBodyVec; // Get local frame wheel location
|
||||
|
||||
gearLoc = Propagate->GetLocation().LocalToLocation(vLocalGear);
|
||||
// Compute the height of the theoretical location of the wheel (if strut is not compressed) with
|
||||
// respect to the ground level
|
||||
// Compute the height of the theoretical location of the wheel (if strut is
|
||||
// not compressed) with respect to the ground level
|
||||
double height = fdmex->GetGroundCallback()->GetAGLevel(t, gearLoc, contact, normal, cvel);
|
||||
vGroundNormal = Propagate->GetTec2b() * normal;
|
||||
|
||||
// The height returned above is the AGL and is expressed in the Z direction of the local
|
||||
// coordinate frame. We now need to transform this height in actual compression of the strut (BOGEY)
|
||||
// of in the normal direction to the ground (STRUCTURE)
|
||||
// The height returned above is the AGL and is expressed in the Z direction
|
||||
// of the ECEF coordinate frame. We now need to transform this height in
|
||||
// actual compression of the strut (BOGEY) of in the normal direction to the
|
||||
// ground (STRUCTURE)
|
||||
double normalZ = (Propagate->GetTec2l()*normal)(eZ);
|
||||
double LGearProj = -(mTGear.Transposed() * vGroundNormal)(eZ);
|
||||
|
||||
switch (eContactType) {
|
||||
case ctBOGEY:
|
||||
verticalZProj = (Propagate->GetTb2l()*mTGear*FGColumnVector3(0.,0.,1.))(eZ);
|
||||
compressLength = verticalZProj > 0.0 ? -height / verticalZProj : 0.0;
|
||||
compressLength = LGearProj > 0.0 ? height * normalZ / LGearProj : 0.0;
|
||||
break;
|
||||
case ctSTRUCTURE:
|
||||
verticalZProj = -(Propagate->GetTec2l()*normal)(eZ);
|
||||
compressLength = fabs(verticalZProj) > 0.0 ? -height / verticalZProj : 0.0;
|
||||
compressLength = height * normalZ / DotProduct(normal, normal);
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -343,13 +310,22 @@ FGColumnVector3& FGLGear::GetBodyForces(void)
|
|||
|
||||
WOW = true;
|
||||
|
||||
// [The next equation should really use the vector to the contact patch of
|
||||
// the tire including the strut compression and not the original vWhlBodyVec.]
|
||||
// The following equations use the vector to the tire contact patch
|
||||
// including the strut compression.
|
||||
FGColumnVector3 vWhlDisplVec;
|
||||
|
||||
FGColumnVector3 vWhlDisplVec = mTGear * FGColumnVector3(0., 0., compressLength);
|
||||
FGColumnVector3 vWhlContactVec = vWhlBodyVec - vWhlDisplVec;
|
||||
vActingXYZn = vXYZn - Tb2s * vWhlDisplVec;
|
||||
FGColumnVector3 vBodyWhlVel = Propagate->GetPQR() * vWhlContactVec;
|
||||
switch(eContactType) {
|
||||
case ctBOGEY:
|
||||
vWhlDisplVec = mTGear * FGColumnVector3(0., 0., -compressLength);
|
||||
break;
|
||||
case ctSTRUCTURE:
|
||||
vWhlDisplVec = compressLength * vGroundNormal;
|
||||
break;
|
||||
}
|
||||
|
||||
FGColumnVector3 vWhlContactVec = vWhlBodyVec + vWhlDisplVec;
|
||||
vActingXYZn = vXYZn + Tb2s * vWhlDisplVec;
|
||||
FGColumnVector3 vBodyWhlVel = Propagate->GetPQR() * vWhlContactVec;
|
||||
vBodyWhlVel += Propagate->GetUVW() - Propagate->GetTec2b() * cvel;
|
||||
|
||||
vWhlVelVec = mTGear.Transposed() * vBodyWhlVel;
|
||||
|
|
@ -360,47 +336,22 @@ FGColumnVector3& FGLGear::GetBodyForces(void)
|
|||
|
||||
vLocalWhlVel = Transform().Transposed() * vBodyWhlVel;
|
||||
|
||||
switch (eContactType) {
|
||||
case ctBOGEY:
|
||||
// Compression speed along the strut
|
||||
compressSpeed = -vWhlVelVec(eZ);
|
||||
case ctSTRUCTURE:
|
||||
// Compression speed along the ground normal
|
||||
compressSpeed = -vLocalWhlVel(eX);
|
||||
}
|
||||
compressSpeed = -vLocalWhlVel(eX);
|
||||
if (eContactType == ctBOGEY)
|
||||
compressSpeed /= LGearProj;
|
||||
|
||||
ComputeVerticalStrutForce();
|
||||
|
||||
// Compute the forces in the wheel ground plane.
|
||||
// Compute the friction coefficients in the wheel ground plane.
|
||||
if (eContactType == ctBOGEY) {
|
||||
ComputeSlipAngle();
|
||||
ComputeBrakeForceCoefficient();
|
||||
ComputeSideForceCoefficient();
|
||||
double sign = vLocalWhlVel(eY)>0?1.0:(vLocalWhlVel(eY)<0?-1.0:0.0);
|
||||
vFn(eY) = - ((1.0 - TirePressureNorm) * 30 + vFn(eX) * BrakeFCoeff) * sign;
|
||||
vFn(eZ) = vFn(eX) * FCoeff;
|
||||
}
|
||||
else if (eContactType == ctSTRUCTURE) {
|
||||
FGColumnVector3 vSlipVec = vLocalWhlVel;
|
||||
vSlipVec(eX) = 0.;
|
||||
vSlipVec.Normalize();
|
||||
vFn -= staticFCoeff * vFn(eX) * vSlipVec;
|
||||
}
|
||||
|
||||
// Lag and attenuate the XY-plane forces dependent on velocity. This code
|
||||
// uses a lag filter, C/(s + C) where "C" is the filter coefficient. When
|
||||
// "C" is chosen at the frame rate (in Hz), the jittering is significantly
|
||||
// reduced. This is because the jitter is present *at* the execution rate.
|
||||
// If a coefficient is set to something equal to or less than zero, the
|
||||
// filter is bypassed.
|
||||
|
||||
if (LongForceLagFilterCoeff > 0) vFn(eY) = LongForceFilter.execute(vFn(eY));
|
||||
if (LatForceLagFilterCoeff > 0) vFn(eZ) = LatForceFilter.execute(vFn(eZ));
|
||||
|
||||
if ((fabs(vLocalWhlVel(eY)) <= RFRV) && RFRV > 0) vFn(eY) *= fabs(vLocalWhlVel(eY))/RFRV;
|
||||
if ((fabs(vLocalWhlVel(eZ)) <= SFRV) && SFRV > 0) vFn(eZ) *= fabs(vLocalWhlVel(eZ))/SFRV;
|
||||
|
||||
// End section for attenuating gear jitter
|
||||
// Prepare the Jacobians and the Lagrange multipliers for later friction
|
||||
// forces calculations.
|
||||
ComputeJacobian(vWhlContactVec);
|
||||
|
||||
} else { // Gear is NOT compressed
|
||||
|
||||
|
|
@ -491,14 +442,13 @@ void FGLGear::ComputeRetractionState(void)
|
|||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// Calculate tire slip angle.
|
||||
|
||||
void FGLGear::ComputeSlipAngle(void)
|
||||
{
|
||||
// Calculate tire slip angle.
|
||||
WheelSlip = -atan2(vLocalWhlVel(eZ), fabs(vLocalWhlVel(eY)))*radtodeg;
|
||||
|
||||
// Filter the wheel slip angle
|
||||
if (WheelSlipLagFilterCoeff > 0) WheelSlip = WheelSlipFilter.execute(WheelSlip);
|
||||
// Check that the speed is non-null otherwise use the current angle
|
||||
if (vLocalWhlVel.Magnitude(eY,eZ) > 1E-3)
|
||||
WheelSlip = -atan2(vLocalWhlVel(eZ), fabs(vLocalWhlVel(eY)))*radtodeg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
|
@ -519,7 +469,7 @@ void FGLGear::ComputeSteeringAngle(void)
|
|||
SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber);
|
||||
else {
|
||||
// Check that the speed is non-null otherwise use the current angle
|
||||
if (vWhlVelVec.Magnitude(eX,eY) > 1E-3)
|
||||
if (vWhlVelVec.Magnitude(eX,eY) > 0.1)
|
||||
SteerAngle = atan2(vWhlVelVec(eY), fabs(vWhlVelVec(eX)));
|
||||
}
|
||||
break;
|
||||
|
|
@ -576,20 +526,18 @@ void FGLGear::InitializeReporting(void)
|
|||
|
||||
void FGLGear::ReportTakeoffOrLanding(void)
|
||||
{
|
||||
double deltaT = fdmex->GetDeltaT()*fdmex->GetGroundReactions()->GetRate();
|
||||
|
||||
if (FirstContact)
|
||||
LandingDistanceTraveled += Auxiliary->GetVground()*deltaT;
|
||||
LandingDistanceTraveled += Auxiliary->GetVground()*dT;
|
||||
|
||||
if (StartedGroundRun) {
|
||||
TakeoffDistanceTraveled50ft += Auxiliary->GetVground()*deltaT;
|
||||
if (WOW) TakeoffDistanceTraveled += Auxiliary->GetVground()*deltaT;
|
||||
TakeoffDistanceTraveled50ft += Auxiliary->GetVground()*dT;
|
||||
if (WOW) TakeoffDistanceTraveled += Auxiliary->GetVground()*dT;
|
||||
}
|
||||
|
||||
if ( ReportEnable
|
||||
&& Auxiliary->GetVground() <= 0.05
|
||||
&& !LandingReported
|
||||
&& fdmex->GetGroundReactions()->GetWOW())
|
||||
&& GroundReactions->GetWOW())
|
||||
{
|
||||
if (debug_lvl > 0) Report(erLand);
|
||||
}
|
||||
|
|
@ -597,7 +545,7 @@ void FGLGear::ReportTakeoffOrLanding(void)
|
|||
if ( ReportEnable
|
||||
&& !TakeoffReported
|
||||
&& (Propagate->GetDistanceAGL() - vLocalGear(eZ)) > 50.0
|
||||
&& !fdmex->GetGroundReactions()->GetWOW())
|
||||
&& !GroundReactions->GetWOW())
|
||||
{
|
||||
if (debug_lvl > 0) Report(erTakeoff);
|
||||
}
|
||||
|
|
@ -738,6 +686,99 @@ double FGLGear::GetGearUnitPos(void)
|
|||
return GearPos;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// Compute the jacobian entries for the friction forces resolution later
|
||||
// in FGPropagate
|
||||
|
||||
void FGLGear::ComputeJacobian(const FGColumnVector3& vWhlContactVec)
|
||||
{
|
||||
// When the point of contact is moving, dynamic friction is used
|
||||
// This type of friction is limited to ctSTRUCTURE elements because their
|
||||
// friction coefficient is the same in every directions
|
||||
if ((eContactType == ctSTRUCTURE) && (vLocalWhlVel.Magnitude(eY,eZ) > 1E-3)) {
|
||||
FGColumnVector3 velocityDirection = vLocalWhlVel;
|
||||
|
||||
StaticFriction = false;
|
||||
|
||||
velocityDirection(eX) = 0.;
|
||||
velocityDirection.Normalize();
|
||||
|
||||
LMultiplier[ftDynamic].ForceJacobian = Transform()*velocityDirection;
|
||||
LMultiplier[ftDynamic].MomentJacobian = vWhlContactVec * LMultiplier[ftDynamic].ForceJacobian;
|
||||
LMultiplier[ftDynamic].Max = 0.;
|
||||
LMultiplier[ftDynamic].Min = -fabs(dynamicFCoeff * vFn(eX));
|
||||
LMultiplier[ftDynamic].value = Constrain(LMultiplier[ftDynamic].Min, LMultiplier[ftDynamic].value, LMultiplier[ftDynamic].Max);
|
||||
}
|
||||
else {
|
||||
// Static friction is used for ctSTRUCTURE when the contact point is not moving.
|
||||
// It is always used for ctBOGEY elements because the friction coefficients
|
||||
// of a tyre depend on the direction of the movement (roll & side directions).
|
||||
// This cannot be handled properly by the so-called "dynamic friction".
|
||||
StaticFriction = true;
|
||||
|
||||
LMultiplier[ftRoll].ForceJacobian = Transform()*FGColumnVector3(0.,1.,0.);
|
||||
LMultiplier[ftSide].ForceJacobian = Transform()*FGColumnVector3(0.,0.,1.);
|
||||
LMultiplier[ftRoll].MomentJacobian = vWhlContactVec * LMultiplier[ftRoll].ForceJacobian;
|
||||
LMultiplier[ftSide].MomentJacobian = vWhlContactVec * LMultiplier[ftSide].ForceJacobian;
|
||||
|
||||
switch(eContactType) {
|
||||
case ctBOGEY:
|
||||
LMultiplier[ftRoll].Max = fabs(BrakeFCoeff * vFn(eX));
|
||||
LMultiplier[ftSide].Max = fabs(FCoeff * vFn(eX));
|
||||
break;
|
||||
case ctSTRUCTURE:
|
||||
LMultiplier[ftRoll].Max = fabs(staticFCoeff * vFn(eX));
|
||||
LMultiplier[ftSide].Max = fabs(staticFCoeff * vFn(eX));
|
||||
break;
|
||||
}
|
||||
|
||||
LMultiplier[ftRoll].Min = -LMultiplier[ftRoll].Max;
|
||||
LMultiplier[ftSide].Min = -LMultiplier[ftSide].Max;
|
||||
LMultiplier[ftRoll].value = Constrain(LMultiplier[ftRoll].Min, LMultiplier[ftRoll].value, LMultiplier[ftRoll].Max);
|
||||
LMultiplier[ftSide].value = Constrain(LMultiplier[ftSide].Min, LMultiplier[ftSide].value, LMultiplier[ftSide].Max);
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// This function is used by the MultiplierIterator class to enumerate the
|
||||
// Lagrange multipliers of a landing gear. This allows to encapsulate the storage
|
||||
// of the multipliers in FGLGear without exposing it. From an outside point of
|
||||
// view, each FGLGear instance has a number of Lagrange multipliers which can be
|
||||
// accessed through this routine without knowing the exact constraint which they
|
||||
// model.
|
||||
|
||||
FGPropagate::LagrangeMultiplier* FGLGear::GetMultiplierEntry(int entry)
|
||||
{
|
||||
switch(entry) {
|
||||
case 0:
|
||||
if (StaticFriction)
|
||||
return &LMultiplier[ftRoll];
|
||||
else
|
||||
return &LMultiplier[ftDynamic];
|
||||
case 1:
|
||||
if (StaticFriction)
|
||||
return &LMultiplier[ftSide];
|
||||
default:
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// This routine is called after the Lagrange multiplier has been computed. The
|
||||
// friction forces of the landing gear are then updated accordingly.
|
||||
FGColumnVector3& FGLGear::UpdateForces(void)
|
||||
{
|
||||
if (StaticFriction) {
|
||||
vFn(eY) = LMultiplier[ftRoll].value;
|
||||
vFn(eZ) = LMultiplier[ftSide].value;
|
||||
}
|
||||
else
|
||||
vFn += LMultiplier[ftDynamic].value * (Transform ().Transposed() * LMultiplier[ftDynamic].ForceJacobian);
|
||||
|
||||
// Return the updated force in the body frame
|
||||
return FGForce::GetBodyForces();
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGLGear::bind(void)
|
||||
|
|
@ -807,11 +848,11 @@ void FGLGear::Report(ReportType repType)
|
|||
<< " ft, " << TakeoffDistanceTraveled*0.3048 << " meters" << endl;
|
||||
cout << " Distance traveled (over 50'): " << TakeoffDistanceTraveled50ft
|
||||
<< " ft, " << TakeoffDistanceTraveled50ft*0.3048 << " meters" << endl;
|
||||
cout << " [Altitude (ASL): " << fdmex->GetPropagate()->GetAltitudeASL() << " ft. / "
|
||||
<< fdmex->GetPropagate()->GetAltitudeASLmeters() << " m | Temperature: "
|
||||
cout << " [Altitude (ASL): " << Propagate->GetAltitudeASL() << " ft. / "
|
||||
<< Propagate->GetAltitudeASLmeters() << " m | Temperature: "
|
||||
<< fdmex->GetAtmosphere()->GetTemperature() - 459.67 << " F / "
|
||||
<< RankineToCelsius(fdmex->GetAtmosphere()->GetTemperature()) << " C]" << endl;
|
||||
cout << " [Velocity (KCAS): " << fdmex->GetAuxiliary()->GetVcalibratedKTS() << "]" << endl;
|
||||
cout << " [Velocity (KCAS): " << Auxiliary->GetVcalibratedKTS() << "]" << endl;
|
||||
TakeoffReported = true;
|
||||
break;
|
||||
case erNone:
|
||||
|
|
@ -866,9 +907,6 @@ void FGLGear::Debug(int from)
|
|||
cout << " Grouping: " << sBrakeGroup << endl;
|
||||
cout << " Max Steer Angle: " << maxSteerAngle << endl;
|
||||
cout << " Retractable: " << isRetractable << endl;
|
||||
cout << " Relaxation Velocities:" << endl;
|
||||
cout << " Rolling: " << RFRV << endl;
|
||||
cout << " Side: " << SFRV << endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@ INCLUDES
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#include "models/propulsion/FGForce.h"
|
||||
#include "models/FGPropagate.h"
|
||||
#include "math/FGColumnVector3.h"
|
||||
#include <string>
|
||||
|
||||
|
|
@ -46,7 +47,7 @@ INCLUDES
|
|||
DEFINITIONS
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#define ID_LGEAR "$Id: FGLGear.h,v 1.38 2010/03/23 22:44:36 andgi Exp $"
|
||||
#define ID_LGEAR "$Id: FGLGear.h,v 1.40 2010/07/30 11:50:01 jberndt Exp $"
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
FORWARD DECLARATIONS
|
||||
|
|
@ -177,19 +178,10 @@ CLASS DOCUMENTATION
|
|||
<retractable>{0 | 1}</retractable>
|
||||
<table type="{CORNERING_COEFF}">
|
||||
</table>
|
||||
<relaxation_velocity>
|
||||
<rolling unit="{FT/SEC | KTS | M/S}"> {number} </rolling>
|
||||
<side unit="{FT/SEC | KTS | M/S}"> {number} </side>
|
||||
</relaxation_velocity>
|
||||
<force_lag_filter>
|
||||
<rolling> {number} </rolling>
|
||||
<side> {number} </side>
|
||||
</force_lag_filter>
|
||||
<wheel_slip_filter> {number} </wheel_slip_filter>
|
||||
</contact>
|
||||
@endcode
|
||||
@author Jon S. Berndt
|
||||
@version $Id: FGLGear.h,v 1.38 2010/03/23 22:44:36 andgi Exp $
|
||||
@version $Id: FGLGear.h,v 1.40 2010/07/30 11:50:01 jberndt Exp $
|
||||
@see Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
|
||||
NASA-Ames", NASA CR-2497, January 1975
|
||||
@see Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
|
||||
|
|
@ -215,6 +207,8 @@ public:
|
|||
enum ReportType {erNone=0, erTakeoff, erLand};
|
||||
/// Damping types
|
||||
enum DampType {dtLinear=0, dtSquare};
|
||||
/// Friction types
|
||||
enum FrictionType {ftRoll=0, ftSide, ftDynamic};
|
||||
/** Constructor
|
||||
@param el a pointer to the XML element that contains the CONTACT info.
|
||||
@param Executive a pointer to the parent executive object
|
||||
|
|
@ -289,6 +283,9 @@ public:
|
|||
bool IsBogey(void) const { return (eContactType == ctBOGEY);}
|
||||
double GetGearUnitPos(void);
|
||||
double GetSteerAngleDeg(void) const { return radtodeg*SteerAngle; }
|
||||
FGPropagate::LagrangeMultiplier* GetMultiplierEntry(int entry);
|
||||
void SetLagrangeMultiplier(double lambda, int entry);
|
||||
FGColumnVector3& UpdateForces(void);
|
||||
|
||||
void bind(void);
|
||||
|
||||
|
|
@ -338,6 +335,7 @@ private:
|
|||
bool GearUp, GearDown;
|
||||
bool Servicable;
|
||||
bool Castered;
|
||||
bool StaticFriction;
|
||||
std::string name;
|
||||
std::string sSteerType;
|
||||
std::string sBrakeGroup;
|
||||
|
|
@ -350,22 +348,14 @@ private:
|
|||
DampType eDampType;
|
||||
DampType eDampTypeRebound;
|
||||
double maxSteerAngle;
|
||||
double RFRV; // Rolling force relaxation velocity
|
||||
double SFRV; // Side force relaxation velocity
|
||||
double LongForceLagFilterCoeff; // Longitudinal Force Lag Filter Coefficient
|
||||
double LatForceLagFilterCoeff; // Lateral Force Lag Filter Coefficient
|
||||
double WheelSlipLagFilterCoeff; // Wheel slip angle lag filter coefficient
|
||||
|
||||
Filter LongForceFilter;
|
||||
Filter LatForceFilter;
|
||||
Filter WheelSlipFilter;
|
||||
FGPropagate::LagrangeMultiplier LMultiplier[3];
|
||||
|
||||
FGState* State;
|
||||
FGAircraft* Aircraft;
|
||||
FGPropagate* Propagate;
|
||||
FGAuxiliary* Auxiliary;
|
||||
FGFCS* FCS;
|
||||
FGMassBalance* MassBalance;
|
||||
FGAuxiliary* Auxiliary;
|
||||
FGPropagate* Propagate;
|
||||
FGFCS* FCS;
|
||||
FGMassBalance* MassBalance;
|
||||
FGGroundReactions* GroundReactions;
|
||||
|
||||
void ComputeRetractionState(void);
|
||||
void ComputeBrakeForceCoefficient(void);
|
||||
|
|
@ -374,6 +364,7 @@ private:
|
|||
void ComputeSideForceCoefficient(void);
|
||||
void ComputeVerticalStrutForce(void);
|
||||
void ComputeGroundCoordSys(void);
|
||||
void ComputeJacobian(const FGColumnVector3& vWhlContactVec);
|
||||
void CrashDetect(void);
|
||||
void InitializeReporting(void);
|
||||
void ResetReporting(void);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -70,7 +71,7 @@ using namespace std;
|
|||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.55 2010/07/09 04:11:45 jberndt Exp $";
|
||||
static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.59 2010/07/30 11:50:01 jberndt Exp $";
|
||||
static const char *IdHdr = ID_PROPAGATE;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
|
@ -216,6 +217,7 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
|
|||
// 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
|
||||
|
||||
|
|
@ -269,6 +271,7 @@ static int ctr;
|
|||
// Calculate state derivatives
|
||||
CalculatePQRdot(); // Angular rate derivative
|
||||
CalculateUVWdot(); // Translational rate derivative
|
||||
ResolveFrictionForces(dt); // Update rate derivatives with friction forces
|
||||
CalculateQuatdot(); // Angular orientation derivative
|
||||
CalculateInertialVelocity(); // Translational position derivative
|
||||
|
||||
|
|
@ -304,8 +307,7 @@ static int ctr;
|
|||
VState.vLocation = Ti2ec*VState.vInertialPosition;
|
||||
RecomputeLocalTerrainRadius();
|
||||
|
||||
// Calculate current aircraft radius from center of planet
|
||||
VehicleRadius = VState.vInertialPosition.Magnitude();
|
||||
VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
|
||||
radInv = 1.0/VehicleRadius;
|
||||
|
||||
VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
|
||||
|
|
@ -386,9 +388,7 @@ void FGPropagate::CalculateUVWdot(void)
|
|||
vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
|
||||
|
||||
// Include Centripetal acceleration.
|
||||
if (!GroundReactions->GetWOW() && Aircraft->GetHoldDown() == 0) {
|
||||
vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
|
||||
}
|
||||
vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
|
||||
|
||||
// Include Gravitation accel
|
||||
switch (gravType) {
|
||||
|
|
@ -464,11 +464,133 @@ void FGPropagate::Integrate( FGQuaternion& Integrand,
|
|||
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
|
||||
case eNone: // do nothing, freeze rotational rate
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// 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.
|
||||
|
||||
void FGPropagate::ResolveFrictionForces(double dt)
|
||||
{
|
||||
const double invMass = 1.0 / MassBalance->GetMass();
|
||||
const FGMatrix33& Jinv = MassBalance->GetJinv();
|
||||
vector <FGColumnVector3> JacF, JacM;
|
||||
FGColumnVector3 vdot, wdot;
|
||||
FGColumnVector3 Fc, Mc;
|
||||
int n = 0, i;
|
||||
|
||||
// Compiles data from the ground reactions to build up the jacobian matrix
|
||||
for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, n++) {
|
||||
JacF.push_back((*it)->ForceJacobian);
|
||||
JacM.push_back((*it)->MomentJacobian);
|
||||
}
|
||||
|
||||
// If no gears are in contact with the ground then return
|
||||
if (!n) return;
|
||||
|
||||
double *a = new double[n*n]; // Will contain J*M^-1*J^T
|
||||
double *eta = new double[n];
|
||||
double *lambda = new double[n];
|
||||
double *lambdaMin = new double[n];
|
||||
double *lambdaMax = new double[n];
|
||||
|
||||
// Initializes the Lagrange multipliers
|
||||
i = 0;
|
||||
for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, i++) {
|
||||
lambda[i] = (*it)->value;
|
||||
lambdaMax[i] = (*it)->Max;
|
||||
lambdaMin[i] = (*it)->Min;
|
||||
}
|
||||
|
||||
vdot = vUVWdot;
|
||||
wdot = vPQRdot;
|
||||
|
||||
if (dt > 0.) {
|
||||
// First compute the ground velocity below the aircraft center of gravity
|
||||
FGLocation contact;
|
||||
FGColumnVector3 normal, cvel;
|
||||
double t = FDMExec->GetSimTime();
|
||||
double height = FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contact, normal, cvel);
|
||||
|
||||
// Instruct the algorithm to zero out the relative movement between the
|
||||
// aircraft and the ground.
|
||||
vdot += (VState.vUVW - Tec2b * cvel) / dt;
|
||||
wdot += VState.vPQR / dt;
|
||||
}
|
||||
|
||||
// Assemble the linear system of equations
|
||||
for (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]);
|
||||
}
|
||||
|
||||
// Prepare the linear system for the Gauss-Seidel algorithm :
|
||||
// divide every line of 'a' and eta by a[i,i]. This is in order to save
|
||||
// a division computation at each iteration of Gauss-Seidel.
|
||||
for (i=0; i < n; i++) {
|
||||
double d = 1.0 / a[i*n+i];
|
||||
|
||||
eta[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 (i=0; i < n; i++) {
|
||||
double lambda0 = lambda[i];
|
||||
double dlambda = eta[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 (i=0; i< n; i++) {
|
||||
Fc += lambda[i]*JacF[i];
|
||||
Mc += lambda[i]*JacM[i];
|
||||
}
|
||||
|
||||
vUVWdot += invMass * Fc;
|
||||
vPQRdot += Jinv * Mc;
|
||||
|
||||
// Save the value of the Lagrange multipliers to accelerate the convergence
|
||||
// of the Gauss-Seidel algorithm at next iteration.
|
||||
i = 0;
|
||||
for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it)
|
||||
(*it)->value = lambda[i++];
|
||||
|
||||
GroundReactions->UpdateForcesAndMoments();
|
||||
|
||||
delete a, eta, lambda, lambdaMin, lambdaMax;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
|
||||
|
|
@ -488,9 +610,8 @@ void FGPropagate::RecomputeLocalTerrainRadius(void)
|
|||
double t = FDMExec->GetSimTime();
|
||||
|
||||
// Get the LocalTerrain radius.
|
||||
// FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
|
||||
// LocalTerrainRadius = contactloc.GetRadius();
|
||||
LocalTerrainRadius = FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius();
|
||||
FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
|
||||
LocalTerrainRadius = contactloc.GetRadius();
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@ INCLUDES
|
|||
DEFINITIONS
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.41 2010/07/09 04:11:45 jberndt Exp $"
|
||||
#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.43 2010/07/25 15:35:11 jberndt Exp $"
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
FORWARD DECLARATIONS
|
||||
|
|
@ -102,7 +102,7 @@ CLASS DOCUMENTATION
|
|||
@endcode
|
||||
|
||||
@author Jon S. Berndt, Mathias Froehlich
|
||||
@version $Id: FGPropagate.h,v 1.41 2010/07/09 04:11:45 jberndt Exp $
|
||||
@version $Id: FGPropagate.h,v 1.43 2010/07/25 15:35:11 jberndt Exp $
|
||||
*/
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
|
@ -181,10 +181,6 @@ public:
|
|||
@return false if no error */
|
||||
bool Run(void);
|
||||
|
||||
void CalculatePQRdot(void);
|
||||
void CalculateQuatdot(void);
|
||||
void CalculateInertialVelocity(void);
|
||||
void CalculateUVWdot(void);
|
||||
const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
|
||||
|
||||
/** Retrieves the velocity vector.
|
||||
|
|
@ -568,6 +564,14 @@ public:
|
|||
VState.vLocation -= vDeltaXYZEC;
|
||||
}
|
||||
|
||||
struct LagrangeMultiplier {
|
||||
FGColumnVector3 ForceJacobian;
|
||||
FGColumnVector3 MomentJacobian;
|
||||
double Min;
|
||||
double Max;
|
||||
double value;
|
||||
};
|
||||
|
||||
private:
|
||||
|
||||
// state vector
|
||||
|
|
@ -606,6 +610,11 @@ private:
|
|||
eIntegrateType integrator_translational_position;
|
||||
int gravType;
|
||||
|
||||
void CalculatePQRdot(void);
|
||||
void CalculateQuatdot(void);
|
||||
void CalculateInertialVelocity(void);
|
||||
void CalculateUVWdot(void);
|
||||
|
||||
void Integrate( FGColumnVector3& Integrand,
|
||||
FGColumnVector3& Val,
|
||||
deque <FGColumnVector3>& ValDot,
|
||||
|
|
@ -618,6 +627,8 @@ private:
|
|||
double dt,
|
||||
eIntegrateType integration_type);
|
||||
|
||||
void ResolveFrictionForces(double dt);
|
||||
|
||||
void bind(void);
|
||||
void Debug(int from);
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in a new issue