X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2Fmodels%2FFGLGear.cpp;h=4106f5ab0c73b1ad6d60c88ad4a406b309a8337f;hb=0917a5e062b531963f9f3d16bb0f95f769d34f61;hp=96d8d2308f28f370730913fb4a7b56d219d1c639;hpb=cf25f69d3639c2e4c821ff7f8ed62b80e8ff495b;p=flightgear.git diff --git a/src/FDM/JSBSim/models/FGLGear.cpp b/src/FDM/JSBSim/models/FGLGear.cpp index 96d8d2308..4106f5ab0 100644 --- a/src/FDM/JSBSim/models/FGLGear.cpp +++ b/src/FDM/JSBSim/models/FGLGear.cpp @@ -3,11 +3,12 @@ Module: FGLGear.cpp Author: Jon S. Berndt Norman H. Princen + Bertrand Coconnier Date started: 11/18/99 Purpose: Encapsulates the landing gear elements Called by: FGAircraft - ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- + ------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) ------------- This program is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software @@ -33,12 +34,23 @@ HISTORY -------------------------------------------------------------------------------- 11/18/99 JSB Created 01/30/01 NHP Extended gear model to properly simulate steering and braking +07/08/09 BC Modified gear model to support large angles between aircraft and ground /%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGLGear.h" +#include "FGGroundReactions.h" +#include "FGFCS.h" +#include "FGAuxiliary.h" +#include "FGAtmosphere.h" +#include "FGMassBalance.h" +#include "math/FGTable.h" +#include +#include + +using namespace std; namespace JSBSim { @@ -50,16 +62,23 @@ DEFINITIONS GLOBAL DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -static const char *IdSrc = "$Id$"; +static const char *IdSrc = "$Id: FGLGear.cpp,v 1.78 2010/10/07 03:45:40 jberndt Exp $"; static const char *IdHdr = ID_LGEAR; +// Body To Structural (body frame is rotated 180 deg about Y and lengths are given in +// ft instead of inches) +const FGMatrix33 FGLGear::Tb2s(-1./inchtoft, 0., 0., 0., 1./inchtoft, 0., 0., 0., -1./inchtoft); + /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : + FGForce(fdmex), GearNumber(number), - Exec(fdmex) + SteerAngle(0.0), + Castered(false), + StaticFriction(false) { Element *force_table=0; Element *dampCoeff=0; @@ -79,7 +98,8 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : } else if (sContactType == "STRUCTURE") { eContactType = ctSTRUCTURE; } else { - eContactType = ctUNKNOWN; + // Unknown contact point types will be treated as STRUCTURE. + eContactType = ctSTRUCTURE; } if (el->FindElement("spring_coeff")) @@ -123,7 +143,7 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : while (force_table) { force_type = force_table->GetAttributeValue("type"); if (force_type == "CORNERING_COEFF") { - ForceY_Table = new FGTable(Exec->GetPropertyManager(), force_table); + ForceY_Table = new FGTable(fdmex->GetPropertyManager(), force_table); } else { cerr << "Undefined force table for " << name << " contact point" << endl; } @@ -137,8 +157,37 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : else sSteerType = "STEERABLE"; Element* element = el->FindElement("location"); - if (element) vXYZ = element->FindElementTripletConvertTo("IN"); + if (element) vXYZn = element->FindElementTripletConvertTo("IN"); else {cerr << "No location given for contact " << name << endl; exit(-1);} + SetTransformType(FGForce::tCustom); + + element = el->FindElement("orientation"); + if (element && (eContactType == ctBOGEY)) { + vGearOrient = element->FindElementTripletConvertTo("RAD"); + + double cp,sp,cr,sr,cy,sy; + + cp=cos(vGearOrient(ePitch)); sp=sin(vGearOrient(ePitch)); + cr=cos(vGearOrient(eRoll)); sr=sin(vGearOrient(eRoll)); + cy=cos(vGearOrient(eYaw)); sy=sin(vGearOrient(eYaw)); + + mTGear(1,1) = cp*cy; + mTGear(2,1) = cp*sy; + mTGear(3,1) = -sp; + + mTGear(1,2) = sr*sp*cy - cr*sy; + mTGear(2,2) = sr*sp*sy + cr*cy; + mTGear(3,2) = sr*cp; + + mTGear(1,3) = cr*sp*cy + sr*sy; + mTGear(2,3) = cr*sp*sy - sr*cy; + mTGear(3,3) = cr*cp; + } + else { + mTGear(1,1) = 1.; + mTGear(2,2) = 1.; + mTGear(3,3) = 1.; + } if (sBrakeGroup == "LEFT" ) eBrakeGrp = bgLeft; else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight; @@ -155,7 +204,7 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : if (sSteerType == "STEERABLE") eSteerType = stSteer; else if (sSteerType == "FIXED" ) eSteerType = stFixed; - else if (sSteerType == "CASTERED" ) eSteerType = stCaster; + else if (sSteerType == "CASTERED" ) {eSteerType = stCaster; Castered = true;} else if (sSteerType.empty() ) {eSteerType = stFixed; sSteerType = "FIXED (defaulted)";} else { @@ -163,44 +212,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"); - } - } - - State = Exec->GetState(); - LongForceLagFilterCoeff = 1/State->Getdt(); // default longitudinal force filter coefficient - LatForceLagFilterCoeff = 1/State->Getdt(); // 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, State->Getdt()); - LatForceFilter = Filter(LatForceLagFilterCoeff, State->Getdt()); - - WheelSlipLagFilterCoeff = 1/State->Getdt(); - - 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, State->Getdt()); + Auxiliary = fdmex->GetAuxiliary(); + Propagate = fdmex->GetPropagate(); + FCS = fdmex->GetFCS(); + MassBalance = fdmex->GetMassBalance(); + GroundReactions = fdmex->GetGroundReactions(); GearUp = false; GearDown = true; @@ -211,13 +227,6 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : // Add some AI here to determine if gear is located properly according to its // brake group type ?? - State = Exec->GetState(); - Aircraft = Exec->GetAircraft(); - Propagate = Exec->GetPropagate(); - Auxiliary = Exec->GetAuxiliary(); - FCS = Exec->GetFCS(); - MassBalance = Exec->GetMassBalance(); - WOW = lastWOW = false; ReportEnable = true; FirstContact = false; @@ -225,12 +234,11 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : TakeoffReported = LandingReported = false; LandingDistanceTraveled = TakeoffDistanceTraveled = TakeoffDistanceTraveled50ft = 0.0; MaximumStrutForce = MaximumStrutTravel = 0.0; - SideForce = RollingForce = 0.0; SinkRate = GroundSpeed = 0.0; - vWhlBodyVec = MassBalance->StructuralToBody(vXYZ); - + vWhlBodyVec = MassBalance->StructuralToBody(vXYZn); vLocalGear = Propagate->GetTb2l() * vWhlBodyVec; + vWhlVelVec.InitMatrix(); compressLength = 0.0; compressSpeed = 0.0; @@ -240,12 +248,6 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : WheelSlip = 0.0; TirePressureNorm = 1.0; - SideWhlVel = 0.0; - RollingWhlVel = 0.0; - - SinWheel = 0.0; - CosWheel = 0.0; - // Set Pacejka terms Stiffness = 0.06; @@ -253,6 +255,9 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : Peak = staticFCoeff; Curvature = 1.03; + // Initialize Lagrange multipliers + memset(LMultiplier, 0, sizeof(LMultiplier)); + Debug(0); } @@ -266,88 +271,101 @@ FGLGear::~FGLGear() //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGColumnVector3& FGLGear::Force(void) +FGColumnVector3& FGLGear::GetBodyForces(void) { - double t = Exec->GetState()->Getsim_time(); - dT = State->Getdt()*Exec->GetGroundReactions()->GetRate(); + double t = fdmex->GetSimTime(); + dT = fdmex->GetDeltaT()*GroundReactions->GetRate(); - vForce.InitMatrix(); - vMoment.InitMatrix(); + vFn.InitMatrix(); if (isRetractable) ComputeRetractionState(); if (GearDown) { + FGColumnVector3 angularVel; - vWhlBodyVec = MassBalance->StructuralToBody(vXYZ); // Get wheel in body frame + vWhlBodyVec = MassBalance->StructuralToBody(vXYZn); // Get wheel in body frame vLocalGear = Propagate->GetTb2l() * vWhlBodyVec; // Get local frame wheel location gearLoc = Propagate->GetLocation().LocalToLocation(vLocalGear); - compressLength = -Exec->GetGroundCallback()->GetAGLevel(t, gearLoc, contact, normal, cvel); - - // The compression length is measured in the Z-axis, only, at this time. + // 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, angularVel); + vGroundNormal = Propagate->GetTec2b() * normal; + + // 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: + compressLength = LGearProj > 0.0 ? height * normalZ / LGearProj : 0.0; + break; + case ctSTRUCTURE: + compressLength = height * normalZ / DotProduct(normal, normal); + break; + } if (compressLength > 0.00) { 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; + + 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 = Propagate->GetTb2l() * (Propagate->GetPQR() * vWhlBodyVec); - vWhlVelVec += Propagate->GetVel() - cvel; - compressSpeed = vWhlVelVec(eZ); + vWhlVelVec = mTGear.Transposed() * vBodyWhlVel; InitializeReporting(); - ComputeBrakeForceCoefficient(); ComputeSteeringAngle(); - ComputeSlipAngle(); - ComputeSideForceCoefficient(); - ComputeVerticalStrutForce(); - - // Compute the forces in the wheel ground plane. + ComputeGroundCoordSys(); - double sign = RollingWhlVel>0?1.0:(RollingWhlVel<0?-1.0:0.0); - RollingForce = ((1.0 - TirePressureNorm) * 30 + vLocalForce(eZ) * BrakeFCoeff) * sign; - SideForce = vLocalForce(eZ) * FCoeff; + vLocalWhlVel = Transform().Transposed() * vBodyWhlVel; - // 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. + compressSpeed = -vLocalWhlVel(eX); + if (eContactType == ctBOGEY) + compressSpeed /= LGearProj; - if (LongForceLagFilterCoeff > 0) RollingForce = LongForceFilter.execute(RollingForce); - if (LatForceLagFilterCoeff > 0) SideForce = LatForceFilter.execute(SideForce); - - if ((fabs(RollingWhlVel) <= RFRV) && RFRV > 0) RollingForce *= fabs(RollingWhlVel)/RFRV; - if ((fabs(SideWhlVel) <= SFRV) && SFRV > 0) SideForce *= fabs(SideWhlVel)/SFRV; - - // Transform these forces back to the local reference frame. - - vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel; - vLocalForce(eY) = SideForce*CosWheel + RollingForce*SinWheel; - - // Transform the forces back to the body frame and compute the moment. - - vForce = Propagate->GetTl2b() * vLocalForce; + ComputeVerticalStrutForce(); - // End section for attentuating gear jitter + // Compute the friction coefficients in the wheel ground plane. + if (eContactType == ctBOGEY) { + ComputeSlipAngle(); + ComputeBrakeForceCoefficient(); + ComputeSideForceCoefficient(); + } - vMoment = vWhlBodyVec * vForce; + // Prepare the Jacobians and the Lagrange multipliers for later friction + // forces calculations. + ComputeJacobian(vWhlContactVec); } else { // Gear is NOT compressed WOW = false; compressLength = 0.0; compressSpeed = 0.0; - - // No wheel conditions - SideWhlVel = WheelSlip = 0.0; + WheelSlip = 0.0; + StrutForce = 0.0; // Let wheel spin down slowly - RollingWhlVel -= 13.0*dT; - if (RollingWhlVel < 0.0) RollingWhlVel = 0.0; + vWhlVelVec(eX) -= 13.0*dT; + if (vWhlVelVec(eX) < 0.0) vWhlVelVec(eX) = 0.0; // Return to neutral position between 1.0 and 0.8 gear pos. SteerAngle *= max(GetGearUnitPos()-0.8, 0.0)/0.2; @@ -364,7 +382,46 @@ FGColumnVector3& FGLGear::Force(void) lastWOW = WOW; - return vForce; + return FGForce::GetBodyForces(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// Build a local "ground" coordinate system defined by +// eX : normal to the ground +// eY : projection of the rolling direction on the ground +// eZ : projection of the sliping direction on the ground + +void FGLGear::ComputeGroundCoordSys(void) +{ + // Euler angles are built up to create a local frame to describe the forces + // applied to the gear by the ground. Here pitch, yaw and roll do not have + // any physical meaning. It is just a convenient notation. + // First, "pitch" and "yaw" are determined in order to align eX with the + // ground normal. + if (vGroundNormal(eZ) < -1.0) + vOrient(ePitch) = 0.5*M_PI; + else if (1.0 < vGroundNormal(eZ)) + vOrient(ePitch) = -0.5*M_PI; + else + vOrient(ePitch) = asin(-vGroundNormal(eZ)); + + if (fabs(vOrient(ePitch)) == 0.5*M_PI) + vOrient(eYaw) = 0.; + else + vOrient(eYaw) = atan2(vGroundNormal(eY), vGroundNormal(eX)); + + vOrient(eRoll) = 0.; + UpdateCustomTransformMatrix(); + + if (eContactType == ctBOGEY) { + // In the case of a bogey, the third angle "roll" is used to align the axis eY and eZ + // to the rolling and sliping direction respectively. + FGColumnVector3 updatedRollingAxis = Transform().Transposed() * mTGear + * FGColumnVector3(-sin(SteerAngle), cos(SteerAngle), 0.); + + vOrient(eRoll) = atan2(updatedRollingAxis(eY), -updatedRollingAxis(eZ)); + UpdateCustomTransformMatrix(); + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -376,7 +433,7 @@ void FGLGear::ComputeRetractionState(void) GearUp = true; WOW = false; GearDown = false; - RollingWhlVel = 0.0; + vWhlVelVec.InitMatrix(); } else if (gearPos > 0.99) { GearDown = true; GearUp = false; @@ -387,18 +444,13 @@ void FGLGear::ComputeRetractionState(void) } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// Calculate tire slip angle. void FGLGear::ComputeSlipAngle(void) { - // Transform the wheel velocities from the local axis system to the wheel axis system. - RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel; - SideWhlVel = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel; - - // Calculate tire slip angle. - WheelSlip = atan2(SideWhlVel, fabs(RollingWhlVel))*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; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -407,9 +459,6 @@ void FGLGear::ComputeSlipAngle(void) void FGLGear::ComputeSteeringAngle(void) { - double casterLocalFrameAngleRad = 0.0; - double casterAngle = 0.0; - switch (eSteerType) { case stSteer: SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber); @@ -418,20 +467,18 @@ void FGLGear::ComputeSteeringAngle(void) SteerAngle = 0.0; break; case stCaster: - // This is not correct for castering gear. Should make steer angle parallel - // to the actual velocity vector of the wheel, given aircraft velocity vector - // and omega. - SteerAngle = 0.0; - casterLocalFrameAngleRad = acos(vWhlVelVec(eX)/vWhlVelVec.Magnitude()); - casterAngle = casterLocalFrameAngleRad - Propagate->GetEuler(ePsi); + if (!Castered) + SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber); + else { + // Check that the speed is non-null otherwise use the current angle + if (vWhlVelVec.Magnitude(eX,eY) > 0.1) + SteerAngle = atan2(vWhlVelVec(eY), fabs(vWhlVelVec(eX))); + } break; default: cerr << "Improper steering type membership detected for this gear." << endl; break; } - - SinWheel = sin(Propagate->GetEuler(ePsi) + SteerAngle); - CosWheel = cos(Propagate->GetEuler(ePsi) + SteerAngle); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -481,20 +528,18 @@ void FGLGear::InitializeReporting(void) void FGLGear::ReportTakeoffOrLanding(void) { - double deltaT = State->Getdt()*Exec->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 - && Exec->GetGroundReactions()->GetWOW()) + && GroundReactions->GetWOW()) { if (debug_lvl > 0) Report(erLand); } @@ -502,7 +547,7 @@ void FGLGear::ReportTakeoffOrLanding(void) if ( ReportEnable && !TakeoffReported && (Propagate->GetDistanceAGL() - vLocalGear(eZ)) > 50.0 - && !Exec->GetGroundReactions()->GetWOW()) + && !GroundReactions->GetWOW()) { if (debug_lvl > 0) Report(erTakeoff); } @@ -516,12 +561,12 @@ void FGLGear::ReportTakeoffOrLanding(void) void FGLGear::CrashDetect(void) { if ( (compressLength > 500.0 || - vForce.Magnitude() > 100000000.0 || - vMoment.Magnitude() > 5000000000.0 || - SinkRate > 1.4666*30 ) && !State->IntegrationSuspended()) + vFn.Magnitude() > 100000000.0 || + GetMoments().Magnitude() > 5000000000.0 || + SinkRate > 1.4666*30 ) && !fdmex->IntegrationSuspended()) { PutMessage("Crash Detected: Simulation FREEZE."); - State->SuspendIntegration(); + fdmex->SuspendIntegration(); } } @@ -612,10 +657,22 @@ void FGLGear::ComputeVerticalStrutForce(void) dampForce = compressSpeed * compressSpeed * bDampRebound; } - vLocalForce(eZ) = min(springForce + dampForce, (double)0.0); + + StrutForce = min(springForce + dampForce, (double)0.0); + + // The reaction force of the wheel is always normal to the ground + switch (eContactType) { + case ctBOGEY: + // Project back the strut force in the local coordinate frame of the ground + vFn(eX) = StrutForce / (mTGear.Transposed()*vGroundNormal)(eZ); + break; + case ctSTRUCTURE: + vFn(eX) = -StrutForce; + break; + } // Remember these values for reporting - MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ))); + MaximumStrutForce = max(MaximumStrutForce, fabs(StrutForce)); MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength)); } @@ -631,6 +688,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) @@ -640,29 +790,35 @@ void FGLGear::bind(void) base_property_name = CreateIndexedPropertyName("gear/unit", GearNumber); if (eContactType == ctBOGEY) { property_name = base_property_name + "/slip-angle-deg"; - Exec->GetPropertyManager()->Tie( property_name.c_str(), &WheelSlip ); + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &WheelSlip ); property_name = base_property_name + "/WOW"; - Exec->GetPropertyManager()->Tie( property_name.c_str(), &WOW ); + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &WOW ); property_name = base_property_name + "/wheel-speed-fps"; - Exec->GetPropertyManager()->Tie( property_name.c_str(), &RollingWhlVel ); + fdmex->GetPropertyManager()->Tie( property_name.c_str(), (FGLGear*)this, + &FGLGear::GetWheelRollVel); property_name = base_property_name + "/z-position"; - Exec->GetPropertyManager()->Tie( property_name.c_str(), (FGLGear*)this, - &FGLGear::GetZPosition, &FGLGear::SetZPosition); + fdmex->GetPropertyManager()->Tie( property_name.c_str(), (FGForce*)this, + &FGForce::GetLocationZ, &FGForce::SetLocationZ); property_name = base_property_name + "/compression-ft"; - Exec->GetPropertyManager()->Tie( property_name.c_str(), &compressLength ); + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &compressLength ); property_name = base_property_name + "/side_friction_coeff"; - Exec->GetPropertyManager()->Tie( property_name.c_str(), &FCoeff ); + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &FCoeff ); property_name = base_property_name + "/static_friction_coeff"; - Exec->GetPropertyManager()->Tie( property_name.c_str(), &staticFCoeff ); + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &staticFCoeff ); + if (eSteerType == stCaster) { + property_name = base_property_name + "/steering-angle-deg"; + fdmex->GetPropertyManager()->Tie( property_name.c_str(), this, &FGLGear::GetSteerAngleDeg ); + property_name = base_property_name + "/castered"; + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &Castered); + } } if( isRetractable ) { property_name = base_property_name + "/pos-norm"; - Exec->GetPropertyManager()->Tie( property_name.c_str(), &GearPos ); + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &GearPos ); } - } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -674,7 +830,7 @@ void FGLGear::Report(ReportType repType) switch(repType) { case erLand: cout << endl << "Touchdown report for " << name << " (WOW at time: " - << Exec->GetState()->Getsim_time() << " seconds)" << endl; + << fdmex->GetSimTime() << " seconds)" << endl; cout << " Sink rate at contact: " << SinkRate << " fps, " << SinkRate*0.3048 << " mps" << endl; cout << " Contact ground speed: " << GroundSpeed*.5925 << " knots, " @@ -689,18 +845,20 @@ void FGLGear::Report(ReportType repType) break; case erTakeoff: cout << endl << "Takeoff report for " << name << " (Liftoff at time: " - << Exec->GetState()->Getsim_time() << " seconds)" << endl; + << fdmex->GetSimTime() << " seconds)" << endl; cout << " Distance traveled: " << TakeoffDistanceTraveled << " ft, " << TakeoffDistanceTraveled*0.3048 << " meters" << endl; cout << " Distance traveled (over 50'): " << TakeoffDistanceTraveled50ft << " ft, " << TakeoffDistanceTraveled50ft*0.3048 << " meters" << endl; - cout << " [Altitude (ASL): " << Exec->GetPropagate()->GetAltitudeASL() << " ft. / " - << Exec->GetPropagate()->GetAltitudeASLmeters() << " m | Temperature: " - << Exec->GetAtmosphere()->GetTemperature() - 459.67 << " F / " - << RankineToCelsius(Exec->GetAtmosphere()->GetTemperature()) << " C]" << endl; - cout << " [Velocity (KCAS): " << Exec->GetAuxiliary()->GetVcalibratedKTS() << "]" << endl; + 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): " << Auxiliary->GetVcalibratedKTS() << "]" << endl; TakeoffReported = true; break; + case erNone: + break; } } @@ -730,7 +888,7 @@ void FGLGear::Debug(int from) if (debug_lvl & 1) { // Standard console startup message output if (from == 0) { // Constructor - loading and initialization cout << " " << sContactType << " " << name << endl; - cout << " Location: " << vXYZ << endl; + cout << " Location: " << vXYZn << endl; cout << " Spring Constant: " << kSpring << endl; if (eDampType == dtLinear) @@ -751,9 +909,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; } } }