From e12e81dd230d453041851dabeb13bdc53cdf6dc3 Mon Sep 17 00:00:00 2001 From: curt Date: Sat, 27 May 2000 05:48:14 +0000 Subject: [PATCH] 5/26/2000 updated from Jon and Tony. --- src/FDM/JSBSim/FGAircraft.cpp | 51 ++++++++++++- src/FDM/JSBSim/FGAircraft.h | 35 +++++---- src/FDM/JSBSim/FGAtmosphere.cpp | 4 +- src/FDM/JSBSim/FGControls.cpp | 32 +-------- src/FDM/JSBSim/FGControls.h | 32 +-------- src/FDM/JSBSim/FGDefs.h | 2 +- src/FDM/JSBSim/FGInitialCondition.cpp | 26 ++++--- src/FDM/JSBSim/FGLGear.cpp | 51 +++++++++++-- src/FDM/JSBSim/FGLGear.h | 24 +++++-- src/FDM/JSBSim/FGMatrix.cpp | 30 +++++++- src/FDM/JSBSim/FGMatrix.h | 1 + src/FDM/JSBSim/FGOutput.cpp | 16 +++++ src/FDM/JSBSim/FGPosition.cpp | 53 ++++++++------ src/FDM/JSBSim/FGPosition.h | 2 + src/FDM/JSBSim/FGState.cpp | 47 ++++++------ src/FDM/JSBSim/FGTranslation.cpp | 5 +- src/FDM/JSBSim/FGTrimLong.cpp | 79 +++++++++++++-------- src/FDM/JSBSim/FGUtility.cpp | 3 - src/FDM/JSBSim/FGUtility.h | 1 - src/FDM/JSBSim/filtersjb/FGFCSComponent.cpp | 1 - src/FDM/JSBSim/filtersjb/FGFCSComponent.h | 12 ++-- src/FDM/JSBSim/filtersjb/FGSummer.cpp | 52 ++++++++------ 22 files changed, 350 insertions(+), 209 deletions(-) diff --git a/src/FDM/JSBSim/FGAircraft.cpp b/src/FDM/JSBSim/FGAircraft.cpp index 6d618ae4e..cb8855631 100644 --- a/src/FDM/JSBSim/FGAircraft.cpp +++ b/src/FDM/JSBSim/FGAircraft.cpp @@ -140,7 +140,8 @@ FGAircraft::FGAircraft(FGFDMExec* fdmex) : FGModel(fdmex), vXYZcg(3), vXYZep(3), vEuler(3), -vFs(3) { + vFs(3) +{ Name = "FGAircraft"; AxisIdx["DRAG"] = 0; @@ -150,6 +151,8 @@ vFs(3) { AxisIdx["PITCH"] = 4; AxisIdx["YAW"] = 5; + GearUp = false; + numTanks = numEngines = numSelectedFuelTanks = numSelectedOxiTanks = 0; } @@ -363,7 +366,8 @@ void FGAircraft::FMGear(void) { } else { for (unsigned int i=0;iForce(); + vForces += lGear[i]->Force(); + vMoments += lGear[i]->Moment(); } } } @@ -556,6 +560,10 @@ void FGAircraft::ReadOutput(FGConfigFile* AC_cfg) { *AC_cfg >> parameter; if (parameter == "ON") subsystems += ssCoefficients; } + if (parameter == "GROUND_REACTIONS") { + *AC_cfg >> parameter; + if (parameter == "ON") subsystems += ssGroundReactions; + } } Output->SetSubsystems(subsystems); @@ -668,4 +676,43 @@ string FGAircraft::GetCoefficientValues(void) { ; } +/******************************************************************************/ + +string FGAircraft::GetGroundReactionStrings(void) { + string GroundReactionStrings = ""; + bool firstime = true; + + for (unsigned int i=0;iGetName() + "_WOW, "); + GroundReactionStrings += (lGear[i]->GetName() + "_compressLength, "); + GroundReactionStrings += (lGear[i]->GetName() + "_compressSpeed, "); + GroundReactionStrings += (lGear[i]->GetName() + "_Force"); + + firstime = false; + } + + return GroundReactionStrings; +} + +/******************************************************************************/ + +string FGAircraft::GetGroundReactionValues(void) { + char buff[20]; + string GroundReactionValues = ""; + + bool firstime = true; + + for (unsigned int i=0;iGetWOW()?"1":"0" ) + ", "; + GroundReactionValues += (string(gcvt(lGear[i]->GetCompLen(), 5, buff)) + ", "); + GroundReactionValues += (string(gcvt(lGear[i]->GetCompVel(), 6, buff)) + ", "); + GroundReactionValues += (string(gcvt(lGear[i]->GetCompForce(), 10, buff))); + + firstime = false; + } + + return GroundReactionValues; +} diff --git a/src/FDM/JSBSim/FGAircraft.h b/src/FDM/JSBSim/FGAircraft.h index 828b51cbc..db7ca5e5e 100644 --- a/src/FDM/JSBSim/FGAircraft.h +++ b/src/FDM/JSBSim/FGAircraft.h @@ -160,22 +160,27 @@ public: inline float GetNlf(void) { return nlf; } inline float GetAlphaCLMax(void) { return alphaclmax; } inline float GetAlphaCLMin(void) { return alphaclmin; } + inline void SetAlphaCLMax(float tt) { alphaclmax=tt; } inline void SetAlphaCLMin(float tt) { alphaclmin=tt; } + string GetCoefficientStrings(void); string GetCoefficientValues(void); - - enum { ssSimulation = 1, - ssAerosurfaces = 2, - ssRates = 4, - ssVelocities = 8, - ssForces = 16, - ssMoments = 32, - ssAtmosphere = 64, - ssMassProps = 128, - ssCoefficients = 256, - ssPosition = 512 } subsystems; + string GetGroundReactionStrings(void); + string GetGroundReactionValues(void); + + enum { ssSimulation = 1, + ssAerosurfaces = 2, + ssRates = 4, + ssVelocities = 8, + ssForces = 16, + ssMoments = 32, + ssAtmosphere = 64, + ssMassProps = 128, + ssCoefficients = 256, + ssPosition = 512, + ssGroundReactions = 1024 } subsystems; private: void GetState(void); @@ -203,10 +208,10 @@ private: string CFGVersion; string AircraftName; - int numTanks; - int numEngines; - int numSelectedOxiTanks; - int numSelectedFuelTanks; + unsigned int numTanks; + unsigned int numEngines; + unsigned int numSelectedOxiTanks; + unsigned int numSelectedFuelTanks; FGTank* Tank[MAX_TANKS]; FGEngine *Engine[MAX_ENGINES]; diff --git a/src/FDM/JSBSim/FGAtmosphere.cpp b/src/FDM/JSBSim/FGAtmosphere.cpp index f31daec2c..71025aef7 100644 --- a/src/FDM/JSBSim/FGAtmosphere.cpp +++ b/src/FDM/JSBSim/FGAtmosphere.cpp @@ -108,8 +108,8 @@ bool FGAtmosphere::Run(void) //and we know that headwinds increase the relative //velocity, so to make a positive delta U from the //southerly wind the sign must be switched. - vWindNED*=-1; - vWindUVW=State->GetTl2b()*vWindNED; + vWindNED *= -1; + vWindUVW = State->GetTl2b()*vWindNED; } soundspeed = sqrt(SHRATIO*Reng*temperature); //cout << "Atmosphere: soundspeed: " << soundspeed << endl; diff --git a/src/FDM/JSBSim/FGControls.cpp b/src/FDM/JSBSim/FGControls.cpp index ccaef3ee4..4ce14792e 100644 --- a/src/FDM/JSBSim/FGControls.cpp +++ b/src/FDM/JSBSim/FGControls.cpp @@ -51,36 +51,8 @@ FGControls::~FGControls() { // $Log$ -// Revision 1.10 2000/05/16 19:35:11 curt -// Updates from the Jon and Tony show. -// -// Tony submitted: -// -// JSBsim: -// Added trimming routine, it is longitudinal & in-air only at this point -// Added support for taking wind & weather data from external source -// Added support for flaps. -// Added independently settable pitch trim -// Added alphamin and max to config file, stall modeling and warning to -// follow -// -// c172.cfg: -// Flaps! -// Adjusted Cmo, model should be speed stable now -// -// FG: -// Hooked up Christian's weather code, should be using it soon. -// Hooked up the trimming routine. Note that the X-15 will not trim. -// This is not a model or trimming routine deficiency, just the -// nature of the X-15 -// The trimming routine sets the pitch trim and and throttle at startup. -// The throttle is set using Norman's code for the autothrottle so the -// autothrottle is on by default. --notrim will turn it off. -// Added --vc, --mach, and --notrim switches -// (vc is airspeed in knots) -// uBody, vBody, and wBody are still supported, last one entered -// on the command line counts, i.e. you can set vc or mach or u,v, -// and w but any combination will be ignored. +// Revision 1.11 2000/05/27 03:48:15 curt +// 5/26/2000 updated from Jon and Tony. // // Revision 1.3 2000/04/26 10:55:57 jsb // Made changes as required by Curt to install JSBSim into FGFS diff --git a/src/FDM/JSBSim/FGControls.h b/src/FDM/JSBSim/FGControls.h index 24b37e7bb..3b16b8cf5 100644 --- a/src/FDM/JSBSim/FGControls.h +++ b/src/FDM/JSBSim/FGControls.h @@ -175,36 +175,8 @@ extern FGControls controls; // $Log$ -// Revision 1.9 2000/05/16 19:35:11 curt -// Updates from the Jon and Tony show. -// -// Tony submitted: -// -// JSBsim: -// Added trimming routine, it is longitudinal & in-air only at this point -// Added support for taking wind & weather data from external source -// Added support for flaps. -// Added independently settable pitch trim -// Added alphamin and max to config file, stall modeling and warning to -// follow -// -// c172.cfg: -// Flaps! -// Adjusted Cmo, model should be speed stable now -// -// FG: -// Hooked up Christian's weather code, should be using it soon. -// Hooked up the trimming routine. Note that the X-15 will not trim. -// This is not a model or trimming routine deficiency, just the -// nature of the X-15 -// The trimming routine sets the pitch trim and and throttle at startup. -// The throttle is set using Norman's code for the autothrottle so the -// autothrottle is on by default. --notrim will turn it off. -// Added --vc, --mach, and --notrim switches -// (vc is airspeed in knots) -// uBody, vBody, and wBody are still supported, last one entered -// on the command line counts, i.e. you can set vc or mach or u,v, -// and w but any combination will be ignored. +// Revision 1.10 2000/05/27 03:48:15 curt +// 5/26/2000 updated from Jon and Tony. // // Revision 1.5 2000/05/12 22:45:35 jsb // Removed extraneous namespace identifiers and header files diff --git a/src/FDM/JSBSim/FGDefs.h b/src/FDM/JSBSim/FGDefs.h index 155e67783..6252b0abd 100644 --- a/src/FDM/JSBSim/FGDefs.h +++ b/src/FDM/JSBSim/FGDefs.h @@ -38,7 +38,7 @@ SENTRY #define MAX_TANKS 30 #define GRAVITY 32.174 #define INVGRAVITY 0.031081 -#define EARTHRAD 20898908.00 // feet +#define EARTHRAD 20925650.00 // feet, equatorial #define OMEGAEARTH 7.2685E-3 // rad/sec #define EARTHRADSQRD 437882827922500.0 #define ONESECOND 4.848136811E-6 diff --git a/src/FDM/JSBSim/FGInitialCondition.cpp b/src/FDM/JSBSim/FGInitialCondition.cpp index ce4a6be0b..06a499c61 100644 --- a/src/FDM/JSBSim/FGInitialCondition.cpp +++ b/src/FDM/JSBSim/FGInitialCondition.cpp @@ -70,12 +70,12 @@ FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) { latitude=longitude=0; u=v=w=0; lastSpeedSet=setvt; - if(fdmex != NULL ) { + if(FDMExec != NULL ) { fdmex=FDMExec; fdmex->GetPosition()->Seth(altitude); fdmex->GetAtmosphere()->Run(); } else { - cout << "FGInitialCondition: This class requires a pointer to an initialized FGFDMExec object" << endl; + cout << "FGInitialCondition: This class requires a pointer to an valid FGFDMExec object" << endl; } } @@ -130,7 +130,7 @@ void FGInitialCondition::SetMachIC(float tt) { void FGInitialCondition::SetClimbRateFpmIC(float tt) { - if(vt != 0) { + if(vt > 0.1) { hdot=tt/60; gamma=asin(hdot/vt); } @@ -159,18 +159,20 @@ void FGInitialCondition::SetAltitudeFtIC(float tt) { altitude=tt; fdmex->GetPosition()->Seth(altitude); fdmex->GetAtmosphere()->Run(); + //lets try to make sure the user gets what they intended + switch(lastSpeedSet) { -setvt: + case setvt: SetVtrueKtsIC(vt); break; -setvc: + case setvc: SetVcalibratedKtsIC(vc); break; -setve: + case setve: SetVequivalentKtsIC(ve); break; -setmach: + case setmach: SetMachIC(mach); break; } @@ -211,7 +213,7 @@ float FGInitialCondition::calcVcas(float Mach) { A = pow(((pt-p)/psl+1),0.28571); vcas = sqrt(7*psl/rhosl*(A-1)); - //cout << "calcVcas: vcas= " << vcas*FPSTOKTS << " mach= " << Mach << " pressure: " << p << endl; + //cout << "calcVcas: vcas= " << vcas*FPSTOKTS << " mach= " << Mach << " pressure: " << pt << endl; return vcas; } @@ -263,7 +265,12 @@ bool FGInitialCondition::getMachFromVcas(float *Mach,float vcas) { float const relax =0.9; int i; bool success=false; - + + if(vcas < 0.1) { + Mach=0; + success=true; + return success; + } //initializations d=1; if(findMachInterval(&x1,&x3,vcas)) { @@ -279,6 +286,7 @@ bool FGInitialCondition::getMachFromVcas(float *Mach,float vcas) { //cout << "getMachFromVcas x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl; d=(x3-x1)/d0; x2=x1-d*d0*f1/(f3-f1); + f2=calcVcas(x2)-vcas; if(f1*f2 <= 0.0) { x3=x2; diff --git a/src/FDM/JSBSim/FGLGear.cpp b/src/FDM/JSBSim/FGLGear.cpp index bcd04b329..865fbe6bf 100644 --- a/src/FDM/JSBSim/FGLGear.cpp +++ b/src/FDM/JSBSim/FGLGear.cpp @@ -37,20 +37,26 @@ INCLUDES *******************************************************************************/ #include "FGLGear.h" +#include /******************************************************************************* ************************************ CODE ************************************** *******************************************************************************/ -FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : vXYZ(3), Exec(fdmex) +FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : vXYZ(3), + vMoment(3), + Exec(fdmex) { string tmp; *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3) >> kSpring >> bDamp >> statFCoeff >> brakeCoeff; - State = Exec->GetState(); - Aircraft = Exec->GetAircraft(); - Position = Exec->GetPosition(); + State = Exec->GetState(); + Aircraft = Exec->GetAircraft(); + Position = Exec->GetPosition(); + Rotation = Exec->GetRotation(); + + WOW = false; } @@ -65,10 +71,43 @@ FGLGear::~FGLGear(void) FGColumnVector FGLGear::Force(void) { static FGColumnVector vForce(3); + static FGColumnVector vLocalForce(3); static FGColumnVector vLocalGear(3); + static FGColumnVector vWhlBodyVec(3); + static FGColumnVector vWhlVelVec(3); + + vWhlBodyVec = vXYZ - Aircraft->GetXYZcg(); + vWhlBodyVec(eX) = -vWhlBodyVec(eX); + vWhlBodyVec(eZ) = -vWhlBodyVec(eZ); + vWhlBodyVec = vWhlBodyVec/12.0; + + vLocalGear = State->GetTb2l() * vWhlBodyVec; + + compressLength = vLocalGear(eZ) - Position->GetDistanceAGL(); + + if (compressLength > 0.00) { + + WOW = true; + + vWhlVelVec = State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec); + compressSpeed = vWhlVelVec(eZ) + Position->GetVd(); + + vLocalForce(eZ) = min(-compressLength * kSpring - compressSpeed * bDamp, (float)0.0); + + vForce = State->GetTl2b() * vLocalForce ; + + // currently only aircraft body axis Z-force modeled + vMoment(eX) = vForce(eZ) * vWhlBodyVec(eY); + vMoment(eY) = -vForce(eZ) * vWhlBodyVec(eX); + vMoment(eZ) = 0.0; + + } else { + + WOW = false; + vForce.InitMatrix(); + vMoment.InitMatrix(); + } - vLocalGear = State->GetTb2l() * (vXYZ - Aircraft->GetXYZcg()); - vLocalGear(3) = -vLocalGear(3); return vForce; } diff --git a/src/FDM/JSBSim/FGLGear.h b/src/FDM/JSBSim/FGLGear.h index 84ea0bbea..9b7fac056 100644 --- a/src/FDM/JSBSim/FGLGear.h +++ b/src/FDM/JSBSim/FGLGear.h @@ -74,6 +74,7 @@ CLASS DECLARATION class FGAircraft; class FGPosition; +class FGRotation; class FGLGear { @@ -82,22 +83,35 @@ public: ~FGLGear(void); FGColumnVector Force(void); + FGColumnVector Moment(void) {return vMoment;} + + inline string GetName(void) {return name; } + inline bool GetWOW(void) {return WOW; } + inline float GetCompLen(void) {return compressLength;} + inline float GetCompVel(void) {return compressSpeed; } + inline float GetCompForce(void) {return Force()(3); } private: + enum {eX=1, eY, eZ}; FGColumnVector vXYZ; - float kSpring, bDamp, compressLength; + FGColumnVector vMoment; + float kSpring, bDamp, compressLength, compressSpeed; float statFCoeff, rollFCoeff, skidFCoeff; float frictionForce, compForce; float brakePct, brakeForce, brakeCoeff; + bool WOW; string name; - FGState* State; - FGAircraft* Aircraft; - FGPosition* Position; - FGFDMExec* Exec; + + FGFDMExec* Exec; + FGState* State; + FGAircraft* Aircraft; + FGPosition* Position; + FGRotation* Rotation; }; #include "FGAircraft.h" #include "FGPosition.h" +#include "FGRotation.h" /******************************************************************************/ #endif diff --git a/src/FDM/JSBSim/FGMatrix.cpp b/src/FDM/JSBSim/FGMatrix.cpp index 1ed6b820c..c7f40c4b7 100644 --- a/src/FDM/JSBSim/FGMatrix.cpp +++ b/src/FDM/JSBSim/FGMatrix.cpp @@ -532,6 +532,7 @@ FGColumnVector operator*(const double scalar, const FGColumnVector& C) } /******************************************************************************/ + float FGColumnVector::Magnitude(void) { double num=0.0; @@ -551,6 +552,33 @@ float FGColumnVector::Magnitude(void) FGColumnVector FGColumnVector::Normalize(void) { - return *this/Magnitude(); + double Mag = Magnitude(); + + for (unsigned int i=1; i<=Rows(); i++) + for (unsigned int j=1; j<=Cols(); j++) + data[i][j] = data[i][j]/Mag; + + return *this; +} + +/******************************************************************************/ + +FGColumnVector FGColumnVector::operator*(const FGColumnVector& V) +{ + if (Rows() != 3 || V.Rows() != 3) { + MatrixException mE; + mE.Message = "Invalid row count in vector cross product function"; + throw mE; + } + + FGColumnVector Product(3); + + Product(1) = data[2][1] * V(3) - data[3][1] * V(2); + Product(2) = data[3][1] * V(1) - data[1][1] * V(3); + Product(3) = data[1][1] * V(2) - data[2][1] * V(1); + + return Product; } +/******************************************************************************/ + diff --git a/src/FDM/JSBSim/FGMatrix.h b/src/FDM/JSBSim/FGMatrix.h index 3faab0548..87af0a2c0 100644 --- a/src/FDM/JSBSim/FGMatrix.h +++ b/src/FDM/JSBSim/FGMatrix.h @@ -121,6 +121,7 @@ public: ~FGColumnVector(); FGColumnVector operator*(const double scalar); + FGColumnVector operator*(const FGColumnVector& V); // Cross product operator FGColumnVector operator/(const double scalar); FGColumnVector operator+(const FGColumnVector& B); FGColumnVector operator-(const FGColumnVector& B); diff --git a/src/FDM/JSBSim/FGOutput.cpp b/src/FDM/JSBSim/FGOutput.cpp index 5e90adb18..a9fc0cac1 100644 --- a/src/FDM/JSBSim/FGOutput.cpp +++ b/src/FDM/JSBSim/FGOutput.cpp @@ -186,6 +186,10 @@ void FGOutput::DelimitedOutput(void) cout << ", "; cout << Aircraft->GetCoefficientStrings(); } + if (SubSystems & FGAircraft::ssGroundReactions) { + cout << ", "; + cout << Aircraft->GetGroundReactionStrings(); + } cout << endl; dFirstPass = false; @@ -251,6 +255,10 @@ void FGOutput::DelimitedOutput(void) cout << ", "; cout << Aircraft->GetCoefficientValues(); } + if (SubSystems & FGAircraft::ssGroundReactions) { + cout << ", "; + cout << Aircraft->GetGroundReactionValues(); + } cout << endl; } @@ -321,6 +329,10 @@ void FGOutput::DelimitedOutput(string fname) datafile << ", "; datafile << Aircraft->GetCoefficientStrings(); } + if (SubSystems & FGAircraft::ssGroundReactions) { + datafile << ", "; + datafile << Aircraft->GetGroundReactionStrings(); + } datafile << endl; sFirstPass = false; } @@ -385,6 +397,10 @@ void FGOutput::DelimitedOutput(string fname) datafile << ", "; datafile << Aircraft->GetCoefficientValues(); } + if (SubSystems & FGAircraft::ssGroundReactions) { + datafile << ", "; + datafile << Aircraft->GetGroundReactionValues(); + } datafile << endl; datafile.flush(); } diff --git a/src/FDM/JSBSim/FGPosition.cpp b/src/FDM/JSBSim/FGPosition.cpp index bdbd5668b..d6538a096 100644 --- a/src/FDM/JSBSim/FGPosition.cpp +++ b/src/FDM/JSBSim/FGPosition.cpp @@ -57,11 +57,14 @@ INCLUDES # include # ifdef FG_HAVE_STD_INCLUDES # include +# include # else # include +# include # endif #else # include +# include #endif #include "FGPosition.h" @@ -91,17 +94,24 @@ FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex), LongitudeDot = LatitudeDot = RadiusDot = 0.0; lastLongitudeDot = lastLatitudeDot = lastRadiusDot = 0.0; Longitude = Latitude = 0.0; - h = 0.0; - Radius = EARTHRAD + h; - gamma=Vt=0.0; - RunwayRadius = EARTHRAD; + h = gamma = Vt = 0.0; + SeaLevelRadius = EARTHRAD; // For initialization ONLY + Radius = SeaLevelRadius + h; + RunwayRadius = SeaLevelRadius; + DistanceAGL = Radius - RunwayRadius; // Geocentric } /******************************************************************************/ FGPosition::~FGPosition(void) {} -/******************************************************************************/ +/*************************************************************************** Run +Purpose: Called on a schedule to perform Positioning algorithms +Notes: [TP] Make sure that -Vt <= hdot <= Vt, which, of course, should always + be the case + [JB] Run in standalone mode, SeaLevelRadius will be EARTHRAD. In FGFS, + SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass. +*/ bool FGPosition:: Run(void) { double cosLat; @@ -110,7 +120,9 @@ bool FGPosition:: Run(void) { if (!FGModel::Run()) { GetState(); - vVel = State->GetTl2b()*vUVW; + invMass = 1.0 / Aircraft->GetMass(); + Radius = h + SeaLevelRadius; + invRadius = 1.0 / Radius; cosLat = cos(Latitude); if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat); @@ -122,22 +134,22 @@ bool FGPosition:: Run(void) { Latitude += 0.5*dt*rate*(LatitudeDot + lastLatitudeDot); Radius += 0.5*dt*rate*(RadiusDot + lastRadiusDot); - h = Radius - EARTHRAD; // Geocentric + h = Radius - SeaLevelRadius; // Geocentric DistanceAGL = Radius - RunwayRadius; // Geocentric - hoverb = h/b; + hoverb = DistanceAGL/b; - if(Vt > 0) { - hdot_Vt=RadiusDot/Vt; - //make sure that -Vt <= hdot <= Vt, which, of course, should always be the case - if(fabs(hdot_Vt) <= 1) gamma= asin(hdot_Vt); - } else - gamma=0.0; + if (Vt > 0) { + hdot_Vt = RadiusDot/Vt; + if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt); + } else { + gamma = 0.0; + } - lastLatitudeDot = LatitudeDot; + lastLatitudeDot = LatitudeDot; lastLongitudeDot = LongitudeDot; - lastRadiusDot = RadiusDot; + lastRadiusDot = RadiusDot; return false; @@ -151,11 +163,10 @@ bool FGPosition:: Run(void) { void FGPosition::GetState(void) { dt = State->Getdt(); - vUVW = Translation->GetUVW(); - Vt = Translation->GetVt(); - invMass = 1.0 / Aircraft->GetMass(); - invRadius = 1.0 / (h + EARTHRAD); - Radius = h + EARTHRAD; + vUVW = Translation->GetUVW(); + Vt = Translation->GetVt(); + vVel = State->GetTb2l()*vUVW; + b = Aircraft->GetWingSpan(); } diff --git a/src/FDM/JSBSim/FGPosition.h b/src/FDM/JSBSim/FGPosition.h index a74fc6304..a03d859ec 100644 --- a/src/FDM/JSBSim/FGPosition.h +++ b/src/FDM/JSBSim/FGPosition.h @@ -61,6 +61,7 @@ class FGPosition : public FGModel { float dt; double RunwayRadius; double DistanceAGL; + double SeaLevelRadius; double gamma; double Vt; double hoverb,b; @@ -89,6 +90,7 @@ public: void SetLongitude(double tt) { Longitude = tt; } void Seth(double tt) { h = tt; } void SetRunwayRadius(double tt) { RunwayRadius = tt; } + void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt;} void SetDistanceAGL(double tt) { DistanceAGL = tt; } bool Run(void); diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp index 6a60656ec..15583cb2c 100644 --- a/src/FDM/JSBSim/FGState.cpp +++ b/src/FDM/JSBSim/FGState.cpp @@ -66,7 +66,8 @@ INCLUDES FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3), mTl2b(3,3), mTs2b(3,3), -vQtrn(4) { + vQtrn(4) +{ FDMExec = fdex; adot = bdot = 0.0; @@ -316,9 +317,15 @@ float FGState::GetParameter(int val_idx) { case FG_ALTITUDE: return FDMExec->GetPosition()->Geth(); case FG_BI2VEL: - return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * FDMExec->GetTranslation()->GetVt()); + if(FDMExec->GetTranslation()->GetVt() > 0) + return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * FDMExec->GetTranslation()->GetVt()); + else + return 0; case FG_CI2VEL: - return FDMExec->GetAircraft()->Getcbar()/(2.0 * FDMExec->GetTranslation()->GetVt()); + if(FDMExec->GetTranslation()->GetVt() > 0) + return FDMExec->GetAircraft()->Getcbar()/(2.0 * FDMExec->GetTranslation()->GetVt()); + else + return 0; case FG_THROTTLE_CMD: return FDMExec->GetFCS()->GetThrottleCmd(0); case FG_THROTTLE_POS: @@ -412,18 +419,18 @@ void FGState::CalcMatrices(void) { Q1Q3 = vQtrn(2)*vQtrn(4); Q2Q3 = vQtrn(3)*vQtrn(4); - mTb2l(1,1) = Q0Q0 + Q1Q1 - Q2Q2 - Q3Q3; - mTb2l(1,2) = 2*(Q1Q2 + Q0Q3); - mTb2l(1,3) = 2*(Q1Q3 - Q0Q2); - mTb2l(2,1) = 2*(Q1Q2 - Q0Q3); - mTb2l(2,2) = Q0Q0 - Q1Q1 + Q2Q2 - Q3Q3; - mTb2l(2,3) = 2*(Q2Q3 + Q0Q1); - mTb2l(3,1) = 2*(Q1Q3 + Q0Q2); - mTb2l(3,2) = 2*(Q2Q3 - Q0Q1); - mTb2l(3,3) = Q0Q0 - Q1Q1 - Q2Q2 + Q3Q3; - - mTl2b = mTb2l; - mTl2b.T(); + mTl2b(1,1) = Q0Q0 + Q1Q1 - Q2Q2 - Q3Q3; + mTl2b(1,2) = 2*(Q1Q2 + Q0Q3); + mTl2b(1,3) = 2*(Q1Q3 - Q0Q2); + mTl2b(2,1) = 2*(Q1Q2 - Q0Q3); + mTl2b(2,2) = Q0Q0 - Q1Q1 + Q2Q2 - Q3Q3; + mTl2b(2,3) = 2*(Q2Q3 + Q0Q1); + mTl2b(3,1) = 2*(Q1Q3 + Q0Q2); + mTl2b(3,2) = 2*(Q2Q3 - Q0Q1); + mTl2b(3,3) = Q0Q0 - Q1Q1 - Q2Q2 + Q3Q3; + + mTb2l = mTl2b; + mTb2l.T(); } /******************************************************************************/ @@ -449,13 +456,13 @@ void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) { FGColumnVector FGState::CalcEuler(void) { static FGColumnVector vEuler(3); - if (mTb2l(3,3) == 0) vEuler(ePhi) = 0.0; - else vEuler(ePhi) = atan2(mTb2l(2,3), mTb2l(3,3)); + if (mTl2b(3,3) == 0) vEuler(ePhi) = 0.0; + else vEuler(ePhi) = atan2(mTl2b(2,3), mTl2b(3,3)); - vEuler(eTht) = asin(-mTb2l(1,3)); + vEuler(eTht) = asin(-mTl2b(1,3)); - if (mTb2l(1,1) == 0.0) vEuler(ePsi) = 0.0; - else vEuler(ePsi) = atan2(mTb2l(1,2), mTb2l(1,1)); + if (mTl2b(1,1) == 0.0) vEuler(ePsi) = 0.0; + else vEuler(ePsi) = atan2(mTl2b(1,2), mTl2b(1,1)); if (vEuler(ePsi) < 0.0) vEuler(ePsi) += 2*M_PI; diff --git a/src/FDM/JSBSim/FGTranslation.cpp b/src/FDM/JSBSim/FGTranslation.cpp index 90450dbef..b7e9e90b8 100644 --- a/src/FDM/JSBSim/FGTranslation.cpp +++ b/src/FDM/JSBSim/FGTranslation.cpp @@ -116,7 +116,7 @@ bool FGTranslation::Run(void) { vUVWdot = mVel*vPQR + vForces/Mass; - vNcg=vUVWdot*INVGRAVITY; + vNcg = vUVWdot * INVGRAVITY; vUVW += 0.5*dt*rate*(vlastUVWdot + vUVWdot) + vWindUVW; @@ -132,7 +132,6 @@ bool FGTranslation::Run(void) { Mach = Vt / State->Geta(); - vlastUVWdot = vUVWdot; } else {} @@ -151,10 +150,8 @@ void FGTranslation::GetState(void) { Mass = Aircraft->GetMass(); rho = Atmosphere->GetDensity(); - vEuler = Rotation->GetEuler(); vWindUVW = Atmosphere->GetWindUVW(); - } diff --git a/src/FDM/JSBSim/FGTrimLong.cpp b/src/FDM/JSBSim/FGTrimLong.cpp index 9ab7dea50..846a264d3 100644 --- a/src/FDM/JSBSim/FGTrimLong.cpp +++ b/src/FDM/JSBSim/FGTrimLong.cpp @@ -1,40 +1,40 @@ /******************************************************************************* - + Header: FGTrimLong.cpp Author: Tony Peden Date started: 9/8/99 - - ------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) ------------- - + + --------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) --------- + This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. - + This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - + You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - + Further information about the GNU General Public License can also be found on the world wide web at http://www.gnu.org. - - + + HISTORY -------------------------------------------------------------------------------- 9/8/99 TP Created - - + + FUNCTIONAL DESCRIPTION -------------------------------------------------------------------------------- - + This class takes the given set of IC's and finds the angle of attack, elevator, and throttle setting required to fly steady level. This is currently for in-air -conditions only. It is implemented using an iterative, one-axis-at-a-time +conditions only. It is implemented using an iterative, one-axis-at-a-time scheme. */ // !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!! @@ -54,10 +54,6 @@ INCLUDES CLASS DECLARATION *******************************************************************************/ - - - - FGTrimLong::FGTrimLong(FGFDMExec *FDMExec,FGInitialCondition *FGIC ) { Ncycles=40; @@ -82,14 +78,13 @@ FGTrimLong::FGTrimLong(FGFDMExec *FDMExec,FGInitialCondition *FGIC ) { trimudot=true; axis_count=0; - } -FGTrimLong::~FGTrimLong(void) {} - - +/******************************************************************************/ +FGTrimLong::~FGTrimLong(void) {} +/******************************************************************************/ void FGTrimLong::TrimStats() { cout << endl << " Trim Statistics: " << endl; @@ -102,6 +97,8 @@ void FGTrimLong::TrimStats() { } } +/******************************************************************************/ + void FGTrimLong::Report(void) { cout << endl << " Trim Results" << endl; cout << " Alpha: " << fdmex->GetTranslation()->Getalpha()*RADTODEG @@ -117,6 +114,8 @@ void FGTrimLong::Report(void) { << " Tolerance " << A_Tolerance << endl; } +/******************************************************************************/ + void FGTrimLong::ReportState(void) { cout << endl << " JSBSim Trim Report" << endl; cout << " Weight: " << fdmex->GetAircraft()->GetWeight() @@ -175,6 +174,8 @@ void FGTrimLong::ReportState(void) { cout << " Throttle: " << fdmex->GetFCS()->GetThrottlePos(0)/100 << endl; } +/******************************************************************************/ + void FGTrimLong::setThrottlesPct(float tt) { float tMin,tMax; @@ -187,6 +188,7 @@ void FGTrimLong::setThrottlesPct(float tt) { } } +/******************************************************************************/ int FGTrimLong::checkLimits(trimfp fp, float current, float min, float max) { float lo,hi; @@ -209,12 +211,15 @@ int FGTrimLong::checkLimits(trimfp fp, float current, float min, float max) { return result; } -bool FGTrimLong::solve(trimfp fp,float guess,float desired, float *result, float eps, float min, float max, int max_iterations, int *actual_its) { +/******************************************************************************/ + +bool FGTrimLong::solve(trimfp fp,float guess,float desired, float *result, + float eps, float min, float max, int max_iterations, int *actual_its) { float x1,x2,x3,f1,f2,f3,d,d0; float const relax =0.9; - x1=x3=0; int i; + x1 = x3 = x2 = 0; d=1; bool success=false; //initializations @@ -233,8 +238,8 @@ bool FGTrimLong::solve(trimfp fp,float guess,float desired, float *result, float i=0; while ((fabs(d) > eps) && (i < max_iterations)) { if(Debug > 1) - cout << "FGTrimLong::solve i,x1,x2,x3: " << i << ", " << x1 << ", " << x2 << ", " << x3 << endl; - + cout << "FGTrimLong::solve i,x1,x2,x3: " << i << ", " << x1 + << ", " << x2 << ", " << x3 << endl; d=(x3-x1)/d0; x2=x1-d*d0*f1/(f3-f1); // if(x2 < min) @@ -265,8 +270,10 @@ bool FGTrimLong::solve(trimfp fp,float guess,float desired, float *result, float return success; } -bool FGTrimLong::findInterval(trimfp fp, float *lo, float *hi,float guess,float desired,int max_iterations) { +/******************************************************************************/ +bool FGTrimLong::findInterval(trimfp fp, float *lo, float *hi,float guess, + float desired,int max_iterations) { int i=0; bool found=false; float flo,fhi,fguess; @@ -292,36 +299,52 @@ bool FGTrimLong::findInterval(trimfp fp, float *lo, float *hi,float guess,float } } if(Debug > 1) - cout << "FGTrimLong::findInterval: i=" << i << " Lo= " << xlo << " Hi= " << xhi << " flo*fhi: " << flo*fhi << endl; + cout << "FGTrimLong::findInterval: i=" << i << " Lo= " << xlo + << " Hi= " << xhi << " flo*fhi: " << flo*fhi << endl; } while((found == 0) && (i <= max_iterations)); *lo=xlo; *hi=xhi; return found; } +/******************************************************************************/ + float FGTrimLong::udot_func(float x) { setThrottlesPct(x); fdmex->RunIC(fgic); return fdmex->GetTranslation()->GetUVWdot()(1); } +/******************************************************************************/ + float FGTrimLong::wdot_func(float x) { fgic->SetAlphaDegIC(x); fdmex->RunIC(fgic); return fdmex->GetTranslation()->GetUVWdot()(3); } +/******************************************************************************/ + float FGTrimLong::qdot_func(float x) { fdmex->GetFCS()->SetPitchTrimCmd(x); fdmex->RunIC(fgic); return fdmex->GetRotation()->GetPQRdot()(2); } +/******************************************************************************/ + bool FGTrimLong::DoTrim(void) { int k=0,j=0,sum=0,trim_failed=0,jmax=Naxis; int its; float step,temp,min,max; + if(fgic->GetVtrueKtsIC() < 1) { + cout << "Trim failed, on-ground trimming not yet implemented." << endl; + cout << "Or did you *really* mean to start in-air" + << " with less than 1 knot airspeed?" << endl; + return false; + } + trimfp fp; fgic -> SetAlphaDegIC((alphaMin+alphaMax)/2); @@ -421,5 +444,5 @@ bool FGTrimLong::DoTrim(void) { } - //YOU WERE WARNED, BUT YOU DID IT ANYWAY. + diff --git a/src/FDM/JSBSim/FGUtility.cpp b/src/FDM/JSBSim/FGUtility.cpp index 6b620a1f4..ffd3de10e 100644 --- a/src/FDM/JSBSim/FGUtility.cpp +++ b/src/FDM/JSBSim/FGUtility.cpp @@ -69,9 +69,6 @@ INCLUDES FGUtility::FGUtility() { - // Move constant stuff to here (if any) so it won't take CPU time - // in the methods below. - SeaLevelR = EARTHRAD * ECCENT; } diff --git a/src/FDM/JSBSim/FGUtility.h b/src/FDM/JSBSim/FGUtility.h index a5a4ecd1d..4fb247ad9 100644 --- a/src/FDM/JSBSim/FGUtility.h +++ b/src/FDM/JSBSim/FGUtility.h @@ -58,7 +58,6 @@ public: protected: private: - float SeaLevelR; FGState* State; FGFDMExec* FDMExec; }; diff --git a/src/FDM/JSBSim/filtersjb/FGFCSComponent.cpp b/src/FDM/JSBSim/filtersjb/FGFCSComponent.cpp index ac59cdca3..694db40f1 100644 --- a/src/FDM/JSBSim/filtersjb/FGFCSComponent.cpp +++ b/src/FDM/JSBSim/filtersjb/FGFCSComponent.cpp @@ -1,4 +1,3 @@ - /******************************************************************************* Module: FGFCSComponent.cpp diff --git a/src/FDM/JSBSim/filtersjb/FGFCSComponent.h b/src/FDM/JSBSim/filtersjb/FGFCSComponent.h index 2b0f3b77b..8e4dd0b54 100644 --- a/src/FDM/JSBSim/filtersjb/FGFCSComponent.h +++ b/src/FDM/JSBSim/filtersjb/FGFCSComponent.h @@ -44,19 +44,17 @@ INCLUDES #ifdef FGFS # include -# include STL_STRING - FG_USING_STD(string); -#else -# include #endif +#include + /******************************************************************************* DEFINES *******************************************************************************/ -class FGFCS; +using std::string; -using namespace std; +class FGFCS; /******************************************************************************* CLASS DECLARATION @@ -89,7 +87,7 @@ public: inline string GetName(void) {return Name;} }; -#include +#include "../FGFCS.h" #endif diff --git a/src/FDM/JSBSim/filtersjb/FGSummer.cpp b/src/FDM/JSBSim/filtersjb/FGSummer.cpp index 7f375c789..02dc930f5 100644 --- a/src/FDM/JSBSim/filtersjb/FGSummer.cpp +++ b/src/FDM/JSBSim/filtersjb/FGSummer.cpp @@ -1,4 +1,3 @@ - /******************************************************************************* Module: FGSummer.cpp @@ -38,7 +37,7 @@ COMMENTS, REFERENCES, and NOTES INCLUDES *******************************************************************************/ -#include "FGSummer.h" +#include "FGSummer.h" /******************************************************************************* ************************************ CODE ************************************** @@ -66,12 +65,16 @@ FGSummer::FGSummer(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs), while ((token = AC_cfg->GetValue()) != "/COMPONENT") { *AC_cfg >> token; + if (token == "ID") { + *AC_cfg >> ID; - cout << " ID: " << ID << endl; + cout << " ID: " << ID << endl; + } else if (token == "INPUT") { + token = AC_cfg->GetValue("INPUT"); - cout << " INPUT: " << token << endl; + cout << " INPUT: " << token << endl; if (token.find("FG_") != token.npos) { *AC_cfg >> token; tmpInputIndex = fcs->GetState()->GetParameterIndex(token); @@ -82,17 +85,20 @@ FGSummer::FGSummer(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs), InputIndices.push_back(tmpInputIndex); InputTypes.push_back(itFCS); } - } else if (token == "CLIPTO") { - *AC_cfg >> clipmin >> clipmax; - if(clipmax > clipmin) { - clip=true; - cout << " CLIPTO: " << clipmin << ", " << clipmax << endl; - } + + } else if (token == "CLIPTO") { + + *AC_cfg >> clipmin >> clipmax; + if (clipmax > clipmin) { + clip = true; + cout << " CLIPTO: " << clipmin << ", " << clipmax << endl; + } + } else if (token == "OUTPUT") { - + IsOutput = true; *AC_cfg >> sOutputIdx; - cout << " OUTPUT: " <GetState()->GetParameterIndex(sOutputIdx); } } @@ -109,9 +115,10 @@ bool FGSummer::Run(void ) unsigned int idx; // The Summer takes several inputs, so do not call the base class Run() - // FGFCSComponent::Run(); + // FGFCSComponent::Run(); Output = 0.0; + for (idx=0; idxGetComponentOutput(InputIndices[idx]); + break; } - } - if(clip) { - if(Output > clipmax) - Output=clipmax; - else if(Output < clipmin) - Output=clipmin; - } - - if (IsOutput) { SetOutput(); } - //cout << "Out FGSummer::Run" << endl; + + if (clip) { + if (Output > clipmax) Output = clipmax; + else if (Output < clipmin) Output = clipmin; + } + + if (IsOutput) SetOutput(); + return true; } -- 2.39.5