From 0d0751e37c71dd1e8c20d499036a551390ee5487 Mon Sep 17 00:00:00 2001 From: Erik Hofman Date: Tue, 3 Aug 2010 09:51:13 +0200 Subject: [PATCH] Sync. with JSBSim CVS. --- src/FDM/JSBSim/FGFDMExec.cpp | 8 +- src/FDM/JSBSim/math/FGColumnVector3.h | 7 + src/FDM/JSBSim/math/FGLocation.h | 7 +- src/FDM/JSBSim/models/FGAircraft.cpp | 6 +- src/FDM/JSBSim/models/FGAuxiliary.cpp | 32 +-- src/FDM/JSBSim/models/FGGroundReactions.cpp | 57 ++++- src/FDM/JSBSim/models/FGGroundReactions.h | 16 +- src/FDM/JSBSim/models/FGLGear.cpp | 264 +++++++++++--------- src/FDM/JSBSim/models/FGLGear.h | 41 ++- src/FDM/JSBSim/models/FGPropagate.cpp | 141 ++++++++++- src/FDM/JSBSim/models/FGPropagate.h | 23 +- 11 files changed, 413 insertions(+), 189 deletions(-) diff --git a/src/FDM/JSBSim/FGFDMExec.cpp b/src/FDM/JSBSim/FGFDMExec.cpp index 820e69daa..5226a3e37 100644 --- a/src/FDM/JSBSim/FGFDMExec.cpp +++ b/src/FDM/JSBSim/FGFDMExec.cpp @@ -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 << "-------------------------------------------------------------------------------" diff --git a/src/FDM/JSBSim/math/FGColumnVector3.h b/src/FDM/JSBSim/math/FGColumnVector3.h index e1564dc3c..b92a47400 100644 --- a/src/FDM/JSBSim/math/FGColumnVector3.h +++ b/src/FDM/JSBSim/math/FGColumnVector3.h @@ -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]; diff --git a/src/FDM/JSBSim/math/FGLocation.h b/src/FDM/JSBSim/math/FGLocation.h index 2785932d5..fd41d94f9 100644 --- a/src/FDM/JSBSim/math/FGLocation.h +++ b/src/FDM/JSBSim/math/FGLocation.h @@ -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 diff --git a/src/FDM/JSBSim/models/FGAircraft.cpp b/src/FDM/JSBSim/models/FGAircraft.cpp index c71a787eb..d292316d6 100644 --- a/src/FDM/JSBSim/models/FGAircraft.cpp +++ b/src/FDM/JSBSim/models/FGAircraft.cpp @@ -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(); diff --git a/src/FDM/JSBSim/models/FGAuxiliary.cpp b/src/FDM/JSBSim/models/FGAuxiliary.cpp index 13a8e67e7..ededdc1cc 100755 --- a/src/FDM/JSBSim/models/FGAuxiliary.cpp +++ b/src/FDM/JSBSim/models/FGAuxiliary.cpp @@ -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(); diff --git a/src/FDM/JSBSim/models/FGGroundReactions.cpp b/src/FDM/JSBSim/models/FGGroundReactions.cpp index 338e79f38..fdbfa254f 100644 --- a/src/FDM/JSBSim/models/FGGroundReactions.cpp +++ b/src/FDM/JSBSim/models/FGGroundReactions.cpp @@ -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; iUpdateForces(); + vMoments += lGear[i]->GetMoments(); + } +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% bool FGGroundReactions::Load(Element* el) diff --git a/src/FDM/JSBSim/models/FGGroundReactions.h b/src/FDM/JSBSim/models/FGGroundReactions.h index 118815625..5c5ea682c 100644 --- a/src/FDM/JSBSim/models/FGGroundReactions.h +++ b/src/FDM/JSBSim/models/FGGroundReactions.h @@ -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(); } diff --git a/src/FDM/JSBSim/models/FGLGear.cpp b/src/FDM/JSBSim/models/FGLGear.cpp index 18ece98d5..569b76ee4 100644 --- a/src/FDM/JSBSim/models/FGLGear.cpp +++ b/src/FDM/JSBSim/models/FGLGear.cpp @@ -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; } } } diff --git a/src/FDM/JSBSim/models/FGLGear.h b/src/FDM/JSBSim/models/FGLGear.h index c3734f914..2190f8b9c 100644 --- a/src/FDM/JSBSim/models/FGLGear.h +++ b/src/FDM/JSBSim/models/FGLGear.h @@ -39,6 +39,7 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "models/propulsion/FGForce.h" +#include "models/FGPropagate.h" #include "math/FGColumnVector3.h" #include @@ -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 {0 | 1}
- - {number} - {number} - - - {number} - {number} - - {number} @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); diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index a698b542c..9c7543c48 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -48,6 +48,7 @@ COMMENTS, REFERENCES, and NOTES Wiley & Sons, 1979 ISBN 0-471-03032-5 [5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons, 1982 ISBN 0-471-08936-2 +[6] Erin Catto, "Iterative Dynamics with Temporal Coherence", February 22, 2005 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES @@ -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 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(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/models/FGPropagate.h b/src/FDM/JSBSim/models/FGPropagate.h index a98a87e20..404946791 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.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 & ValDot, @@ -618,6 +627,8 @@ private: double dt, eIntegrateType integration_type); + void ResolveFrictionForces(double dt); + void bind(void); void Debug(int from); };