X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2Fmodels%2FFGLGear.cpp;h=4106f5ab0c73b1ad6d60c88ad4a406b309a8337f;hb=0917a5e062b531963f9f3d16bb0f95f769d34f61;hp=b25b95a89aae7ff4e6fcc7692c43a3a6afcb5edd;hpb=405c0136184fd31282c6e461b4d6fdcaa352de9e;p=flightgear.git diff --git a/src/FDM/JSBSim/models/FGLGear.cpp b/src/FDM/JSBSim/models/FGLGear.cpp index b25b95a89..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,34 +62,70 @@ 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) : Exec(fdmex), - GearNumber(number) +FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : + FGForce(fdmex), + GearNumber(number), + SteerAngle(0.0), + Castered(false), + StaticFriction(false) { Element *force_table=0; + Element *dampCoeff=0; + Element *dampCoeffRebound=0; string force_type=""; kSpring = bDamp = bDampRebound = dynamicFCoeff = staticFCoeff = rollingFCoeff = maxSteerAngle = 0; sSteerType = sBrakeGroup = sSteerType = ""; isRetractable = 0; + eDampType = dtLinear; + eDampTypeRebound = dtLinear; name = el->GetAttributeValue("name"); sContactType = el->GetAttributeValue("type"); + if (sContactType == "BOGEY") { + eContactType = ctBOGEY; + } else if (sContactType == "STRUCTURE") { + eContactType = ctSTRUCTURE; + } else { + // Unknown contact point types will be treated as STRUCTURE. + eContactType = ctSTRUCTURE; + } + if (el->FindElement("spring_coeff")) kSpring = el->FindElementValueAsNumberConvertTo("spring_coeff", "LBS/FT"); - if (el->FindElement("damping_coeff")) - bDamp = el->FindElementValueAsNumberConvertTo("damping_coeff", "LBS/FT/SEC"); + if (el->FindElement("damping_coeff")) { + dampCoeff = el->FindElement("damping_coeff"); + if (dampCoeff->GetAttributeValue("type") == "SQUARE") { + eDampType = dtSquare; + bDamp = el->FindElementValueAsNumberConvertTo("damping_coeff", "LBS/FT2/SEC2"); + } else { + bDamp = el->FindElementValueAsNumberConvertTo("damping_coeff", "LBS/FT/SEC"); + } + } - if (el->FindElement("damping_coeff_rebound")) - bDampRebound = el->FindElementValueAsNumberConvertTo("damping_coeff_rebound", "LBS/FT/SEC"); - else + if (el->FindElement("damping_coeff_rebound")) { + dampCoeffRebound = el->FindElement("damping_coeff_rebound"); + if (dampCoeffRebound->GetAttributeValue("type") == "SQUARE") { + eDampTypeRebound = dtSquare; + bDampRebound = el->FindElementValueAsNumberConvertTo("damping_coeff_rebound", "LBS/FT2/SEC2"); + } else { + bDampRebound = el->FindElementValueAsNumberConvertTo("damping_coeff_rebound", "LBS/FT/SEC"); + } + } else { bDampRebound = bDamp; + eDampTypeRebound = eDampType; + } if (el->FindElement("dynamic_friction")) dynamicFCoeff = el->FindElementValueAsNumber("dynamic_friction"); @@ -95,7 +143,7 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : Exec(fdmex), 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; } @@ -109,8 +157,37 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : Exec(fdmex), 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; @@ -127,7 +204,7 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : Exec(fdmex), 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 { @@ -135,54 +212,21 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : Exec(fdmex), << 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"); - } - } - - WheelSlipLagFilterCoeff = 1/State->Getdt(); + Auxiliary = fdmex->GetAuxiliary(); + Propagate = fdmex->GetPropagate(); + FCS = fdmex->GetFCS(); + MassBalance = fdmex->GetMassBalance(); + GroundReactions = fdmex->GetGroundReactions(); - Element *wheel_slip_angle_lag_elem = el->FindElement("wheel_slip_filter"); - if (wheel_slip_angle_lag_elem) { - WheelSlipLagFilterCoeff = wheel_slip_angle_lag_elem->GetDataAsNumber(); - } - GearUp = false; GearDown = true; + GearPos = 1.0; + useFCSGearPos = false; Servicable = true; // 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; @@ -190,12 +234,11 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : Exec(fdmex), 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; @@ -205,220 +248,193 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : Exec(fdmex), WheelSlip = 0.0; TirePressureNorm = 1.0; - SideWhlVel = 0.0; - RollingWhlVel = 0.0; + // Set Pacejka terms - SinWheel = 0.0; - CosWheel = 0.0; + Stiffness = 0.06; + Shape = 2.8; + Peak = staticFCoeff; + Curvature = 1.03; - prevSlipIn = 0.0; - prevSlipOut = 0.0; + // Initialize Lagrange multipliers + memset(LMultiplier, 0, sizeof(LMultiplier)); Debug(0); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGLGear::FGLGear(const FGLGear& lgear) -{ - GearNumber = lgear.GearNumber; - State = lgear.State; - Aircraft = lgear.Aircraft; - Propagate = lgear.Propagate; - Auxiliary = lgear.Auxiliary; - Exec = lgear.Exec; - FCS = lgear.FCS; - MassBalance = lgear.MassBalance; - - vXYZ = lgear.vXYZ; - vMoment = lgear.vMoment; - vWhlBodyVec = lgear.vWhlBodyVec; - vLocalGear = lgear.vLocalGear; - - WOW = lgear.WOW; - lastWOW = lgear.lastWOW; - ReportEnable = lgear.ReportEnable; - FirstContact = lgear.FirstContact; - StartedGroundRun = lgear.StartedGroundRun; - LandingDistanceTraveled = lgear.LandingDistanceTraveled; - TakeoffDistanceTraveled = lgear.TakeoffDistanceTraveled; - TakeoffDistanceTraveled50ft = lgear.TakeoffDistanceTraveled50ft; - MaximumStrutForce = lgear.MaximumStrutForce; - MaximumStrutTravel = lgear.MaximumStrutTravel; - SideForce = lgear.SideForce; - RollingForce = lgear.RollingForce; - - kSpring = lgear.kSpring; - bDamp = lgear.bDamp; - bDampRebound = lgear.bDampRebound; - compressLength = lgear.compressLength; - compressSpeed = lgear.compressSpeed; - staticFCoeff = lgear.staticFCoeff; - dynamicFCoeff = lgear.dynamicFCoeff; - rollingFCoeff = lgear.rollingFCoeff; - brakePct = lgear.brakePct; - maxCompLen = lgear.maxCompLen; - SinkRate = lgear.SinkRate; - GroundSpeed = lgear.GroundSpeed; - LandingReported = lgear.LandingReported; - TakeoffReported = lgear.TakeoffReported; - name = lgear.name; - sSteerType = lgear.sSteerType; - sRetractable = lgear.sRetractable; - sContactType = lgear.sContactType; - sBrakeGroup = lgear.sBrakeGroup; - eSteerType = lgear.eSteerType; - eBrakeGrp = lgear.eBrakeGrp; - maxSteerAngle = lgear.maxSteerAngle; - isRetractable = lgear.isRetractable; - GearUp = lgear.GearUp; - GearDown = lgear.GearDown; - WheelSlip = lgear.WheelSlip; - TirePressureNorm = lgear.TirePressureNorm; - Servicable = lgear.Servicable; - ForceY_Table = lgear.ForceY_Table; - CosWheel = lgear.CosWheel; - SinWheel = lgear.SinWheel; - prevOut = lgear.prevOut; - prevIn = lgear.prevIn; - prevSlipIn = lgear.prevSlipIn; - prevSlipOut = lgear.prevSlipOut; - RFRV = lgear.RFRV; - SFRV = lgear.SFRV; - LongForceLagFilterCoeff = lgear.LongForceLagFilterCoeff; - LatForceLagFilterCoeff = lgear.LatForceLagFilterCoeff; - WheelSlipLagFilterCoeff = lgear.WheelSlipLagFilterCoeff; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - FGLGear::~FGLGear() { + delete ForceY_Table; Debug(1); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGColumnVector3& FGLGear::Force(void) +FGColumnVector3& FGLGear::GetBodyForces(void) { - FGColumnVector3 normal, cvel; - FGLocation contact, gearLoc; - 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 (GearUp) return vForce; + if (GearDown) { + 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, 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) { - vWhlBodyVec = MassBalance->StructuralToBody(vXYZ); // Get wheel in body frame - vLocalGear = Propagate->GetTb2l() * vWhlBodyVec; // Get local frame wheel location + WOW = true; - gearLoc = Propagate->GetLocation().LocalToLocation(vLocalGear); - compressLength = -Exec->GetGroundCallback()->GetAGLevel(t, gearLoc, contact, normal, cvel); + // The following equations use the vector to the tire contact patch + // including the strut compression. + FGColumnVector3 vWhlDisplVec; - // The compression length is measured in the Z-axis, only, at this time. + switch(eContactType) { + case ctBOGEY: + vWhlDisplVec = mTGear * FGColumnVector3(0., 0., -compressLength); + break; + case ctSTRUCTURE: + vWhlDisplVec = compressLength * vGroundNormal; + break; + } - if (compressLength > 0.00) { + FGColumnVector3 vWhlContactVec = vWhlBodyVec + vWhlDisplVec; + vActingXYZn = vXYZn + Tb2s * vWhlDisplVec; + FGColumnVector3 vBodyWhlVel = Propagate->GetPQR() * vWhlContactVec; + vBodyWhlVel += Propagate->GetUVW() - Propagate->GetTec2b() * cvel; - WOW = true; + vWhlVelVec = mTGear.Transposed() * vBodyWhlVel; - // [The next equation should really use the vector to the contact patch of - // the tire including the strut compression and not the original vWhlBodyVec.] + InitializeReporting(); + ComputeSteeringAngle(); + ComputeGroundCoordSys(); - vWhlVelVec = Propagate->GetTb2l() * (Propagate->GetPQR() * vWhlBodyVec); - vWhlVelVec += Propagate->GetVel() - cvel; - compressSpeed = vWhlVelVec(eZ); + vLocalWhlVel = Transform().Transposed() * vBodyWhlVel; - InitializeReporting(); - ComputeBrakeForceCoefficient(); - ComputeSteeringAngle(); - ComputeSlipAngle(); - ComputeSideForceCoefficient(); - ComputeVerticalStrutForce(); + compressSpeed = -vLocalWhlVel(eX); + if (eContactType == ctBOGEY) + compressSpeed /= LGearProj; - // Compute the forces in the wheel ground plane. + ComputeVerticalStrutForce(); - RollingForce = ((1.0 - TirePressureNorm) * 30 - + vLocalForce(eZ) * BrakeFCoeff) * (RollingWhlVel>=0?1.0:-1.0); + // Compute the friction coefficients in the wheel ground plane. + if (eContactType == ctBOGEY) { + ComputeSlipAngle(); + ComputeBrakeForceCoefficient(); + ComputeSideForceCoefficient(); + } - SideForce = vLocalForce(eZ) * FCoeff; + // Prepare the Jacobians and the Lagrange multipliers for later friction + // forces calculations. + ComputeJacobian(vWhlContactVec); - // Transform these forces back to the local reference frame. + } else { // Gear is NOT compressed - vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel; - vLocalForce(eY) = SideForce*CosWheel + RollingForce*SinWheel; + WOW = false; + compressLength = 0.0; + compressSpeed = 0.0; + WheelSlip = 0.0; + StrutForce = 0.0; - // Transform the forces back to the body frame and compute the moment. + // Let wheel spin down slowly + vWhlVelVec(eX) -= 13.0*dT; + if (vWhlVelVec(eX) < 0.0) vWhlVelVec(eX) = 0.0; - vForce = Propagate->GetTl2b() * vLocalForce; + // Return to neutral position between 1.0 and 0.8 gear pos. + SteerAngle *= max(GetGearUnitPos()-0.8, 0.0)/0.2; -// Start experimental section for gear jitter reduction -// -// Lag and attenuate the XY-plane forces dependent on velocity - - double ca, cb, denom; - FGColumnVector3 Output; - -// This code implements 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) { - denom = 2.00 + dT*LongForceLagFilterCoeff; - ca = dT*LongForceLagFilterCoeff / denom; - cb = (2.00 - dT*LongForceLagFilterCoeff) / denom; - Output(eX) = vForce(eX) * ca + prevIn(eX) * ca + prevOut(eX) * cb; - vForce(eX) = Output(eX); + ResetReporting(); } - if (LatForceLagFilterCoeff > 0) { - denom = 2.00 + dT*LatForceLagFilterCoeff; - ca = dT*LatForceLagFilterCoeff / denom; - cb = (2.00 - dT*LatForceLagFilterCoeff) / denom; - Output(eY) = vForce(eY) * ca + prevIn(eY) * ca + prevOut(eY) * cb; - vForce(eY) = Output(eY); - } - - prevIn = vForce; - prevOut = Output; + } - if ((fabs(RollingWhlVel) <= RFRV) && RFRV > 0) vForce(eX) *= fabs(RollingWhlVel)/RFRV; - if ((fabs(SideWhlVel) <= SFRV) && SFRV > 0) vForce(eY) *= fabs(SideWhlVel)/SFRV; + ReportTakeoffOrLanding(); -// End section for attentuating gear jitter + // Require both WOW and LastWOW to be true before checking crash conditions + // to allow the WOW flag to be used in terminating a scripted run. + if (WOW && lastWOW) CrashDetect(); - vMoment = vWhlBodyVec * vForce; + lastWOW = WOW; - } else { // Gear is NOT compressed + return FGForce::GetBodyForces(); +} - WOW = false; - compressLength = 0.0; +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// 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 - // Return to neutral position between 1.0 and 0.8 gear pos. - SteerAngle *= max(FCS->GetGearPos()-0.8, 0.0)/0.2; +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)); - ResetReporting(); - } + if (fabs(vOrient(ePitch)) == 0.5*M_PI) + vOrient(eYaw) = 0.; + else + vOrient(eYaw) = atan2(vGroundNormal(eY), vGroundNormal(eX)); + + vOrient(eRoll) = 0.; + UpdateCustomTransformMatrix(); - ReportTakeoffOrLanding(); - CrashDetect(); + 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.); - return vForce; + vOrient(eRoll) = atan2(updatedRollingAxis(eY), -updatedRollingAxis(eZ)); + UpdateCustomTransformMatrix(); + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGLGear::ComputeRetractionState(void) { - if (FCS->GetGearPos() < 0.01) { + double gearPos = GetGearUnitPos(); + if (gearPos < 0.01) { GearUp = true; + WOW = false; GearDown = false; - } else if (FCS->GetGearPos() > 0.99) { + vWhlVelVec.InitMatrix(); + } else if (gearPos > 0.99) { GearDown = true; GearUp = false; } else { @@ -428,30 +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 - - double SlipOutput, ca, cb, denom; - - if (WheelSlipLagFilterCoeff > 0) { - denom = 2.00 + dT*WheelSlipLagFilterCoeff; - ca = dT*WheelSlipLagFilterCoeff / denom; - cb = (2.00 - dT*WheelSlipLagFilterCoeff) / denom; - - SlipOutput = ca * (WheelSlip + prevSlipIn) + cb * prevSlipOut; - - prevSlipIn = WheelSlip; - WheelSlip = prevSlipOut = SlipOutput; - } +// 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; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -468,18 +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; + 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); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -491,6 +490,7 @@ void FGLGear::ResetReporting(void) FirstContact = false; StartedGroundRun = false; LandingReported = false; + TakeoffReported = true; LandingDistanceTraveled = 0.0; MaximumStrutForce = MaximumStrutTravel = 0.0; } @@ -515,7 +515,7 @@ void FGLGear::InitializeReporting(void) if ((Propagate->GetVel().Magnitude() > 0.1) && (FCS->GetBrake(bgLeft) == 0) && (FCS->GetBrake(bgRight) == 0) && - (FCS->GetThrottlePos(0) == 1) && !StartedGroundRun) + (FCS->GetThrottlePos(0) > 0.90) && !StartedGroundRun) { TakeoffDistanceTraveled = 0; TakeoffDistanceTraveled50ft = 0; @@ -528,27 +528,31 @@ void FGLGear::InitializeReporting(void) void FGLGear::ReportTakeoffOrLanding(void) { - double deltaT = State->Getdt()*Exec->GetGroundReactions()->GetRate(); - - if (FirstContact) LandingDistanceTraveled += Auxiliary->GetVground()*deltaT; + if (FirstContact) + 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) { + if ( ReportEnable + && Auxiliary->GetVground() <= 0.05 + && !LandingReported + && GroundReactions->GetWOW()) + { if (debug_lvl > 0) Report(erLand); } - if (ReportEnable && !TakeoffReported && - (vLocalGear(eZ) - Propagate->GetDistanceAGL()) < -50.0) + if ( ReportEnable + && !TakeoffReported + && (Propagate->GetDistanceAGL() - vLocalGear(eZ)) > 50.0 + && !GroundReactions->GetWOW()) { if (debug_lvl > 0) Report(erTakeoff); } if (lastWOW != WOW) PutMessage("GEAR_CONTACT: " + name, WOW); - lastWOW = WOW; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -557,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(); } } @@ -607,27 +611,22 @@ void FGLGear::ComputeBrakeForceCoefficient(void) } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// Compute the sideforce coefficients using similar assumptions to LaRCSim for now. -// Allow a maximum of 10 degrees tire slip angle before wheel slides. At that point, -// transition from static to dynamic friction. There are more complicated formulations -// of this that avoid the discrete jump (similar to Pacejka). Will fix this later. +// Compute the sideforce coefficients using Pacejka's Magic Formula. +// +// y(x) = D sin {C arctan [Bx - E(Bx - arctan Bx)]} +// +// Where: B = Stiffness Factor (0.06, here) +// C = Shape Factor (2.8, here) +// D = Peak Factor (0.8, here) +// E = Curvature Factor (1.03, here) void FGLGear::ComputeSideForceCoefficient(void) { if (ForceY_Table) { - FCoeff = ForceY_Table->GetValue(WheelSlip); - } else { - - if (fabs(WheelSlip) <= 10.0) { - FCoeff = staticFCoeff*WheelSlip/10.0; - } else if (fabs(WheelSlip) <= 40.0) { - FCoeff = (dynamicFCoeff*(fabs(WheelSlip) - 10.0)/10.0 - + staticFCoeff*(40.0 - fabs(WheelSlip))/10.0)*(WheelSlip>=0?1.0:-1.0); - } else { - FCoeff = dynamicFCoeff*(WheelSlip>=0?1.0:-1.0); - } + double StiffSlip = Stiffness*WheelSlip; + FCoeff = Peak * sin(Shape*atan(StiffSlip - Curvature*(StiffSlip - atan(StiffSlip)))); } } @@ -646,42 +645,192 @@ void FGLGear::ComputeVerticalStrutForce(void) springForce = -compressLength * kSpring; if (compressSpeed >= 0.0) { - dampForce = -compressSpeed * bDamp; + + if (eDampType == dtLinear) dampForce = -compressSpeed * bDamp; + else dampForce = -compressSpeed * compressSpeed * bDamp; + } else { - dampForce = -compressSpeed * bDampRebound; + + if (eDampTypeRebound == dtLinear) + dampForce = -compressSpeed * bDampRebound; + else + dampForce = compressSpeed * compressSpeed * bDampRebound; + + } + + 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; } - vLocalForce(eZ) = min(springForce + dampForce, (double)0.0); // Remember these values for reporting - MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ))); + MaximumStrutForce = max(MaximumStrutForce, fabs(StrutForce)); MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength)); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGLGear::bind(void) +double FGLGear::GetGearUnitPos(void) +{ + // hack to provide backward compatibility to gear/gear-pos-norm property + if( useFCSGearPos || FCS->GetGearPos() != 1.0 ) { + useFCSGearPos = true; + return FCS->GetGearPos(); + } + return GearPos; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// Compute the jacobian entries for the friction forces resolution later +// in FGPropagate + +void FGLGear::ComputeJacobian(const FGColumnVector3& vWhlContactVec) { - char property_name[80]; - snprintf(property_name, 80, "gear/unit[%d]/slip-angle-deg", GearNumber); - Exec->GetPropertyManager()->Tie( property_name, &WheelSlip ); + // 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; + } +} -void FGLGear::unbind(void) +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// 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) { - char property_name[80]; - snprintf(property_name, 80, "gear/unit[%d]/slip-angle-deg", GearNumber); - Exec->GetPropertyManager()->Untie( property_name ); + 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) +{ + string property_name; + string base_property_name; + base_property_name = CreateIndexedPropertyName("gear/unit", GearNumber); + if (eContactType == ctBOGEY) { + property_name = base_property_name + "/slip-angle-deg"; + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &WheelSlip ); + property_name = base_property_name + "/WOW"; + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &WOW ); + property_name = base_property_name + "/wheel-speed-fps"; + fdmex->GetPropertyManager()->Tie( property_name.c_str(), (FGLGear*)this, + &FGLGear::GetWheelRollVel); + property_name = base_property_name + "/z-position"; + fdmex->GetPropertyManager()->Tie( property_name.c_str(), (FGForce*)this, + &FGForce::GetLocationZ, &FGForce::SetLocationZ); + property_name = base_property_name + "/compression-ft"; + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &compressLength ); + property_name = base_property_name + "/side_friction_coeff"; + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &FCoeff ); + + property_name = base_property_name + "/static_friction_coeff"; + 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"; + fdmex->GetPropertyManager()->Tie( property_name.c_str(), &GearPos ); + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGLGear::Report(ReportType repType) { + if (fabs(TakeoffDistanceTraveled) < 0.001) return; // Don't print superfluous reports + switch(repType) { case erLand: - cout << endl << "Touchdown report for " << name << endl; + cout << endl << "Touchdown report for " << name << " (WOW at time: " + << fdmex->GetSimTime() << " seconds)" << endl; cout << " Sink rate at contact: " << SinkRate << " fps, " << SinkRate*0.3048 << " mps" << endl; cout << " Contact ground speed: " << GroundSpeed*.5925 << " knots, " @@ -695,13 +844,21 @@ void FGLGear::Report(ReportType repType) LandingReported = true; break; case erTakeoff: - cout << endl << "Takeoff report for " << name << endl; + cout << endl << "Takeoff report for " << name << " (Liftoff at time: " + << 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): " << 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; } } @@ -731,20 +888,27 @@ 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; - cout << " Damping Constant: " << bDamp << endl; + + if (eDampType == dtLinear) + cout << " Damping Constant: " << bDamp << " (linear)" << endl; + else + cout << " Damping Constant: " << bDamp << " (square law)" << endl; + + if (eDampTypeRebound == dtLinear) + cout << " Rebound Damping Constant: " << bDampRebound << " (linear)" << endl; + else + cout << " Rebound Damping Constant: " << bDampRebound << " (square law)" << endl; + cout << " Dynamic Friction: " << dynamicFCoeff << endl; cout << " Static Friction: " << staticFCoeff << endl; - if (sContactType == "BOGEY") { + if (eContactType == ctBOGEY) { cout << " Rolling Friction: " << rollingFCoeff << endl; cout << " Steering Type: " << sSteerType << endl; 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; } } }