46 #include "math/FGFunction.h" 48 #include "models/FGGroundReactions.h" 49 #include "math/FGTable.h" 50 #include "input_output/FGXMLElement.h" 64 IDENT(IdSrc,
"$Id: FGLGear.cpp,v 1.125 2017/02/21 21:14:13 bcoconni Exp $");
65 IDENT(IdHdr,ID_LGEAR);
69 const FGMatrix33 FGLGear::Tb2s(-1./inchtoft, 0., 0., 0., 1./inchtoft, 0., 0., 0., -1./inchtoft);
70 const FGMatrix33 FGLGear::Ts2b(-inchtoft, 0., 0., 0., inchtoft, 0., 0., 0., -inchtoft);
83 StaticFriction(false),
86 kSpring = bDamp = bDampRebound = dynamicFCoeff = staticFCoeff = rollingFCoeff = maxSteerAngle = 0;
87 isRetractable =
false;
89 eDampTypeRebound = dtLinear;
93 if (sContactType ==
"BOGEY") {
94 eContactType = ctBOGEY;
95 }
else if (sContactType ==
"STRUCTURE") {
96 eContactType = ctSTRUCTURE;
99 eContactType = ctSTRUCTURE;
103 if (eContactType == ctSTRUCTURE) {
104 kSpring = in.EmptyWeight;
106 bDampRebound = kSpring * 10;
117 fStrutForce =
new FGFunction(PropertyManager, springFunc);
125 eDampType = dtSquare;
135 eDampTypeRebound = dtSquare;
141 bDampRebound = bDamp;
142 eDampTypeRebound = eDampType;
160 if ((maxSteerAngle == 360 && !castered_el)
162 eSteerType = stCaster;
165 else if (maxSteerAngle == 0.0) {
166 eSteerType = stFixed;
169 eSteerType = stSteer;
175 while (force_table) {
177 if (force_type ==
"CORNERING_COEFF") {
178 ForceY_Table =
new FGTable(PropertyManager, force_table);
181 cerr <<
"Undefined force table for " << name <<
" contact point" << endl;
188 else {cerr <<
"No location given for contact " << name << endl; exit(-1);}
189 SetTransformType(FGForce::tCustom);
192 if (element && (eContactType == ctBOGEY)) {
195 mTGear = quatFromEuler.GetT();
205 if (sBrakeGroup ==
"LEFT" ) eBrakeGrp = bgLeft;
206 else if (sBrakeGroup ==
"RIGHT" ) eBrakeGrp = bgRight;
207 else if (sBrakeGroup ==
"CENTER") eBrakeGrp = bgCenter;
208 else if (sBrakeGroup ==
"NOSE" ) eBrakeGrp = bgCenter;
209 else if (sBrakeGroup ==
"TAIL" ) eBrakeGrp = bgCenter;
210 else if (sBrakeGroup ==
"NONE" ) eBrakeGrp = bgNone;
211 else if (sBrakeGroup.empty() ) eBrakeGrp = bgNone;
213 cerr <<
"Improper braking group specification in config file: " 214 << sBrakeGroup <<
" is undefined." << endl;
220 useFCSGearPos =
false;
222 TakeoffReported = LandingReported =
false;
247 void FGLGear::ResetToIC(
void)
251 WOW = lastWOW =
false;
252 FirstContact =
false;
253 StartedGroundRun =
false;
254 LandingDistanceTraveled = TakeoffDistanceTraveled = TakeoffDistanceTraveled50ft = 0.0;
255 MaximumStrutForce = MaximumStrutTravel = 0.0;
256 SinkRate = GroundSpeed = 0.0;
259 vWhlVelVec.InitMatrix();
261 compressLength = 0.0;
268 for (
int i=0; i < 3; i++) {
269 LMultiplier[i].ForceJacobian.InitMatrix();
270 LMultiplier[i].MomentJacobian.InitMatrix();
271 LMultiplier[i].Min = 0.0;
272 LMultiplier[i].Max = 0.0;
273 LMultiplier[i].value = 0.0;
281 double gearPos = 1.0;
285 if (isRetractable) gearPos = GetGearUnitPos();
287 if (gearPos > 0.99) {
292 vLocalGear = in.Tb2l * vWhlBodyVec;
297 double height = gearLoc.
GetContactPoint(contact, normal, terrainVel, dummy);
301 height -= (*surface).GetBumpHeight();
302 staticFFactor = (*surface).GetStaticFFactor();
303 rollingFFactor = (*surface).GetRollingFFactor();
304 maximumForce = (*surface).GetMaximumForce();
305 isSolid = (*surface).GetSolid();
310 vGroundNormal = in.Tec2b * normal;
316 double normalZ = (in.Tec2l*normal)(eZ);
317 double LGearProj = -(mTGear.
Transposed() * vGroundNormal)(eZ);
322 switch(eContactType) {
325 compressLength = LGearProj > 0.0 ? height * normalZ / LGearProj : 0.0;
329 compressLength = 0.0;
330 vWhlDisplVec = 0.0 * vGroundNormal;
334 compressLength = height * normalZ / DotProduct(normal, normal);
335 vWhlDisplVec = compressLength * vGroundNormal;
340 vActingXYZn = vXYZn + Tb2s * vWhlDisplVec;
342 vBodyWhlVel += in.UVW - in.Tec2b * terrainVel;
345 vWhlVelVec = mTGear.
Transposed() * vBodyWhlVel;
348 vWhlVelVec(eX) -= 13.0 * in.TotalDeltaT;
349 if (vWhlVelVec(eX) < 0.0) vWhlVelVec(eX) = 0.0;
352 InitializeReporting();
353 ComputeSteeringAngle();
354 ComputeGroundFrame();
356 vGroundWhlVel = mT.
Transposed() * vBodyWhlVel;
358 if (fdmex->GetTrimStatus())
361 compressSpeed = -vGroundWhlVel(eZ);
362 if (eContactType == ctBOGEY)
363 compressSpeed /= LGearProj;
366 ComputeVerticalStrutForce();
369 if (eContactType == ctBOGEY) {
371 ComputeBrakeForceCoefficient();
372 ComputeSideForceCoefficient();
377 ComputeJacobian(vWhlContactVec);
382 compressLength = 0.0;
387 LMultiplier[ftRoll].value = 0.0;
388 LMultiplier[ftSide].value = 0.0;
389 LMultiplier[ftDynamic].value = 0.0;
392 vWhlVelVec(eX) -= 13.0 * in.TotalDeltaT;
393 if (vWhlVelVec(eX) < 0.0) vWhlVelVec(eX) = 0.0;
396 SteerAngle *= max(gearPos-0.8, 0.0)/0.2;
401 }
else if (gearPos < 0.01) {
404 vWhlVelVec.InitMatrix();
407 if (!fdmex->GetTrimStatus()) {
408 ReportTakeoffOrLanding();
412 if (WOW && lastWOW) CrashDetect();
417 return FGForce::GetBodyForces();
426 void FGLGear::ComputeGroundFrame(
void)
429 FGColumnVector3 side = vGroundNormal * roll;
431 roll -= DotProduct(roll, vGroundNormal) * vGroundNormal;
435 mT(eX,eX) = roll(eX);
436 mT(eY,eX) = roll(eY);
437 mT(eZ,eX) = roll(eZ);
438 mT(eX,eY) = side(eX);
439 mT(eY,eY) = side(eY);
440 mT(eZ,eY) = side(eZ);
441 mT(eX,eZ) = vGroundNormal(eX);
442 mT(eY,eZ) = vGroundNormal(eY);
443 mT(eZ,eZ) = vGroundNormal(eZ);
449 void FGLGear::ComputeSlipAngle(
void)
452 if (vGroundWhlVel.
Magnitude(eX,eY) > 1E-3)
453 WheelSlip = -atan2(vGroundWhlVel(eY), fabs(vGroundWhlVel(eX)))*radtodeg;
460 void FGLGear::ComputeSteeringAngle(
void)
465 SteerAngle = atan2(vWhlVelVec(eY), fabs(vWhlVelVec(eX)));
472 void FGLGear::ResetReporting(
void)
474 if (in.DistanceAGL > 200.0) {
475 FirstContact =
false;
476 StartedGroundRun =
false;
477 LandingReported =
false;
478 TakeoffReported =
true;
479 LandingDistanceTraveled = 0.0;
480 MaximumStrutForce = MaximumStrutTravel = 0.0;
486 void FGLGear::InitializeReporting(
void)
493 SinkRate = compressSpeed;
494 GroundSpeed = in.Vground;
495 TakeoffReported =
false;
500 if ((in.Vground > 0.1) &&
501 (in.BrakePos[bgLeft] == 0) &&
502 (in.BrakePos[bgRight] == 0) &&
503 (in.TakeoffThrottle && !StartedGroundRun))
505 TakeoffDistanceTraveled = 0;
506 TakeoffDistanceTraveled50ft = 0;
507 StartedGroundRun =
true;
514 void FGLGear::ReportTakeoffOrLanding(
void)
517 LandingDistanceTraveled += in.Vground * in.TotalDeltaT;
519 if (StartedGroundRun) {
520 TakeoffDistanceTraveled50ft += in.Vground * in.TotalDeltaT;
521 if (WOW) TakeoffDistanceTraveled += in.Vground * in.TotalDeltaT;
525 && in.Vground <= 0.05
529 if (debug_lvl > 0) Report(erLand);
534 && (in.DistanceAGL - vLocalGear(eZ)) > 50.0
537 if (debug_lvl > 0) Report(erTakeoff);
543 buf <<
"GEAR_CONTACT: " << fdmex->
GetSimTime() <<
" seconds: " << name;
551 void FGLGear::CrashDetect(
void)
553 if ( (compressLength > 500.0 ||
555 GetMoments().
Magnitude() > 5000000000.0 ||
559 buf <<
"*CRASH DETECTED* " << fdmex->
GetSimTime() <<
" seconds: " << name;
573 void FGLGear::ComputeBrakeForceCoefficient(
void)
575 BrakeFCoeff = rollingFFactor * rollingFCoeff;
577 if (eBrakeGrp != bgNone)
578 BrakeFCoeff += in.BrakePos[eBrakeGrp] * staticFFactor * (staticFCoeff - rollingFCoeff);
591 void FGLGear::ComputeSideForceCoefficient(
void)
594 FCoeff = ForceY_Table->GetValue(WheelSlip);
596 double StiffSlip = Stiffness*WheelSlip;
597 FCoeff = Peak * sin(Shape*atan(StiffSlip - Curvature*(StiffSlip - atan(StiffSlip))));
599 FCoeff *= staticFFactor;
609 void FGLGear::ComputeVerticalStrutForce()
611 double springForce = 0;
612 double dampForce = 0;
615 StrutForce = min(fStrutForce->
GetValue(), (double)0.0);
617 springForce = -compressLength * kSpring;
619 if (compressSpeed >= 0.0) {
621 if (eDampType == dtLinear)
622 dampForce = -compressSpeed * bDamp;
624 dampForce = -compressSpeed * compressSpeed * bDamp;
628 if (eDampTypeRebound == dtLinear)
629 dampForce = -compressSpeed * bDampRebound;
631 dampForce = compressSpeed * compressSpeed * bDampRebound;
635 StrutForce = min(springForce + dampForce, (
double)0.0);
636 if (StrutForce > maximumForce) {
637 StrutForce = maximumForce;
638 compressLength = -StrutForce / kSpring;
643 switch (eContactType) {
646 vFn(eZ) = StrutForce / (mTGear.
Transposed()*vGroundNormal)(eZ);
649 vFn(eZ) = -StrutForce;
654 MaximumStrutForce = max(MaximumStrutForce, fabs(StrutForce));
655 MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
660 double FGLGear::GetGearUnitPos(
void)
const 663 if( useFCSGearPos || in.FCSGearPos != 1.0 ) {
664 useFCSGearPos =
true;
665 return in.FCSGearPos;
679 if ((eContactType == ctSTRUCTURE) && (vGroundWhlVel.
Magnitude(eX,eY) > 1E-3)) {
683 StaticFriction =
false;
685 velocityDirection(eZ) = 0.;
688 LMultiplier[ftDynamic].ForceJacobian = mT * velocityDirection;
689 LMultiplier[ftDynamic].MomentJacobian = vWhlContactVec * LMultiplier[ftDynamic].ForceJacobian;
690 LMultiplier[ftDynamic].Max = 0.;
691 LMultiplier[ftDynamic].Min = -fabs(staticFFactor * dynamicFCoeff * vFn(eZ));
697 LMultiplier[ftDynamic].value =
Constrain(LMultiplier[ftDynamic].Min, LMultiplier[ftDynamic].value, LMultiplier[ftDynamic].Max);
699 GroundReactions->RegisterLagrangeMultiplier(&LMultiplier[ftDynamic]);
706 StaticFriction =
true;
709 LMultiplier[ftSide].ForceJacobian = mT * FGColumnVector3(0.,1.,0.);
710 LMultiplier[ftRoll].MomentJacobian = vWhlContactVec * LMultiplier[ftRoll].ForceJacobian;
711 LMultiplier[ftSide].MomentJacobian = vWhlContactVec * LMultiplier[ftSide].ForceJacobian;
713 switch(eContactType) {
715 LMultiplier[ftRoll].Max = fabs(BrakeFCoeff * vFn(eZ));
716 LMultiplier[ftSide].Max = fabs(FCoeff * vFn(eZ));
719 LMultiplier[ftRoll].Max = fabs(staticFFactor * staticFCoeff * vFn(eZ));
720 LMultiplier[ftSide].Max = LMultiplier[ftRoll].Max;
724 LMultiplier[ftRoll].Min = -LMultiplier[ftRoll].Max;
725 LMultiplier[ftSide].Min = -LMultiplier[ftSide].Max;
731 LMultiplier[ftRoll].value =
Constrain(LMultiplier[ftRoll].Min, LMultiplier[ftRoll].value, LMultiplier[ftRoll].Max);
732 LMultiplier[ftSide].value =
Constrain(LMultiplier[ftSide].Min, LMultiplier[ftSide].value, LMultiplier[ftSide].Max);
734 GroundReactions->RegisterLagrangeMultiplier(&LMultiplier[ftRoll]);
735 GroundReactions->RegisterLagrangeMultiplier(&LMultiplier[ftSide]);
743 void FGLGear::UpdateForces(
void)
745 if (StaticFriction) {
746 vFn(eX) = LMultiplier[ftRoll].value;
747 vFn(eY) = LMultiplier[ftSide].value;
751 vFn(eX) = LMultiplier[ftDynamic].value * forceDir(eX);
752 vFn(eY) = LMultiplier[ftDynamic].value * forceDir(eY);
758 void FGLGear::SetstaticFCoeff(
double coeff)
760 staticFCoeff = coeff;
766 void FGLGear::bind(
void)
768 string property_name;
769 string base_property_name;
771 switch(eContactType) {
773 eSurfaceType = FGSurface::ctBOGEY;
774 base_property_name = CreateIndexedPropertyName(
"gear/unit", GearNumber);
777 eSurfaceType = FGSurface::ctSTRUCTURE;
778 base_property_name = CreateIndexedPropertyName(
"contact/unit", GearNumber);
785 property_name = base_property_name +
"/WOW";
786 PropertyManager->
Tie( property_name.c_str(), &WOW );
787 property_name = base_property_name +
"/x-position";
788 PropertyManager->
Tie( property_name.c_str(), (
FGForce*)
this,
789 &FGForce::GetLocationX, &FGForce::SetLocationX);
790 property_name = base_property_name +
"/y-position";
791 PropertyManager->
Tie( property_name.c_str(), (
FGForce*)
this,
792 &FGForce::GetLocationY, &FGForce::SetLocationY);
793 property_name = base_property_name +
"/z-position";
794 PropertyManager->
Tie( property_name.c_str(), (
FGForce*)
this,
795 &FGForce::GetLocationZ, &FGForce::SetLocationZ);
796 property_name = base_property_name +
"/compression-ft";
797 PropertyManager->
Tie( property_name.c_str(), &compressLength );
798 property_name = base_property_name +
"/compression-velocity-fps";
799 PropertyManager->
Tie( property_name.c_str(), &compressSpeed );
800 property_name = base_property_name +
"/static_friction_coeff";
801 PropertyManager->
Tie( property_name.c_str(), (
FGLGear*)
this,
802 &FGLGear::GetstaticFCoeff, &FGLGear::SetstaticFCoeff);
803 property_name = base_property_name +
"/dynamic_friction_coeff";
804 PropertyManager->
Tie( property_name.c_str(), &dynamicFCoeff );
806 if (eContactType == ctBOGEY) {
807 property_name = base_property_name +
"/slip-angle-deg";
808 PropertyManager->
Tie( property_name.c_str(), &WheelSlip );
809 property_name = base_property_name +
"/wheel-speed-fps";
810 PropertyManager->
Tie( property_name.c_str(), (
FGLGear*)
this,
811 &FGLGear::GetWheelRollVel);
812 property_name = base_property_name +
"/side_friction_coeff";
813 PropertyManager->
Tie( property_name.c_str(), &FCoeff );
814 property_name = base_property_name +
"/rolling_friction_coeff";
815 PropertyManager->
Tie( property_name.c_str(), &rollingFCoeff );
817 if (eSteerType == stCaster) {
818 property_name = base_property_name +
"/steering-angle-deg";
819 PropertyManager->
Tie( property_name.c_str(),
this, &FGLGear::GetSteerAngleDeg );
820 property_name = base_property_name +
"/castered";
821 PropertyManager->
Tie( property_name.c_str(), &Castered);
825 if( isRetractable ) {
826 property_name = base_property_name +
"/pos-norm";
827 PropertyManager->
Tie( property_name.c_str(), &GearPos );
830 if (eSteerType != stFixed) {
834 string tmp = CreateIndexedPropertyName(
"fcs/steer-pos-deg", GearNumber);
835 PropertyManager->
Tie(tmp.c_str(),
this, &FGLGear::GetSteerAngleDeg, &FGLGear::SetSteerAngleDeg);
843 if (fabs(TakeoffDistanceTraveled) < 0.001)
return;
847 cout << endl <<
"Touchdown report for " << name <<
" (WOW at time: " 848 << fdmex->
GetSimTime() <<
" seconds)" << endl;
849 cout <<
" Sink rate at contact: " << SinkRate <<
" fps, " 850 << SinkRate*0.3048 <<
" mps" << endl;
851 cout <<
" Contact ground speed: " << GroundSpeed*.5925 <<
" knots, " 852 << GroundSpeed*0.3048 <<
" mps" << endl;
853 cout <<
" Maximum contact force: " << MaximumStrutForce <<
" lbs, " 854 << MaximumStrutForce*4.448 <<
" Newtons" << endl;
855 cout <<
" Maximum strut travel: " << MaximumStrutTravel*12.0 <<
" inches, " 856 << MaximumStrutTravel*30.48 <<
" cm" << endl;
857 cout <<
" Distance traveled: " << LandingDistanceTraveled <<
" ft, " 858 << LandingDistanceTraveled*0.3048 <<
" meters" << endl;
859 LandingReported =
true;
862 cout << endl <<
"Takeoff report for " << name <<
" (Liftoff at time: " 863 << fdmex->
GetSimTime() <<
" seconds)" << endl;
864 cout <<
" Distance traveled: " << TakeoffDistanceTraveled
865 <<
" ft, " << TakeoffDistanceTraveled*0.3048 <<
" meters" << endl;
866 cout <<
" Distance traveled (over 50'): " << TakeoffDistanceTraveled50ft
867 <<
" ft, " << TakeoffDistanceTraveled50ft*0.3048 <<
" meters" << endl;
868 cout <<
" [Altitude (ASL): " << in.DistanceASL <<
" ft. / " 869 << in.DistanceASL*FGJSBBase::fttom <<
" m | Temperature: " 870 << in.Temperature - 459.67 <<
" F / " 872 cout <<
" [Velocity (KCAS): " << in.VcalibratedKts <<
"]" << endl;
873 TakeoffReported =
true;
899 void FGLGear::Debug(
int from)
901 static const char* sSteerType[] = {
"STEERABLE",
"FIXED",
"CASTERED" };
902 static const char* sBrakeGroup[] = {
"NONE",
"LEFT",
"RIGHT",
"CENTER",
"NOSE",
"TAIL"};
903 static const char* sContactType[] = {
"BOGEY",
"STRUCTURE" };
905 if (debug_lvl <= 0)
return;
909 cout <<
" " << sContactType[eContactType] <<
" " << name << endl;
910 cout <<
" Location: " << vXYZn << endl;
911 cout <<
" Spring Constant: " << kSpring << endl;
913 if (eDampType == dtLinear)
914 cout <<
" Damping Constant: " << bDamp <<
" (linear)" << endl;
916 cout <<
" Damping Constant: " << bDamp <<
" (square law)" << endl;
918 if (eDampTypeRebound == dtLinear)
919 cout <<
" Rebound Damping Constant: " << bDampRebound <<
" (linear)" << endl;
921 cout <<
" Rebound Damping Constant: " << bDampRebound <<
" (square law)" << endl;
923 cout <<
" Dynamic Friction: " << dynamicFCoeff << endl;
924 cout <<
" Static Friction: " << staticFCoeff << endl;
925 if (eContactType == ctBOGEY) {
926 cout <<
" Rolling Friction: " << rollingFCoeff << endl;
927 cout <<
" Steering Type: " << sSteerType[eSteerType] << endl;
928 cout <<
" Grouping: " << sBrakeGroup[eBrakeGrp] << endl;
929 cout <<
" Max Steer Angle: " << maxSteerAngle << endl;
930 cout <<
" Retractable: " << isRetractable << endl;
934 if (debug_lvl & 2 ) {
935 if (from == 0) cout <<
"Instantiated: FGLGear" << endl;
936 if (from == 1) cout <<
"Destroyed: FGLGear" << endl;
938 if (debug_lvl & 4 ) {
940 if (debug_lvl & 8 ) {
942 if (debug_lvl & 16) {
944 if (debug_lvl & 64) {
946 cout << IdSrc << endl;
947 cout << IdHdr << endl;
Models the Quaternion representation of rotations.
ReportType
Report type enumerators.
static double Constrain(double min, double value, double max)
Constrain a value between a minimum and a maximum value.
std::string GetAttributeValue(const std::string &key)
Retrieves an attribute.
static double RankineToCelsius(double rankine)
Converts from degrees Rankine to degrees Celsius.
double FindElementValueAsNumberConvertTo(const std::string &el, const std::string &target_units)
Searches for the named element and converts and returns the data belonging to it. ...
FGLocation holds an arbitrary location in the Earth centered Earth fixed reference frame (ECEF)...
FGColumnVector3 & Normalize(void)
Normalize.
Element * FindElement(const std::string &el="")
Searches for a specified element.
double FindElementValueAsNumber(const std::string &el="")
Searches for the named element and returns the data belonging to it as a number.
double GetContactPoint(FGLocation &contact, FGColumnVector3 &normal, FGColumnVector3 &v, FGColumnVector3 &w) const
Get terrain contact point information below the current location.
void PutMessage(const Message &msg)
Places a Message structure on the Message queue.
double GetValue(void) const
Retrieves the value of the function object.
FGPropertyManager * GetPropertyManager(void)
Returns a pointer to the property manager object.
void Tie(const std::string &name, bool *pointer, bool useDefault=true)
Tie a property to an external bool variable.
double GetDataAsNumber(void)
Converts the element data to a number.
FGGroundReactions * GetGroundReactions(void)
Returns the FGGroundReactions pointer.
FGLocation LocalToLocation(const FGColumnVector3 &lvec) const
Conversion from Local frame coordinates to a location in the earth centered and fixed frame...
std::string FindElementValue(const std::string &el="")
Searches for the named element and returns the string data belonging to it.
Represents a mathematical function.
This class implements a 3 element column vector.
bool IntegrationSuspended(void) const
Returns the simulation suspension state.
double GetSimTime(void) const
Returns the cumulative simulation time in seconds.
Element * FindNextElement(const std::string &el="")
Searches for the next element as specified.
FGMatrix33 Transposed(void) const
Transposed matrix.
double Magnitude(void) const
Length of the vector.
Base class for all surface properties.
Encapsulates the JSBSim simulation executive.
Utility class that aids in the conversion of forces between coordinate systems and calculation of mom...
FGColumnVector3 FindElementTripletConvertTo(const std::string &target_units)
Composes a 3-element column vector for the supplied location or orientation.