%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGLGear.h"
-//#include <algorithm>
namespace JSBSim {
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
+FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex, int number) : Exec(fdmex)
{
string tmp;
-
+
+ GearNumber = number;
+
*AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
>> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
- >> rollingFCoeff >> sSteerType >> sBrakeGroup
+ >> rollingFCoeff >> sSteerType >> sBrakeGroup
>> maxSteerAngle >> sRetractable;
if (sBrakeGroup == "LEFT" ) eBrakeGrp = bgLeft;
cerr << "Improper steering type specification in config file: "
<< sSteerType << " is undefined." << endl;
}
-
+
if ( sRetractable == "RETRACT" ) {
isRetractable = true;
} else {
isRetractable = false;
- }
-
+ }
+
GearUp = false;
GearDown = true;
Servicable = true;
State = Exec->GetState();
Aircraft = Exec->GetAircraft();
- Position = Exec->GetPosition();
- Rotation = Exec->GetRotation();
+ Propagate = Exec->GetPropagate();
+ Auxiliary = Exec->GetAuxiliary();
FCS = Exec->GetFCS();
MassBalance = Exec->GetMassBalance();
TakeoffReported = LandingReported = false;
LandingDistanceTraveled = TakeoffDistanceTraveled = TakeoffDistanceTraveled50ft = 0.0;
MaximumStrutForce = MaximumStrutTravel = 0.0;
+ SideForce = RollingForce = 0.0;
SinkRate = GroundSpeed = 0.0;
- vWhlBodyVec = (vXYZ - MassBalance->GetXYZcg()) / 12.0;
- vWhlBodyVec(eX) = -vWhlBodyVec(eX);
- vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
-
- vLocalGear = State->GetTb2l() * vWhlBodyVec;
+ vWhlBodyVec = MassBalance->StructuralToBody(vXYZ);
+
+ vLocalGear = Propagate->GetTb2l() * vWhlBodyVec;
compressLength = 0.0;
compressSpeed = 0.0;
FGLGear::FGLGear(const FGLGear& lgear)
{
+ GearNumber = lgear.GearNumber;
+
State = lgear.State;
Aircraft = lgear.Aircraft;
- Position = lgear.Position;
- Rotation = lgear.Rotation;
+ Propagate = lgear.Propagate;
+ Auxiliary = lgear.Auxiliary;
Exec = lgear.Exec;
FCS = lgear.FCS;
MassBalance = lgear.MassBalance;
TakeoffDistanceTraveled50ft = lgear.TakeoffDistanceTraveled50ft;
MaximumStrutForce = lgear.MaximumStrutForce;
MaximumStrutTravel = lgear.MaximumStrutTravel;
+ SideForce = lgear.SideForce;
+ RollingForce = lgear.RollingForce;
kSpring = lgear.kSpring;
bDamp = lgear.bDamp;
FGColumnVector3& FGLGear::Force(void)
{
- double SteerGain = 0;
double SinWheel, CosWheel;
- double deltaSlip;
double deltaT = State->Getdt()*Aircraft->GetRate();
- double maxdeltaSlip = 0.5*deltaT;
vForce.InitMatrix();
vMoment.InitMatrix();
} else {
GearUp = false;
GearDown = true;
- }
-
+ }
+
+ // Compute the steering angle in any case.
+ // Will make shure that animations will look right.
+ switch (eSteerType) {
+ case stSteer:
+ SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber);
+ break;
+ case stFixed:
+ SteerAngle = 0.0;
+ break;
+ case stCaster:
+ // Note to Jon: This is not correct for castering gear. I'll fix it later.
+ SteerAngle = 0.0;
+ break;
+ default:
+ cerr << "Improper steering type membership detected for this gear." << endl;
+ break;
+ }
+
if (GearDown) {
- vWhlBodyVec = (vXYZ - MassBalance->GetXYZcg()) / 12.0;
- vWhlBodyVec(eX) = -vWhlBodyVec(eX);
- vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
+ vWhlBodyVec = MassBalance->StructuralToBody(vXYZ);
// vWhlBodyVec now stores the vector from the cg to this wheel
- vLocalGear = State->GetTb2l() * vWhlBodyVec;
+ vLocalGear = Propagate->GetTb2l() * vWhlBodyVec;
// vLocalGear now stores the vector from the cg to the wheel in local coords.
- compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();
+ compressLength = vLocalGear(eZ) - Propagate->GetDistanceAGL();
// The compression length is currently measured in the Z-axis, only, at this time.
// It should be measured along the strut axis. If the local-frame gear position
// (used for calculating damping force) is found by taking the Z-component of the
// wheel velocity.
- vWhlVelVec = State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
-
- vWhlVelVec += Position->GetVel();
-
+ vWhlVelVec = Propagate->GetTb2l() * (Propagate->GetPQR() * vWhlBodyVec);
+ vWhlVelVec += Propagate->GetVel();
compressSpeed = vWhlVelVec(eZ);
// If this is the first time the wheel has made contact, remember some values
if (!FirstContact) {
FirstContact = true;
SinkRate = compressSpeed;
- GroundSpeed = Position->GetVel().Magnitude();
+ GroundSpeed = Propagate->GetVel().Magnitude();
TakeoffReported = false;
}
// If the takeoff run is starting, initialize.
- if ((Position->GetVel().Magnitude() > 0.1) &&
+ if ((Propagate->GetVel().Magnitude() > 0.1) &&
(FCS->GetBrake(bgLeft) == 0) &&
(FCS->GetBrake(bgRight) == 0) &&
(FCS->GetThrottlePos(0) == 1) && !StartedGroundRun)
break;
}
- switch (eSteerType) {
- case stSteer:
- SteerAngle = -maxSteerAngle * FCS->GetDrCmd() * 0.01745;
- break;
- case stFixed:
- SteerAngle = 0.0;
- break;
- case stCaster:
-// Note to Jon: This is not correct for castering gear. I'll fix it later.
- SteerAngle = 0.0;
- break;
- default:
- cerr << "Improper steering type membership detected for this gear." << endl;
- break;
- }
-
// Transform the wheel velocities from the local axis system to the wheel axis system.
// For now, steering angle is assumed to happen in the Local Z axis,
// not the strut axis as it should be. Will fix this later.
- SinWheel = sin(Rotation->Getpsi() + SteerAngle);
- CosWheel = cos(Rotation->Getpsi() + SteerAngle);
+ SinWheel = sin(Propagate->GetEuler(ePsi) + SteerAngle);
+ CosWheel = cos(Propagate->GetEuler(ePsi) + SteerAngle);
RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel;
SideWhlVel = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel;
if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
WheelSlip = 0.0;
} else if (fabs(RollingWhlVel) < 1.0) {
- WheelSlip = 0.05*radtodeg*atan2(SideWhlVel, RollingWhlVel) + 0.95*WheelSlip;
+ WheelSlip = 0.05*radtodeg*atan2(SideWhlVel, fabs(RollingWhlVel)) + 0.95*WheelSlip;
} else {
- WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
+ WheelSlip = radtodeg*atan2(SideWhlVel, fabs(RollingWhlVel));
}
/*
+ double maxdeltaSlip = 0.5*deltaT;
+
if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
WheelSlip = 0.0;
} else if (RollingWhlVel < 1.0) {
{
WheelSlip = 0.0;
}
-*/
+*/
lastWheelSlip = WheelSlip;
// 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. Will fix this later.
+// of this that avoid the discrete jump (similar to Pacejka). Will fix this later.
if (fabs(WheelSlip) <= 20.0) {
FCoeff = staticFCoeff*WheelSlip/20.0;
} else if (fabs(WheelSlip) <= 40.0) {
// FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
- FCoeff = (dynamicFCoeff*(fabs(WheelSlip) - 20.0)/20.0 +
+ FCoeff = (dynamicFCoeff*(fabs(WheelSlip) - 20.0)/20.0 +
staticFCoeff*(40.0 - fabs(WheelSlip))/20.0)*fabs(WheelSlip)/WheelSlip;
} else {
FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
}
-#if 0
- // A negative force coefficient will result in a force pulling the wheel(s)
- // back instead of trying to stop them from moving.
- if (FCoeff < 0.0)
- FCoeff = 0.0;
-#endif
-
// Compute the vertical force on the wheel using square-law damping (per comment
// in paper AIAA-2000-4303 - see header prologue comments). We might consider
// allowing for both square and linear damping force calculation. Also need to
// Transform the forces back to the body frame and compute the moment.
- vForce = State->GetTl2b() * vLocalForce;
+ vForce = Propagate->GetTl2b() * vLocalForce;
vMoment = vWhlBodyVec * vForce;
} else { // Gear is NOT compressed
WOW = false;
- if (Position->GetDistanceAGL() > 200.0) {
+ // Return to neutral position between 1.0 and 0.8 gear pos.
+ SteerAngle *= max(FCS->GetGearPos()-0.8, 0.0)/0.2;
+
+ if (Propagate->GetDistanceAGL() > 200.0) {
FirstContact = false;
StartedGroundRun = false;
LandingReported = false;
compressLength = 0.0; // reset compressLength to zero for data output validity
}
- if (FirstContact) LandingDistanceTraveled += Position->GetVground()*deltaT;
-
+ if (FirstContact) LandingDistanceTraveled += Auxiliary->GetVground()*deltaT;
+
if (StartedGroundRun) {
- TakeoffDistanceTraveled50ft += Position->GetVground()*deltaT;
- if (WOW) TakeoffDistanceTraveled += Position->GetVground()*deltaT;
+ TakeoffDistanceTraveled50ft += Auxiliary->GetVground()*deltaT;
+ if (WOW) TakeoffDistanceTraveled += Auxiliary->GetVground()*deltaT;
}
- if (ReportEnable && Position->GetVground() <= 0.05 && !LandingReported) {
+ if (ReportEnable && Auxiliary->GetVground() <= 0.05 && !LandingReported) {
if (debug_lvl > 0) Report(erLand);
}
if (ReportEnable && !TakeoffReported &&
- (vLocalGear(eZ) - Position->GetDistanceAGL()) < -50.0)
+ (vLocalGear(eZ) - Propagate->GetDistanceAGL()) < -50.0)
{
if (debug_lvl > 0) Report(erTakeoff);
}
PutMessage("Crash Detected: Simulation FREEZE.");
Exec->Freeze();
}
- }
- return vForce;
+ }
+ return vForce;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
projects / flightgear.git / blobdiff