X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;ds=sidebyside;f=src%2FFDM%2FJSBSim%2Fmodels%2FFGLGear.cpp;h=4106f5ab0c73b1ad6d60c88ad4a406b309a8337f;hb=0917a5e062b531963f9f3d16bb0f95f769d34f61;hp=b0c448b41777baf98a5ff7375f6017505f067c27;hpb=3836abcf4b60844d4942e6c5da237fb3a1f93d24;p=flightgear.git diff --git a/src/FDM/JSBSim/models/FGLGear.cpp b/src/FDM/JSBSim/models/FGLGear.cpp index b0c448b41..4106f5ab0 100644 --- a/src/FDM/JSBSim/models/FGLGear.cpp +++ b/src/FDM/JSBSim/models/FGLGear.cpp @@ -41,7 +41,6 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGLGear.h" -#include "FGState.h" #include "FGGroundReactions.h" #include "FGFCS.h" #include "FGAuxiliary.h" @@ -49,6 +48,7 @@ INCLUDES #include "FGMassBalance.h" #include "math/FGTable.h" #include +#include using namespace std; @@ -62,7 +62,7 @@ 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 @@ -76,7 +76,9 @@ CLASS IMPLEMENTATION FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : FGForce(fdmex), GearNumber(number), - SteerAngle(0.0) + SteerAngle(0.0), + Castered(false), + StaticFriction(false) { Element *force_table=0; Element *dampCoeff=0; @@ -202,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 { @@ -210,50 +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 = fdmex->GetState(); - Aircraft = fdmex->GetAircraft(); - Propagate = fdmex->GetPropagate(); - Auxiliary = fdmex->GetAuxiliary(); - FCS = fdmex->GetFCS(); - MassBalance = fdmex->GetMassBalance(); - - 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; @@ -292,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); } @@ -307,36 +273,38 @@ FGLGear::~FGLGear() FGColumnVector3& FGLGear::GetBodyForces(void) { - double t = fdmex->GetState()->Getsim_time(); - dT = State->Getdt()*fdmex->GetGroundReactions()->GetRate(); + double t = fdmex->GetSimTime(); + dT = fdmex->GetDeltaT()*GroundReactions->GetRate(); vFn.InitMatrix(); if (isRetractable) ComputeRetractionState(); if (GearDown) { - double verticalZProj = 0.; + FGColumnVector3 angularVel; 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 - double height = fdmex->GetGroundCallback()->GetAGLevel(t, gearLoc, contact, normal, cvel); - vGroundNormal = -1. * 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) + // 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: - 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; } @@ -344,13 +312,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; + + switch(eContactType) { + case ctBOGEY: + vWhlDisplVec = mTGear * FGColumnVector3(0., 0., -compressLength); + break; + case ctSTRUCTURE: + vWhlDisplVec = compressLength * vGroundNormal; + break; + } - FGColumnVector3 vWhlDisplVec = mTGear * FGColumnVector3(0., 0., compressLength); - FGColumnVector3 vWhlContactVec = vWhlBodyVec - vWhlDisplVec; - vActingXYZn = vXYZn - Tb2s * vWhlDisplVec; - FGColumnVector3 vBodyWhlVel = Propagate->GetPQR() * vWhlContactVec; + FGColumnVector3 vWhlContactVec = vWhlBodyVec + vWhlDisplVec; + vActingXYZn = vXYZn + Tb2s * vWhlDisplVec; + FGColumnVector3 vBodyWhlVel = Propagate->GetPQR() * vWhlContactVec; vBodyWhlVel += Propagate->GetUVW() - Propagate->GetTec2b() * cvel; vWhlVelVec = mTGear.Transposed() * vBodyWhlVel; @@ -361,47 +338,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 @@ -492,14 +444,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; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -516,7 +467,13 @@ void FGLGear::ComputeSteeringAngle(void) SteerAngle = 0.0; break; case stCaster: - SteerAngle = atan2(vWhlVelVec(eY), fabs(vWhlVelVec(eX))); + 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; @@ -571,20 +528,18 @@ void FGLGear::InitializeReporting(void) void FGLGear::ReportTakeoffOrLanding(void) { - double deltaT = State->Getdt()*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); } @@ -592,7 +547,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); } @@ -608,10 +563,10 @@ void FGLGear::CrashDetect(void) if ( (compressLength > 500.0 || vFn.Magnitude() > 100000000.0 || GetMoments().Magnitude() > 5000000000.0 || - SinkRate > 1.4666*30 ) && !State->IntegrationSuspended()) + SinkRate > 1.4666*30 ) && !fdmex->IntegrationSuspended()) { PutMessage("Crash Detected: Simulation FREEZE."); - State->SuspendIntegration(); + fdmex->SuspendIntegration(); } } @@ -733,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) @@ -760,8 +808,10 @@ void FGLGear::bind(void) fdmex->GetPropertyManager()->Tie( property_name.c_str(), &staticFCoeff ); if (eSteerType == stCaster) { - property_name = base_property_name + "/steering-angle-rad"; - fdmex->GetPropertyManager()->Tie( property_name.c_str(), &SteerAngle ); + 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); } } @@ -780,7 +830,7 @@ void FGLGear::Report(ReportType repType) switch(repType) { case erLand: cout << endl << "Touchdown report for " << name << " (WOW at time: " - << fdmex->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, " @@ -795,16 +845,16 @@ void FGLGear::Report(ReportType repType) break; case erTakeoff: cout << endl << "Takeoff report for " << name << " (Liftoff at time: " - << fdmex->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): " << 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: @@ -859,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; } } }