From 1cf207e0540712d3344c48f936a7aade3c5c2797 Mon Sep 17 00:00:00 2001 From: Erik Hofman Date: Sun, 28 Nov 2010 15:14:12 +0100 Subject: [PATCH] Quick update with Bertrand Coconnier's fixes --- src/FDM/JSBSim/math/FGQuaternion.cpp | 4 +- src/FDM/JSBSim/models/FGLGear.cpp | 6 +- src/FDM/JSBSim/models/FGPropagate.cpp | 109 ++++++++++++++++++-------- src/FDM/JSBSim/models/FGPropagate.h | 12 ++- 4 files changed, 92 insertions(+), 39 deletions(-) diff --git a/src/FDM/JSBSim/math/FGQuaternion.cpp b/src/FDM/JSBSim/math/FGQuaternion.cpp index 07868cfa0..0d6e83030 100644 --- a/src/FDM/JSBSim/math/FGQuaternion.cpp +++ b/src/FDM/JSBSim/math/FGQuaternion.cpp @@ -57,7 +57,7 @@ using std::endl; namespace JSBSim { -static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.16 2010/06/30 03:13:40 jberndt Exp $"; +static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.17 2010/11/28 13:15:26 bcoconni Exp $"; static const char *IdHdr = ID_QUATERNION; //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -146,8 +146,6 @@ FGQuaternion::FGQuaternion(const FGMatrix33& m) : mCacheValid(false) data[2] = t*(m(3,1) - m(1,3)); data[3] = t*(m(1,2) - m(2,1)); - ComputeDerivedUnconditional(); - Normalize(); } diff --git a/src/FDM/JSBSim/models/FGLGear.cpp b/src/FDM/JSBSim/models/FGLGear.cpp index 4106f5ab0..596e495f0 100644 --- a/src/FDM/JSBSim/models/FGLGear.cpp +++ b/src/FDM/JSBSim/models/FGLGear.cpp @@ -62,7 +62,7 @@ DEFINITIONS GLOBAL DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -static const char *IdSrc = "$Id: FGLGear.cpp,v 1.78 2010/10/07 03:45:40 jberndt Exp $"; +static const char *IdSrc = "$Id: FGLGear.cpp,v 1.79 2010/11/28 13:20:47 bcoconni Exp $"; static const char *IdHdr = ID_LGEAR; // Body To Structural (body frame is rotated 180 deg about Y and lengths are given in @@ -806,6 +806,10 @@ void FGLGear::bind(void) property_name = base_property_name + "/static_friction_coeff"; fdmex->GetPropertyManager()->Tie( property_name.c_str(), &staticFCoeff ); + property_name = base_property_name + "/rolling_friction_coeff"; + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &rollingFCoeff ); + property_name = base_property_name + "/dynamic_friction_coeff"; + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &dynamicFCoeff ); if (eSteerType == stCaster) { property_name = base_property_name + "/steering-angle-deg"; diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index 066010405..5405c8639 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -71,7 +71,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.73 2010/11/18 12:38:06 jberndt Exp $"; +static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.74 2010/11/28 13:02:43 bcoconni Exp $"; static const char *IdHdr = ID_PROPAGATE; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -454,6 +454,55 @@ void FGPropagate::Integrate( FGQuaternion& Integrand, } } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// Evaluates the rates (translation or rotation) that the friction forces have +// to resist to. This includes the external forces and moments as well as the +// relative movement between the aircraft and the ground. +// Erin Catto's paper (see ref [6]) only supports Euler integration scheme and +// this algorithm has been adapted to handle the multistep algorithms that +// JSBSim supports (i.e. Trapezoidal, Adams-Bashforth 2, 3 and 4). The capacity +// to handle the multistep integration schemes adds some complexity but it +// significantly helps stabilizing the friction forces. + +void FGPropagate::EvaluateRateToResistTo(FGColumnVector3& vdot, + const FGColumnVector3& Val, + const FGColumnVector3& ValDot, + const FGColumnVector3& LocalTerrainVal, + deque & dqValDot, + const double dt, + const eIntegrateType integration_type) +{ + switch(integration_type) { + case eAdamsBashforth4: + vdot = ValDot + Ti2b * (-59.*dqValDot[0]+37.*dqValDot[1]-9.*dqValDot[2])/55.; + if (dt > 0.) // Zeroes out the relative movement between aircraft and ground + vdot += 24.*(Val - Tec2b * LocalTerrainVal) / (55.*dt); + break; + case eAdamsBashforth3: + vdot = ValDot + Ti2b * (-16.*dqValDot[0]+5.*dqValDot[1])/23.; + if (dt > 0.) // Zeroes out the relative movement between aircraft and ground + vdot += 12.*(Val - Tec2b * LocalTerrainVal) / (23.*dt); + break; + case eAdamsBashforth2: + vdot = ValDot - Ti2b * dqValDot[0]/3.; + if (dt > 0.) // Zeroes out the relative movement between aircraft and ground + vdot += 2.*(Val - Tec2b * LocalTerrainVal) / (3.*dt); + break; + case eTrapezoidal: + vdot = ValDot + Ti2b * dqValDot[0]; + if (dt > 0.) // Zeroes out the relative movement between aircraft and ground + vdot += 2.*(Val - Tec2b * LocalTerrainVal) / dt; + break; + case eRectEuler: + vdot = ValDot; + if (dt > 0.) // Zeroes out the relative movement between aircraft and ground + vdot += (Val - Tec2b * LocalTerrainVal) / dt; + break; + case eNone: + break; + } +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // Resolves the contact forces just before integrating the EOM. // This routine is using Lagrange multipliers and the projected Gauss-Seidel @@ -472,58 +521,52 @@ void FGPropagate::ResolveFrictionForces(double dt) const double invMass = 1.0 / FDMExec->GetMassBalance()->GetMass(); const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv(); vector JacF, JacM; + vector lambda, lambdaMin, lambdaMax; FGColumnVector3 vdot, wdot; FGColumnVector3 Fc, Mc; - int n = 0, i; + int n = 0; // Compiles data from the ground reactions to build up the jacobian matrix for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it, n++) { JacF.push_back((*it)->ForceJacobian); JacM.push_back((*it)->MomentJacobian); + lambda.push_back((*it)->value); + lambdaMax.push_back((*it)->Max); + lambdaMin.push_back((*it)->Min); } // If no gears are in contact with the ground then return if (!n) return; vector a(n*n); // Will contain J*M^-1*J^T - vector eta(n); - vector lambda(n); - vector lambdaMin(n); - vector lambdaMax(n); - - // Initializes the Lagrange multipliers - i = 0; - for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it, i++) { - lambda[i] = (*it)->value; - lambdaMax[i] = (*it)->Max; - lambdaMin[i] = (*it)->Min; - } - - vdot = vUVWdot; - wdot = vPQRdot; - - if (dt > 0.) { - // Instruct the algorithm to zero out the relative movement between the - // aircraft and the ground. - vdot += (VState.vUVW - Tec2b * LocalTerrainVelocity) / dt; - wdot += (VState.vPQR - Tec2b * LocalTerrainAngularVelocity) / dt; - } + vector rhs(n); // Assemble the linear system of equations - for (i=0; i < n; i++) { + for (int 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]); } + // Assemble the RHS member + + // Translation + EvaluateRateToResistTo(vdot, VState.vUVW, vUVWdot, LocalTerrainVelocity, + VState.dqUVWidot, dt, integrator_translational_rate); + + // Rotation + EvaluateRateToResistTo(wdot, VState.vPQR, vPQRdot, LocalTerrainAngularVelocity, + VState.dqPQRidot, dt, integrator_rotational_rate); + // 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++) { + // 1. Compute the right hand side member 'rhs' + // 2. Divide every line of 'a' and 'lhs' by a[i,i]. This is in order to save + // a division computation at each iteration of Gauss-Seidel. + for (int i=0; i < n; i++) { double d = 1.0 / a[i*n+i]; - eta[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d; + rhs[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d; for (int j=0; j < n; j++) a[i*n+j] *= d; } @@ -532,9 +575,9 @@ void FGPropagate::ResolveFrictionForces(double dt) for (int iter=0; iter < 50; iter++) { double norm = 0.; - for (i=0; i < n; i++) { + for (int i=0; i < n; i++) { double lambda0 = lambda[i]; - double dlambda = eta[i]; + double dlambda = rhs[i]; for (int j=0; j < n; j++) dlambda -= a[i*n+j]*lambda[j]; @@ -553,7 +596,7 @@ void FGPropagate::ResolveFrictionForces(double dt) Fc.InitMatrix(); Mc.InitMatrix(); - for (i=0; i< n; i++) { + for (int i=0; i< n; i++) { Fc += lambda[i]*JacF[i]; Mc += lambda[i]*JacM[i]; } @@ -565,7 +608,7 @@ void FGPropagate::ResolveFrictionForces(double dt) // Save the value of the Lagrange multipliers to accelerate the convergence // of the Gauss-Seidel algorithm at next iteration. - i = 0; + int i = 0; for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it) (*it)->value = lambda[i++]; diff --git a/src/FDM/JSBSim/models/FGPropagate.h b/src/FDM/JSBSim/models/FGPropagate.h index 1c8d70cb5..00de83c7f 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.52 2010/10/31 04:48:46 jberndt Exp $" +#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -102,7 +102,7 @@ CLASS DOCUMENTATION @endcode @author Jon S. Berndt, Mathias Froehlich - @version $Id: FGPropagate.h,v 1.52 2010/10/31 04:48:46 jberndt Exp $ + @version $Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 bcoconni Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -666,6 +666,14 @@ private: double dt, eIntegrateType integration_type); + void EvaluateRateToResistTo(FGColumnVector3& vdot, + const FGColumnVector3& Val, + const FGColumnVector3& ValDot, + const FGColumnVector3& LocalTerrainVal, + deque & dqValDot, + const double dt, + const eIntegrateType integration_type); + void ResolveFrictionForces(double dt); void UpdateLocationMatrices(void);