From 5b340578ae65a47f0b29bf9bd301f92391955ad6 Mon Sep 17 00:00:00 2001 From: ehofman Date: Sat, 11 Sep 2004 12:41:05 +0000 Subject: [PATCH] Sync. w. JSBSim CVS. --- src/FDM/JSBSim/FGAtmosphere.cpp | 22 +++ src/FDM/JSBSim/FGAtmosphere.h | 14 ++ src/FDM/JSBSim/FGAuxiliary.cpp | 10 +- src/FDM/JSBSim/FGColumnVector3.h | 9 ++ src/FDM/JSBSim/FGEngine.cpp | 6 +- src/FDM/JSBSim/FGFCS.cpp | 29 ++++ src/FDM/JSBSim/FGFCS.h | 20 ++- src/FDM/JSBSim/FGGroundReactions.cpp | 4 +- src/FDM/JSBSim/FGLGear.cpp | 48 +++--- src/FDM/JSBSim/FGLGear.h | 9 +- src/FDM/JSBSim/FGLocation.cpp | 13 +- src/FDM/JSBSim/FGNozzle.cpp | 12 +- src/FDM/JSBSim/FGNozzle.h | 2 +- src/FDM/JSBSim/FGOutput.cpp | 6 +- src/FDM/JSBSim/FGPropagate.cpp | 15 +- src/FDM/JSBSim/FGPropagate.h | 16 +- src/FDM/JSBSim/FGPropeller.cpp | 19 ++- src/FDM/JSBSim/FGPropeller.h | 2 +- src/FDM/JSBSim/FGQuaternion.cpp | 7 +- src/FDM/JSBSim/FGQuaternion.h | 219 +++++++++++++-------------- src/FDM/JSBSim/FGThruster.cpp | 18 ++- src/FDM/JSBSim/FGThruster.h | 11 +- src/FDM/JSBSim/FGTrimAxis.cpp | 37 +++-- src/FDM/JSBSim/FGTurbine.cpp | 1 + src/FDM/JSBSim/JSBSim.cxx | 42 ++--- 25 files changed, 374 insertions(+), 217 deletions(-) diff --git a/src/FDM/JSBSim/FGAtmosphere.cpp b/src/FDM/JSBSim/FGAtmosphere.cpp index 1a75a3f8b..d779258a5 100644 --- a/src/FDM/JSBSim/FGAtmosphere.cpp +++ b/src/FDM/JSBSim/FGAtmosphere.cpp @@ -87,6 +87,8 @@ FGAtmosphere::FGAtmosphere(FGFDMExec* fdmex) : FGModel(fdmex) TurbGain = 0.0; TurbRate = 1.0; + T_dev_sl = T_dev = delta_T = 0.0; + bind(); Debug(0); } @@ -234,6 +236,17 @@ void FGAtmosphere::Calculate(double altitude) } + T_dev = 0.0; + if (delta_T != 0.0) { + T_dev = delta_T; + } else { + if ((h < 36089.239) && (T_dev_sl != 0.0)) { + T_dev = T_dev_sl * ( 1.0 - (h/36089.239)); + } + } + density_altitude = h + T_dev * 66.7; + + reftemp+=T_dev; if (slope == 0) { intTemperature = reftemp; intPressure = refpress*exp(-Inertial->SLgravity()/(reftemp*Reng)*(altitude-htab[i])); @@ -430,6 +443,12 @@ void FGAtmosphere::bind(void) &FGAtmosphere::GetSoundSpeedRatio); PropertyManager->Tie("atmosphere/psiw-rad", this, &FGAtmosphere::GetWindPsi); + PropertyManager->Tie("atmosphere/delta-T", this, + &FGAtmosphere::GetDeltaT, &FGAtmosphere::SetDeltaT); + PropertyManager->Tie("atmosphere/T-sl-dev-F", this, + &FGAtmosphere::GetSLTempDev, &FGAtmosphere::SetSLTempDev); + PropertyManager->Tie("atmosphere/density-altitude", this, + &FGAtmosphere::GetDensityAltitude); PropertyManager->Tie("atmosphere/p-turb-rad_sec", this,1, (PMF)&FGAtmosphere::GetTurbPQR); PropertyManager->Tie("atmosphere/q-turb-rad_sec", this,2, @@ -450,6 +469,9 @@ void FGAtmosphere::unbind(void) PropertyManager->Untie("atmosphere/rho-sl-slugs_ft3"); PropertyManager->Untie("atmosphere/P-sl-psf"); PropertyManager->Untie("atmosphere/a-sl-fps"); + PropertyManager->Untie("atmosphere/delta-T"); + PropertyManager->Untie("atmosphere/T-sl-dev-F"); + PropertyManager->Untie("atmosphere/density-altitude"); PropertyManager->Untie("atmosphere/theta-norm"); PropertyManager->Untie("atmosphere/sigma-norm"); PropertyManager->Untie("atmosphere/delta-norm"); diff --git a/src/FDM/JSBSim/FGAtmosphere.h b/src/FDM/JSBSim/FGAtmosphere.h index fb48a80d6..711057f75 100644 --- a/src/FDM/JSBSim/FGAtmosphere.h +++ b/src/FDM/JSBSim/FGAtmosphere.h @@ -129,6 +129,19 @@ public: /// Provides the external atmosphere model with an interface to set the pressure. inline void SetExPressure(double p) { exPressure=p; } + /// Sets the temperature deviation at sea-level in degrees Fahrenheit + inline void SetSLTempDev(double d) { T_dev_sl = d; } + /// Gets the temperature deviation at sea-level in degrees Fahrenheit + inline double GetSLTempDev(void) const { return T_dev_sl; } + /// Sets the current delta-T in degrees Fahrenheit + inline void SetDeltaT(double d) { delta_T = d; } + /// Gets the current delta-T in degrees Fahrenheit + inline double GetDeltaT(void) const { return delta_T; } + /// Gets the at-altitude temperature deviation in degrees Fahrenheit + inline double GetTempDev(void) const { return T_dev; } + /// Gets the density altitude in feet + inline double GetDensityAltitude(void) const { return density_altitude; } + /// Sets the wind components in NED frame. inline void SetWindNED(double wN, double wE, double wD) { vWindNED(1)=wN; vWindNED(2)=wE; vWindNED(3)=wD;} @@ -164,6 +177,7 @@ protected: bool useExternal; double exTemperature,exDensity,exPressure; double intTemperature, intDensity, intPressure; + double T_dev_sl, T_dev, delta_T, density_altitude; double MagnitudedAccelDt, MagnitudeAccel, Magnitude; double TurbGain; diff --git a/src/FDM/JSBSim/FGAuxiliary.cpp b/src/FDM/JSBSim/FGAuxiliary.cpp index c9da9ed30..c7e29e7b8 100644 --- a/src/FDM/JSBSim/FGAuxiliary.cpp +++ b/src/FDM/JSBSim/FGAuxiliary.cpp @@ -116,9 +116,9 @@ bool FGAuxiliary::Run() // Rotation - double cTht = Propagate->GetCostht(); - double cPhi = Propagate->GetCosphi(); - double sPhi = Propagate->GetSinphi(); + double cTht = Propagate->GetCosEuler(eTht); + double cPhi = Propagate->GetCosEuler(ePhi); + double sPhi = Propagate->GetSinEuler(ePhi); vEulerRates(eTht) = vPQR(eQ)*cPhi - vPQR(eR)*sPhi; if (cTht != 0.0) { @@ -246,7 +246,7 @@ double FGAuxiliary::GetHeadWind(void) psiw = Atmosphere->GetWindPsi(); vw = Atmosphere->GetWindNED().Magnitude(); - return vw*cos(psiw - Propagate->Getpsi()); + return vw*cos(psiw - Propagate->GetEuler(ePsi)); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -258,7 +258,7 @@ double FGAuxiliary::GetCrossWind(void) psiw = Atmosphere->GetWindPsi(); vw = Atmosphere->GetWindNED().Magnitude(); - return vw*sin(psiw - Propagate->Getpsi()); + return vw*sin(psiw - Propagate->GetEuler(ePsi)); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/FGColumnVector3.h b/src/FDM/JSBSim/FGColumnVector3.h index 5b28a9e94..a83afacdd 100644 --- a/src/FDM/JSBSim/FGColumnVector3.h +++ b/src/FDM/JSBSim/FGColumnVector3.h @@ -299,6 +299,15 @@ public: */ double Magnitude(void) const; + /** Length of the vector in a coordinate axis plane. + + Compute and return the euclidean norm of this vector projected into + the coordinate axis plane idx1-idx2. + */ + double Magnitude(int idx1, int idx2) const { + return sqrt( Entry(idx1)*Entry(idx1) + Entry(idx2)*Entry(idx2) ); + } + /** Normalize. Normalize the vector to have the Magnitude() == 1.0. If the vector diff --git a/src/FDM/JSBSim/FGEngine.cpp b/src/FDM/JSBSim/FGEngine.cpp index 1983708ef..edf834ba3 100644 --- a/src/FDM/JSBSim/FGEngine.cpp +++ b/src/FDM/JSBSim/FGEngine.cpp @@ -236,11 +236,11 @@ bool FGEngine::LoadThruster(FGConfigFile* AC_cfg) thrType = Cfg_ptr->GetValue(); if (thrType == "FG_PROPELLER") { - Thruster = new FGPropeller(FDMExec, Cfg_ptr); + Thruster = new FGPropeller(FDMExec, Cfg_ptr, EngineNumber); } else if (thrType == "FG_NOZZLE") { - Thruster = new FGNozzle(FDMExec, Cfg_ptr); + Thruster = new FGNozzle(FDMExec, Cfg_ptr, EngineNumber); } else if (thrType == "FG_DIRECT") { - Thruster = new FGThruster( FDMExec, Cfg_ptr); + Thruster = new FGThruster( FDMExec, Cfg_ptr, EngineNumber); } AC_cfg->GetNextConfigLine(); diff --git a/src/FDM/JSBSim/FGFCS.cpp b/src/FDM/JSBSim/FGFCS.cpp index b6a961a68..cfeb2d571 100644 --- a/src/FDM/JSBSim/FGFCS.cpp +++ b/src/FDM/JSBSim/FGFCS.cpp @@ -100,6 +100,7 @@ FGFCS::~FGFCS() MixturePos.clear(); PropAdvanceCmd.clear(); PropAdvance.clear(); + SteerPosDeg.clear(); unsigned int i; @@ -117,10 +118,17 @@ bool FGFCS::Run(void) if (FGModel::Run()) return true; // fast exit if nothing to do + // Set the default engine commands for (i=0; iGetGearUnit(i); + SteerPosDeg[i] = gear->GetDefaultSteerAngle( GetDsCmd() ); + } + for (i=0; iRun(); // cycle AP components for (i=0; iRun(); // cycle FCS components @@ -473,6 +481,13 @@ void FGFCS::AddThrottle(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +void FGFCS::AddGear(void) +{ + SteerPosDeg.push_back(0.0); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + void FGFCS::Normalize(void) { //not all of these are guaranteed to be defined for every model @@ -533,6 +548,10 @@ void FGFCS::bind(void) &FGFCS::GetDrCmd, &FGFCS::SetDrCmd, true); + PropertyManager->Tie("fcs/steer-cmd-norm", this, + &FGFCS::GetDsCmd, + &FGFCS::SetDsCmd, + true); PropertyManager->Tie("fcs/flap-cmd-norm", this, &FGFCS::GetDfCmd, &FGFCS::SetDfCmd, @@ -783,6 +802,16 @@ void FGFCS::bindModel(void) true ); } } + + for (i=0; iGetGearUnit(i)->GetSteerable()) { + snprintf(tmp,80,"fcs/steer-pos-deg[%u]",i); + PropertyManager->Tie( tmp, this, i, + &FGFCS::GetSteerPosDeg, + &FGFCS::SetSteerPosDeg, + true ); + } + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/FGFCS.h b/src/FDM/JSBSim/FGFCS.h index 0340fd432..43d8822b2 100644 --- a/src/FDM/JSBSim/FGFCS.h +++ b/src/FDM/JSBSim/FGFCS.h @@ -176,6 +176,10 @@ public: @return rudder command in percent */ inline double GetDrCmd(void) const { return DrCmd; } + /** Gets the steering command. + @return steering command in percent */ + inline double GetDsCmd(void) const { return DsCmd; } + /** Gets the flaps command. @return flaps command in percent */ inline double GetDfCmd(void) const { return DfCmd; } @@ -402,6 +406,10 @@ public: @return mixture position for the given engine in percent ( 0 - 100)*/ inline double GetMixturePos(int engine) const { return MixturePos[engine]; } + /** Gets the steering position. + @return steering position in degrees */ + double GetSteerPosDeg(int gear) const { return SteerPosDeg[gear]; } + /** Gets the gear position (0 up, 1 down), defaults to down @return gear position (0 up, 1 down) */ inline double GetGearPos(void) const { return GearPos; } @@ -437,6 +445,10 @@ public: @param cmd rudder command in percent*/ inline void SetDrCmd(double cmd) { DrCmd = cmd; } + /** Sets the steering command + @param cmd steering command in percent*/ + inline void SetDsCmd(double cmd) { DsCmd = cmd; } + /** Sets the flaps command @param cmd flaps command in percent*/ inline void SetDfCmd(double cmd) { DfCmd = cmd; } @@ -547,6 +559,10 @@ public: @param cmd mixture setting in percent (0 - 100)*/ void SetMixturePos(int engine, double cmd); + /** Sets the steering position + @param cmd steering position in degrees*/ + void SetSteerPosDeg(int gear, double pos) { SteerPosDeg[gear] = pos; } + /** Set the gear extend/retract position, defaults to down @param gear position 0 up, 1 down */ void SetGearPos(double gearpos) { GearPos = gearpos; } @@ -587,6 +603,7 @@ public: bool Load(FGConfigFile* AC_cfg); void AddThrottle(void); + void AddGear(void); FGPropertyManager* GetPropertyManager(void) { return PropertyManager; } @@ -595,7 +612,7 @@ public: void unbind(FGPropertyManager *node); private: - double DaCmd, DeCmd, DrCmd, DfCmd, DsbCmd, DspCmd; + double DaCmd, DeCmd, DrCmd, DsCmd, DfCmd, DsbCmd, DspCmd; double AP_DaCmd, AP_DeCmd, AP_DrCmd, AP_ThrottleCmd; double DePos[NForms], DaLPos[NForms], DaRPos[NForms], DrPos[NForms]; double DfPos[NForms], DsbPos[NForms], DspPos[NForms]; @@ -606,6 +623,7 @@ private: vector MixturePos; vector PropAdvanceCmd; vector PropAdvance; + vector SteerPosDeg; double LeftBrake, RightBrake, CenterBrake; // Brake settings double GearCmd,GearPos; diff --git a/src/FDM/JSBSim/FGGroundReactions.cpp b/src/FDM/JSBSim/FGGroundReactions.cpp index 57dc4c2ff..abd19c88d 100644 --- a/src/FDM/JSBSim/FGGroundReactions.cpp +++ b/src/FDM/JSBSim/FGGroundReactions.cpp @@ -110,7 +110,9 @@ bool FGGroundReactions::Load(FGConfigFile* AC_cfg) AC_cfg->GetNextConfigLine(); while ((token = AC_cfg->GetValue()) != string("/UNDERCARRIAGE")) { - lGear.push_back(FGLGear(AC_cfg, FDMExec)); + int num = lGear.size(); + lGear.push_back(FGLGear(AC_cfg, FDMExec, num)); + FCS->AddGear(); } return true; diff --git a/src/FDM/JSBSim/FGLGear.cpp b/src/FDM/JSBSim/FGLGear.cpp index 6c9d6b209..22836a86d 100644 --- a/src/FDM/JSBSim/FGLGear.cpp +++ b/src/FDM/JSBSim/FGLGear.cpp @@ -57,10 +57,12 @@ static const char *IdHdr = ID_LGEAR; 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 @@ -140,6 +142,8 @@ FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex) FGLGear::FGLGear(const FGLGear& lgear) { + GearNumber = lgear.GearNumber; + State = lgear.State; Aircraft = lgear.Aircraft; Propagate = lgear.Propagate; @@ -207,7 +211,6 @@ FGLGear::~FGLGear() FGColumnVector3& FGLGear::Force(void) { double SinWheel, CosWheel; - double deltaSlip; double deltaT = State->Getdt()*Aircraft->GetRate(); vForce.InitMatrix(); @@ -229,6 +232,24 @@ FGColumnVector3& FGLGear::Force(void) 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 = MassBalance->StructuralToBody(vXYZ); @@ -323,28 +344,12 @@ FGColumnVector3& FGLGear::Force(void) 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(Propagate->Getpsi() + SteerAngle); - CosWheel = cos(Propagate->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; @@ -447,6 +452,9 @@ FGColumnVector3& FGLGear::Force(void) WOW = false; + // 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; diff --git a/src/FDM/JSBSim/FGLGear.h b/src/FDM/JSBSim/FGLGear.h index 6bf8778f4..842b8077d 100644 --- a/src/FDM/JSBSim/FGLGear.h +++ b/src/FDM/JSBSim/FGLGear.h @@ -183,7 +183,7 @@ public: /** Constructor @param Executive a pointer to the parent executive object @param File a pointer to the config file instance */ - FGLGear(FGConfigFile* File, FGFDMExec* Executive); + FGLGear(FGConfigFile* File, FGFDMExec* Executive, int number); /** Constructor @param lgear a reference to an existing FGLGear object */ FGLGear(const FGLGear& lgear); @@ -229,12 +229,14 @@ public: /** Get the console touchdown reporting feature @return true if reporting is turned on */ inline bool GetReport(void) { return ReportEnable; } - inline double GetSteerAngle(void) { return SteerAngle;} - inline double GetstaticFCoeff(void) { return staticFCoeff;} + double GetSteerNorm(void) const { return radtodeg/maxSteerAngle*SteerAngle; } + double GetDefaultSteerAngle(double cmd) const { return cmd*maxSteerAngle; } + double GetstaticFCoeff(void) { return staticFCoeff; } inline int GetBrakeGroup(void) { return (int)eBrakeGrp; } inline int GetSteerType(void) { return (int)eSteerType; } + bool GetSteerable(void) const { return eSteerType != stFixed; } inline bool GetRetractable(void) { return isRetractable; } inline bool GetGearUnitUp(void) { return GearUp; } inline bool GetGearUnitDown(void) { return GearDown; } @@ -246,6 +248,7 @@ public: double GetWheelVel(int axis) { return vWhlVelVec(axis);} private: + int GearNumber; FGColumnVector3 vXYZ; FGColumnVector3 vMoment; FGColumnVector3 vWhlBodyVec; diff --git a/src/FDM/JSBSim/FGLocation.cpp b/src/FDM/JSBSim/FGLocation.cpp index 758ce3a80..83484c0de 100644 --- a/src/FDM/JSBSim/FGLocation.cpp +++ b/src/FDM/JSBSim/FGLocation.cpp @@ -82,15 +82,20 @@ FGLocation::FGLocation(double lon, double lat, double radius) void FGLocation::SetLongitude(double longitude) { - double rtmp = sqrt(mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY)); + double rtmp = mECLoc.Magnitude(eX, eY); + // Check if we have zero radius. + // If so set it to 1, so that we can set a position + if (0.0 == mECLoc.Magnitude()) + rtmp = 1.0; + // Fast return if we are on the north or south pole ... if (rtmp == 0.0) return; mCacheValid = false; - mECLoc(eX) = rtmp*sin(longitude); - mECLoc(eY) = rtmp*cos(longitude); + mECLoc(eX) = rtmp*cos(longitude); + mECLoc(eY) = rtmp*sin(longitude); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -105,7 +110,7 @@ void FGLocation::SetLatitude(double latitude) r = 1.0; } - double rtmp = sqrt(mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY)); + double rtmp = mECLoc.Magnitude(eX, eY); if (rtmp != 0.0) { double fac = r/rtmp*cos(latitude); mECLoc(eX) *= fac; diff --git a/src/FDM/JSBSim/FGNozzle.cpp b/src/FDM/JSBSim/FGNozzle.cpp index fa890f2ee..1c66c476c 100644 --- a/src/FDM/JSBSim/FGNozzle.cpp +++ b/src/FDM/JSBSim/FGNozzle.cpp @@ -50,7 +50,7 @@ CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -FGNozzle::FGNozzle(FGFDMExec* FDMExec, FGConfigFile* Nzl_cfg) : FGThruster(FDMExec) +FGNozzle::FGNozzle(FGFDMExec* FDMExec, FGConfigFile* Nzl_cfg, int num) : FGThruster(FDMExec) { string token; @@ -71,6 +71,10 @@ FGNozzle::FGNozzle(FGFDMExec* FDMExec, FGConfigFile* Nzl_cfg) : FGThruster(FDMEx Area2 = (Diameter*Diameter/4.0)*M_PI; AreaT = Area2/ExpR; + char property_name[80]; + snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum); + PropertyManager->Tie( property_name, &ThrustCoeff ); + Debug(0); } @@ -78,6 +82,10 @@ FGNozzle::FGNozzle(FGFDMExec* FDMExec, FGConfigFile* Nzl_cfg) : FGThruster(FDMEx FGNozzle::~FGNozzle() { + char property_name[80]; + snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum); + PropertyManager->Untie( property_name ); + Debug(1); } @@ -89,6 +97,8 @@ double FGNozzle::Calculate(double CfPc) Thrust = max((double)0.0, (CfPc * AreaT + (PE - pAtm)*Area2) * nzlEff); vFn(1) = Thrust * cos(ReverserAngle); + ThrustCoeff = CfPc / ((pAtm - PE) * Area2); + return Thrust; } diff --git a/src/FDM/JSBSim/FGNozzle.h b/src/FDM/JSBSim/FGNozzle.h index 8ee26dff6..5c9e76c18 100644 --- a/src/FDM/JSBSim/FGNozzle.h +++ b/src/FDM/JSBSim/FGNozzle.h @@ -69,7 +69,7 @@ class FGNozzle : public FGThruster { public: /// Constructor - FGNozzle(FGFDMExec* exec, FGConfigFile* AC_cfg); + FGNozzle(FGFDMExec* exec, FGConfigFile* AC_cfg, int num = 0); /// Destructor ~FGNozzle(); diff --git a/src/FDM/JSBSim/FGOutput.cpp b/src/FDM/JSBSim/FGOutput.cpp index 794f8c489..dc7730d66 100644 --- a/src/FDM/JSBSim/FGOutput.cpp +++ b/src/FDM/JSBSim/FGOutput.cpp @@ -381,9 +381,9 @@ void FGOutput::SocketOutput(void) socket->Clear(); socket->Append(State->Getsim_time()); socket->Append(Propagate->Geth()); - socket->Append(Propagate->Getphi()); - socket->Append(Propagate->Gettht()); - socket->Append(Propagate->Getpsi()); + socket->Append(Propagate->GetEuler(ePhi)); + socket->Append(Propagate->GetEuler(eTht)); + socket->Append(Propagate->GetEuler(ePsi)); socket->Append(Atmosphere->GetDensity()); socket->Append(Auxiliary->GetVt()); socket->Append(Propagate->GetUVW(eU)); diff --git a/src/FDM/JSBSim/FGPropagate.cpp b/src/FDM/JSBSim/FGPropagate.cpp index e97184031..57f489b5b 100644 --- a/src/FDM/JSBSim/FGPropagate.cpp +++ b/src/FDM/JSBSim/FGPropagate.cpp @@ -152,6 +152,9 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) // Compute some derived values. vVel = VState.vQtrn.GetTInv()*VState.vUVW; + + // Finaly make shure that the quaternion stays normalized. + VState.vQtrn.Normalize(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -289,13 +292,13 @@ void FGPropagate::bind(void) PropertyManager->Tie("metrics/runway-radius", this, &FGPropagate::GetRunwayRadius, &FGPropagate::SetRunwayRadius); - PropertyManager->Tie("attitude/phi-rad", this, &FGPropagate::Getphi); - PropertyManager->Tie("attitude/theta-rad", this, &FGPropagate::Gettht); - PropertyManager->Tie("attitude/psi-rad", this, &FGPropagate::Getpsi); + PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler); + PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler); + PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler); - PropertyManager->Tie("attitude/roll-rad", this, &FGPropagate::Getphi); - PropertyManager->Tie("attitude/pitch-rad", this, &FGPropagate::Gettht); - PropertyManager->Tie("attitude/heading-true-rad", this, &FGPropagate::Getpsi); + PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler); + PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler); + PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/FGPropagate.h b/src/FDM/JSBSim/FGPropagate.h index 420876774..d4eb21633 100644 --- a/src/FDM/JSBSim/FGPropagate.h +++ b/src/FDM/JSBSim/FGPropagate.h @@ -106,7 +106,9 @@ public: double Geth(void) const { return VState.vLocation.GetRadius() - SeaLevelRadius; } double GetPQR(int axis) const {return VState.vPQR(axis);} double GetPQRdot(int idx) const {return vPQRdot(idx);} - double GetEuler(int axis) const { return VState.vQtrn.GetEuler()(axis); } + double GetEuler(int axis) const { return VState.vQtrn.GetEuler(axis); } + double GetCosEuler(int idx) const { return VState.vQtrn.GetCosEuler(idx); } + double GetSinEuler(int idx) const { return VState.vQtrn.GetSinEuler(idx); } double Gethdot(void) const { return -vVel(eDown); } /** Returns the "constant" RunwayRadius. @@ -122,18 +124,6 @@ public: double GetLatitude(void) const { return VState.vLocation.GetLatitude(); } const FGLocation& GetLocation(void) const { return VState.vLocation; } - double Getphi(void) const { return VState.vQtrn.GetEulerPhi(); } - double Gettht(void) const { return VState.vQtrn.GetEulerTheta(); } - double Getpsi(void) const { return VState.vQtrn.GetEulerPsi(); } - - double GetCosphi(void) const { return VState.vQtrn.GetCosEulerPhi(); } - double GetCostht(void) const { return VState.vQtrn.GetCosEulerTheta(); } - double GetCospsi(void) const { return VState.vQtrn.GetCosEulerPsi(); } - - double GetSinphi(void) const { return VState.vQtrn.GetSinEulerPhi(); } - double GetSintht(void) const { return VState.vQtrn.GetSinEulerTheta(); } - double GetSinpsi(void) const { return VState.vQtrn.GetSinEulerPsi(); } - /** Retrieves the local-to-body transformation matrix. @return a reference to the local-to-body transformation matrix. */ const FGMatrix33& GetTl2b(void) const { return VState.vQtrn.GetT(); } diff --git a/src/FDM/JSBSim/FGPropeller.cpp b/src/FDM/JSBSim/FGPropeller.cpp index 9b352bc55..2832498ca 100644 --- a/src/FDM/JSBSim/FGPropeller.cpp +++ b/src/FDM/JSBSim/FGPropeller.cpp @@ -56,7 +56,7 @@ CLASS IMPLEMENTATION // not just the X-axis of the engine/propeller. This may or may not work for a // helicopter. -FGPropeller::FGPropeller(FGFDMExec* exec, FGConfigFile* Prop_cfg) : FGThruster(exec) +FGPropeller::FGPropeller(FGFDMExec* exec, FGConfigFile* Prop_cfg, int num) : FGThruster(exec) { string token; int rows, cols; @@ -107,6 +107,10 @@ FGPropeller::FGPropeller(FGFDMExec* exec, FGConfigFile* Prop_cfg) : FGThruster(e RPM = 0; vTorque.InitMatrix(); + char property_name[80]; + snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum); + PropertyManager->Tie( property_name, &ThrustCoeff ); + Debug(0); } @@ -116,6 +120,11 @@ FGPropeller::~FGPropeller() { if (cThrust) delete cThrust; if (cPower) delete cPower; + + char property_name[80]; + snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum); + PropertyManager->Untie( property_name ); + Debug(1); } @@ -135,7 +144,7 @@ FGPropeller::~FGPropeller() double FGPropeller::Calculate(double PowerAvailable) { - double J, C_Thrust, omega; + double J, omega; double Vel = fdmex->GetAuxiliary()->GetAeroUVW(eU); double rho = fdmex->GetAtmosphere()->GetDensity(); double RPS = RPM/60.0; @@ -148,9 +157,9 @@ double FGPropeller::Calculate(double PowerAvailable) } if (MaxPitch == MinPitch) { // Fixed pitch prop - C_Thrust = cThrust->GetValue(J); + ThrustCoeff = cThrust->GetValue(J); } else { // Variable pitch prop - C_Thrust = cThrust->GetValue(J, Pitch); + ThrustCoeff = cThrust->GetValue(J, Pitch); } if (P_Factor > 0.0001) { @@ -162,7 +171,7 @@ double FGPropeller::Calculate(double PowerAvailable) cerr << "P-Factor value in config file must be greater than zero" << endl; } - Thrust = C_Thrust*RPS*RPS*Diameter*Diameter*Diameter*Diameter*rho; + Thrust = ThrustCoeff*RPS*RPS*Diameter*Diameter*Diameter*Diameter*rho; omega = RPS*2.0*M_PI; vFn(1) = Thrust; diff --git a/src/FDM/JSBSim/FGPropeller.h b/src/FDM/JSBSim/FGPropeller.h index cc026049c..0c8ba9f17 100644 --- a/src/FDM/JSBSim/FGPropeller.h +++ b/src/FDM/JSBSim/FGPropeller.h @@ -89,7 +89,7 @@ public: /** Constructor for FGPropeller. @param exec a pointer to the main executive object @param AC_cfg a pointer to the main aircraft config file object */ - FGPropeller(FGFDMExec* exec, FGConfigFile* AC_cfg); + FGPropeller(FGFDMExec* exec, FGConfigFile* AC_cfg, int num = 0); /// Destructor for FGPropeller - deletes the FGTable objects ~FGPropeller(); diff --git a/src/FDM/JSBSim/FGQuaternion.cpp b/src/FDM/JSBSim/FGQuaternion.cpp index 8c62dd500..55edcf88e 100644 --- a/src/FDM/JSBSim/FGQuaternion.cpp +++ b/src/FDM/JSBSim/FGQuaternion.cpp @@ -130,12 +130,17 @@ FGQuaternion::FGQuaternion(double phi, double tht, double psi) angular velocities PQR. */ FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR) const { + double norm = Magnitude(); + if (norm == 0.0) + return FGQuaternion::zero(); + double rnorm = 1.0/norm; + FGQuaternion QDot; QDot(1) = -0.5*(Entry(2)*PQR(eP) + Entry(3)*PQR(eQ) + Entry(4)*PQR(eR)); QDot(2) = 0.5*(Entry(1)*PQR(eP) + Entry(3)*PQR(eR) - Entry(4)*PQR(eQ)); QDot(3) = 0.5*(Entry(1)*PQR(eQ) + Entry(4)*PQR(eP) - Entry(2)*PQR(eR)); QDot(4) = 0.5*(Entry(1)*PQR(eR) + Entry(2)*PQR(eQ) - Entry(3)*PQR(eP)); - return QDot; + return rnorm*QDot; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/FGQuaternion.h b/src/FDM/JSBSim/FGQuaternion.h index 7f5db7557..2800a1549 100644 --- a/src/FDM/JSBSim/FGQuaternion.h +++ b/src/FDM/JSBSim/FGQuaternion.h @@ -110,6 +110,39 @@ public: @param psi The euler Z axis (heading) angle in radians */ FGQuaternion(double phi, double tht, double psi); + /** Initializer by one euler angle. + Initialize the quaternion with the single euler angle where its index + is given in the first argument. + @param idx Index of the euler angle to initialize + @param angle The euler angle in radians */ + FGQuaternion(int idx, double angle) + : mCacheValid(false) { + double angle2 = 0.5*angle; + + double Sangle2 = sin(angle2); + double Cangle2 = cos(angle2); + + if (idx == ePhi) { + Entry(1) = Cangle2; + Entry(2) = Sangle2; + Entry(3) = 0.0; + Entry(4) = 0.0; + + } else if (idx == eTht) { + Entry(1) = Cangle2; + Entry(2) = 0.0; + Entry(3) = Sangle2; + Entry(4) = 0.0; + + } else { + Entry(1) = Cangle2; + Entry(2) = 0.0; + Entry(3) = 0.0; + Entry(4) = Sangle2; + + } + } + /// Destructor. ~FGQuaternion() {} @@ -123,171 +156,106 @@ public: /** Transformation matrix. @return a reference to the transformation/rotation matrix corresponding to this quaternion rotation. */ - const FGMatrix33& GetT() const { ComputeDerived(); return mT; } + const FGMatrix33& GetT(void) const { ComputeDerived(); return mT; } /** Backward transformation matrix. @return a reference to the inverse transformation/rotation matrix corresponding to this quaternion rotation. */ - const FGMatrix33& GetTInv() const { ComputeDerived(); return mTInv; } + const FGMatrix33& GetTInv(void) const { ComputeDerived(); return mTInv; } /** Retrieves the Euler angles. @return a reference to the triad of euler angles corresponding to this quaternion rotation. @units radians */ - const FGColumnVector3& GetEuler() const { + const FGColumnVector3& GetEuler(void) const { ComputeDerived(); return mEulerAngles; } - /** Euler angle theta. - @return the euler angle theta (pitch attitude) corresponding to this - quaternion rotation. - @units radians */ - double GetEulerTheta() const { + /** Retrieves the Euler angles. + @param i the euler angle index. + @return a reference to the i-th euler angles corresponding + to this quaternion rotation. + @units radians */ + double GetEuler(int i) const { ComputeDerived(); - return mEulerAngles(eTht); + return mEulerAngles(i); } - /** Euler angle theta. - @return the euler angle theta (pitch attitude) corresponding to - this quaternion rotation. - @units degrees */ - double GetEulerThetaDeg() const { + /** Retrieves the Euler angles. + @param i the euler angle index. + @return a reference to the i-th euler angles corresponding + to this quaternion rotation. + @units degrees */ + double GetEulerDeg(int i) const { ComputeDerived(); - return radtodeg*mEulerAngles(eTht); + return radtodeg*mEulerAngles(i); } - /** Euler angle psi. - @return the heading euler angle (psi) corresponding to this quaternion - rotation. - @units radians */ - double GetEulerPsi() const { - ComputeDerived(); - return mEulerAngles(ePsi); - } - - /** Retrieves the heading angle. - @return the Euler angle psi (heading) corresponding to this quaternion - rotation. - @units degrees */ - double GetEulerPsiDeg() const { - ComputeDerived(); - return radtodeg*mEulerAngles(ePsi); - } - - /** Retrieves the roll angle. - @return the euler angle phi (roll) corresponding to this quaternion - rotation. - @units radians */ - double GetEulerPhi() const { - ComputeDerived(); - return mEulerAngles(ePhi); - } - - /** Retrieves the roll angle. - Returns the Euler angle phi (roll) corresponding to this quaternion rotation. - @units degrees */ - double GetEulerPhiDeg() const { - ComputeDerived(); - return radtodeg*mEulerAngles(ePhi); - } - - /** Retrieves sine theta. + /** Retrieves sine of the given euler angle. @return the sine of the Euler angle theta (pitch attitude) corresponding to this quaternion rotation. */ - double GetSinEulerTheta() const { + double GetSinEuler(int i) const { ComputeDerived(); - return mEulerSines(eTht); + return mEulerSines(i); } - /** Retrieves sine psi. - @return the sine of the Euler angle psi (heading) corresponding to this - quaternion rotation. */ - double GetSinEulerPsi() const { + /** Retrieves cosine of the given euler angle. + @return the sine of the Euler angle theta (pitch attitude) corresponding + to this quaternion rotation. */ + double GetCosEuler(int i) const { ComputeDerived(); - return mEulerSines(ePsi); - } - - /** Sine of euler angle phi. - @return the sine of the Euler angle phi (roll) corresponding to this - quaternion rotation. */ - double GetSinEulerPhi() const { - ComputeDerived(); - return mEulerSines(ePhi); - } - - /** Cosine of euler angle theta. - @return the cosine of the Euler angle theta (pitch) corresponding to this - quaternion rotation. */ - double GetCosEulerTheta() const { - ComputeDerived(); - return mEulerCosines(eTht); - } - - /** Cosine of euler angle psi. - @return the cosine of the Euler angle psi (heading) corresponding to this - quaternion rotation. */ - double GetCosEulerPsi() const { - ComputeDerived(); - return mEulerCosines(ePsi); - } - - /** Cosine of euler angle phi. - @return the cosine of the Euler angle phi (roll) corresponding to this - quaternion rotation. */ - double GetCosEulerPhi() const { - ComputeDerived(); - return mEulerCosines(ePhi); + return mEulerCosines(i); } /** Read access the entries of the vector. - + @param idx the component index. - + Return the value of the matrix entry at the given index. Indices are counted starting with 1. - + Note that the index given in the argument is unchecked. */ double operator()(unsigned int idx) const { return Entry(idx); } /** Write access the entries of the vector. - + @param idx the component index. - + Return a reference to the vector entry at the given index. Indices are counted starting with 1. - + Note that the index given in the argument is unchecked. */ double& operator()(unsigned int idx) { return Entry(idx); } /** Read access the entries of the vector. - + @param idx the component index. - + Return the value of the matrix entry at the given index. Indices are counted starting with 1. - + This function is just a shortcut for the @ref double operator()(unsigned int idx) const function. It is used internally to access the elements in a more convenient way. - + Note that the index given in the argument is unchecked. */ double Entry(unsigned int idx) const { return mData[idx-1]; } /** Write access the entries of the vector. - + @param idx the component index. - + Return a reference to the vector entry at the given index. Indices are counted starting with 1. - + This function is just a shortcut for the @ref double& operator()(unsigned int idx) function. It is used internally to access the elements in a more convenient way. - + Note that the index given in the argument is unchecked. */ double& Entry(unsigned int idx) { mCacheValid = false; return mData[idx-1]; } @@ -413,29 +381,53 @@ public: return *this; } + /** Inverse of the quaternion. + + Compute and return the inverse of the quaternion so that the orientation + represented with *this multiplied with the returned value is equal to + the identity orientation. + */ + FGQuaternion Inverse(void) const { + double norm = Magnitude(); + if (norm == 0.0) + return *this; + double rNorm = 1.0/norm; + return FGQuaternion( Entry(1)*rNorm, -Entry(2)*rNorm, + -Entry(3)*rNorm, -Entry(4)*rNorm ); + } + + /** Conjugate of the quaternion. + + Compute and return the conjugate of the quaternion. This one is equal + to the inverse iff the quaternion is normalized. + */ + FGQuaternion Conjugate(void) const { + return FGQuaternion( Entry(1), -Entry(2), -Entry(3), -Entry(4) ); + } + friend FGQuaternion operator*(double, const FGQuaternion&); - + /** Length of the vector. - + Compute and return the euclidean norm of this vector. */ - double Magnitude() const { return sqrt(SqrMagnitude()); } + double Magnitude(void) const { return sqrt(SqrMagnitude()); } /** Square of the length of the vector. - + Compute and return the square of the euclidean norm of this vector. */ - double SqrMagnitude() const { + double SqrMagnitude(void) const { return Entry(1)*Entry(1)+Entry(2)*Entry(2) +Entry(3)*Entry(3)+Entry(4)*Entry(4); } /** Normialze. - + Normalize the vector to have the Magnitude() == 1.0. If the vector is equal to zero it is left untouched. */ - void Normalize(); + void Normalize(void); /** Zero quaternion vector. Does not represent any orientation. Useful for initialization of increments */ @@ -454,7 +446,10 @@ private: /** Computation of derived values. This function checks if the derived values like euler angles and transformation matrices are already computed. If so, it - returns. If they need to be computed this is done here. */ + returns. If they need to be computed the real worker routine + \ref FGQuaternion::ComputeDerivedUnconditional(void) const + is called. + This function is inlined to avoid function calls in the fast path. */ void ComputeDerived(void) const { if (!mCacheValid) ComputeDerivedUnconditional(); diff --git a/src/FDM/JSBSim/FGThruster.cpp b/src/FDM/JSBSim/FGThruster.cpp index a03df18f4..2573daa9c 100644 --- a/src/FDM/JSBSim/FGThruster.cpp +++ b/src/FDM/JSBSim/FGThruster.cpp @@ -54,18 +54,30 @@ FGThruster::FGThruster(FGFDMExec *FDMExec) : FGForce(FDMExec) Type = ttDirect; SetTransformType(FGForce::tCustom); + EngineNum = 0; + PropertyManager = FDMExec->GetPropertyManager(); + Debug(0); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FGThruster::FGThruster(FGFDMExec *FDMExec, - FGConfigFile *Eng_cfg ): FGForce(FDMExec) + FGConfigFile *Eng_cfg, int num ): FGForce(FDMExec) { Type = ttDirect; SetTransformType(FGForce::tCustom); Name = Eng_cfg->GetValue(); GearRatio = 1.0; + + EngineNum = num; + ThrustCoeff = 0.0; + PropertyManager = FDMExec->GetPropertyManager(); + + char property_name[80]; + snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum); + PropertyManager->Tie( property_name, &ThrustCoeff ); + Debug(0); } @@ -73,6 +85,10 @@ FGThruster::FGThruster(FGFDMExec *FDMExec, FGThruster::~FGThruster() { + char property_name[80]; + snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum); + PropertyManager->Untie( property_name ); + Debug(1); } diff --git a/src/FDM/JSBSim/FGThruster.h b/src/FDM/JSBSim/FGThruster.h index b9bef6d3e..23dd31fc2 100644 --- a/src/FDM/JSBSim/FGThruster.h +++ b/src/FDM/JSBSim/FGThruster.h @@ -40,6 +40,7 @@ INCLUDES #include "FGForce.h" #include "FGConfigFile.h" +#include "FGPropertyManager.h" #include /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -72,13 +73,16 @@ class FGThruster : public FGForce { public: /// Constructor FGThruster(FGFDMExec *FDMExec); - FGThruster(FGFDMExec *FDMExec, FGConfigFile *Eng_cfg ); + FGThruster(FGFDMExec *FDMExec, FGConfigFile *Eng_cfg, int num ); /// Destructor virtual ~FGThruster(); enum eType {ttNozzle, ttRotor, ttPropeller, ttDirect}; - virtual double Calculate(double tt) { Thrust = tt; vFn(1) = Thrust; return 0.0; } + virtual double Calculate(double tt) { + Thrust = tt; vFn(1) = Thrust; + return 0.0; + } void SetName(string name) {Name = name;} virtual void SetRPM(double rpm) {}; virtual double GetPowerRequired(void) {return 0.0;} @@ -98,6 +102,9 @@ protected: double PowerRequired; double deltaT; double GearRatio; + double ThrustCoeff; + int EngineNum; + FGPropertyManager* PropertyManager; virtual void Debug(int from); }; } diff --git a/src/FDM/JSBSim/FGTrimAxis.cpp b/src/FDM/JSBSim/FGTrimAxis.cpp index 02ee8a0d5..27a82eadf 100644 --- a/src/FDM/JSBSim/FGTrimAxis.cpp +++ b/src/FDM/JSBSim/FGTrimAxis.cpp @@ -124,13 +124,13 @@ FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, State st, solver_eps=tolerance/100; break; case tTheta: - control_min=fdmex->GetPropagate()->Gettht() - 5*degtorad; - control_max=fdmex->GetPropagate()->Gettht() + 5*degtorad; + control_min=fdmex->GetPropagate()->GetEuler(eTht) - 5*degtorad; + control_max=fdmex->GetPropagate()->GetEuler(eTht) + 5*degtorad; state_convert=radtodeg; break; case tPhi: - control_min=fdmex->GetPropagate()->Getphi() - 30*degtorad; - control_max=fdmex->GetPropagate()->Getphi() + 30*degtorad; + control_min=fdmex->GetPropagate()->GetEuler(ePhi) - 30*degtorad; + control_max=fdmex->GetPropagate()->GetEuler(ePhi) + 30*degtorad; state_convert=radtodeg; control_convert=radtodeg; break; @@ -141,8 +141,8 @@ FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, State st, control_convert=radtodeg; break; case tHeading: - control_min=fdmex->GetPropagate()->Getpsi() - 30*degtorad; - control_max=fdmex->GetPropagate()->Getpsi() + 30*degtorad; + control_min=fdmex->GetPropagate()->GetEuler(ePsi) - 30*degtorad; + control_max=fdmex->GetPropagate()->GetEuler(ePsi) + 30*degtorad; state_convert=radtodeg; break; } @@ -190,10 +190,10 @@ void FGTrimAxis::getControl(void) { case tYawTrim: case tRudder: control_value=fdmex->GetFCS() -> GetDrCmd(); break; case tAltAGL: control_value=fdmex->GetPropagate()->GetDistanceAGL();break; - case tTheta: control_value=fdmex->GetPropagate()->Gettht(); break; - case tPhi: control_value=fdmex->GetPropagate()->Getphi(); break; + case tTheta: control_value=fdmex->GetPropagate()->GetEuler(eTht); break; + case tPhi: control_value=fdmex->GetPropagate()->GetEuler(ePhi); break; case tGamma: control_value=fdmex->GetAuxiliary()->GetGamma();break; - case tHeading: control_value=fdmex->GetPropagate()->Getpsi(); break; + case tHeading: control_value=fdmex->GetPropagate()->GetEuler(ePsi); break; } } @@ -202,7 +202,7 @@ void FGTrimAxis::getControl(void) { double FGTrimAxis::computeHmgt(void) { double diff; - diff = fdmex->GetPropagate()->Getpsi() - + diff = fdmex->GetPropagate()->GetEuler(ePsi) - fdmex->GetAuxiliary()->GetGroundTrack(); if( diff < -M_PI ) { @@ -270,8 +270,8 @@ void FGTrimAxis::SetThetaOnGround(double ff) { } cout << "SetThetaOnGround ref gear: " << ref << endl; if(ref >= 0) { - double sp = fdmex->GetPropagate()->GetSinphi(); - double cp = fdmex->GetPropagate()->GetCosphi(); + double sp = fdmex->GetPropagate()->GetSinEuler(ePhi); + double cp = fdmex->GetPropagate()->GetCosEuler(ePhi); double lx = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(1); double ly = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(2); double lz = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(3); @@ -344,7 +344,7 @@ bool FGTrimAxis::initTheta(void) { } //cout << i << endl; if (debug_lvl > 0) { - cout << " Initial Theta: " << fdmex->GetPropagate()->Gettht()*radtodeg << endl; + cout << " Initial Theta: " << fdmex->GetPropagate()->GetEuler(eTht)*radtodeg << endl; cout << " Used gear unit " << iAft << " as aft and " << iForward << " as forward" << endl; } control_min=(theta+5)*degtorad; @@ -370,8 +370,8 @@ void FGTrimAxis::SetPhiOnGround(double ff) { i++; } if (ref >= 0) { - double st = sin(fdmex->GetPropagate()->Gettht()); - double ct = cos(fdmex->GetPropagate()->Gettht()); + double st = fdmex->GetPropagate()->GetSinEuler(eTht); + double ct = fdmex->GetPropagate()->GetCosEuler(eTht); double lx = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(1); double ly = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(2); double lz = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(3); @@ -431,11 +431,16 @@ void FGTrimAxis::setThrottlesPct(void) { void FGTrimAxis::AxisReport(void) { char out[80]; - sprintf(out," %20s: %6.2f %5s: %9.2e Tolerance: %3.0e\n", + + sprintf(out," %20s: %6.2f %5s: %9.2e Tolerance: %3.0e", GetControlName().c_str(), GetControl()*control_convert, GetStateName().c_str(), GetState()+state_target, GetTolerance()); cout << out; + if( abs(GetState()+state_target) < abs(GetTolerance()) ) + cout << " Passed" << endl; + else + cout << " Failed" << endl; } /*****************************************************************************/ diff --git a/src/FDM/JSBSim/FGTurbine.cpp b/src/FDM/JSBSim/FGTurbine.cpp index 69afe4778..51e8a3911 100644 --- a/src/FDM/JSBSim/FGTurbine.cpp +++ b/src/FDM/JSBSim/FGTurbine.cpp @@ -396,6 +396,7 @@ bool FGTurbine::Load(FGConfigFile *Eng_cfg) ThrustTables.back()->Load(Eng_cfg); } else cerr << "Unhandled token in Engine config file: " << token << endl; + if (token == "EOF") return false; } // Pre-calculations and initializations diff --git a/src/FDM/JSBSim/JSBSim.cxx b/src/FDM/JSBSim/JSBSim.cxx index ff1f163c0..cf9e68372 100644 --- a/src/FDM/JSBSim/JSBSim.cxx +++ b/src/FDM/JSBSim/JSBSim.cxx @@ -314,11 +314,11 @@ void FGJSBsim::init() stall_warning->setDoubleValue(0); SG_LOG( SG_FLIGHT, SG_INFO, " Bank Angle: " - << Propagate->Getphi()*RADTODEG << " deg" ); + << Propagate->GetEuler(ePhi)*RADTODEG << " deg" ); SG_LOG( SG_FLIGHT, SG_INFO, " Pitch Angle: " - << Propagate->Gettht()*RADTODEG << " deg" ); + << Propagate->GetEuler(eTht)*RADTODEG << " deg" ); SG_LOG( SG_FLIGHT, SG_INFO, " True Heading: " - << Propagate->Getpsi()*RADTODEG << " deg" ); + << Propagate->GetEuler(ePsi)*RADTODEG << " deg" ); SG_LOG( SG_FLIGHT, SG_INFO, " Latitude: " << Propagate->GetLocation().GetLatitudeDeg() << " deg" ); SG_LOG( SG_FLIGHT, SG_INFO, " Longitude: " @@ -415,7 +415,10 @@ bool FGJSBsim::copy_to_JSBsim() FCS->SetPitchTrimCmd( globals->get_controls()->get_elevator_trim() ); FCS->SetDrCmd( -globals->get_controls()->get_rudder() ); FCS->SetYawTrimCmd( -globals->get_controls()->get_rudder_trim() ); - FCS->SetDfCmd( globals->get_controls()->get_flaps() ); + // FIXME: make that get_steering work +// FCS->SetDsCmd( globals->get_controls()->get_steering()/80.0 ); + FCS->SetDsCmd( globals->get_controls()->get_rudder() ); + FCS->SetDfCmd( globals->get_controls()->get_flaps() ); FCS->SetDsbCmd( globals->get_controls()->get_speedbrake() ); FCS->SetDspCmd( globals->get_controls()->get_spoilers() ); @@ -576,9 +579,9 @@ bool FGJSBsim::copy_from_JSBsim() _set_Altitude_AGL( Propagate->GetDistanceAGL() ); - _set_Euler_Angles( Propagate->Getphi(), - Propagate->Gettht(), - Propagate->Getpsi() ); + _set_Euler_Angles( Propagate->GetEuler(ePhi), + Propagate->GetEuler(eTht), + Propagate->GetEuler(ePsi) ); _set_Alpha( Auxiliary->Getalpha() ); _set_Beta( Auxiliary->Getbeta() ); @@ -914,17 +917,17 @@ void FGJSBsim::init_gear(void ) FGGroundReactions* gr=fdmex->GetGroundReactions(); int Ngear=GroundReactions->GetNumGearUnits(); for (int i=0;iGetGearUnit(i); SGPropertyNode * node = fgGetNode("gear/gear", i, true); - node->setDoubleValue("xoffset-in", - gr->GetGearUnit(i)->GetBodyLocation()(1)); - node->setDoubleValue("yoffset-in", - gr->GetGearUnit(i)->GetBodyLocation()(2)); - node->setDoubleValue("zoffset-in", - gr->GetGearUnit(i)->GetBodyLocation()(3)); - node->setBoolValue("wow", gr->GetGearUnit(i)->GetWOW()); - node->setBoolValue("has-brake", gr->GetGearUnit(i)->GetBrakeGroup() > 0); + node->setDoubleValue("xoffset-in", gear->GetBodyLocation()(1)); + node->setDoubleValue("yoffset-in", gear->GetBodyLocation()(2)); + node->setDoubleValue("zoffset-in", gear->GetBodyLocation()(3)); + node->setBoolValue("wow", gear->GetWOW()); + node->setBoolValue("has-brake", gear->GetBrakeGroup() > 0); node->setDoubleValue("position-norm", FCS->GetGearPos()); - node->setDoubleValue("tire-pressure-norm", gr->GetGearUnit(i)->GetTirePressure()); + node->setDoubleValue("tire-pressure-norm", gear->GetTirePressure()); + if ( gear->GetSteerable() ) + node->setDoubleValue("steering-norm", gear->GetSteerNorm()); } } @@ -933,10 +936,13 @@ void FGJSBsim::update_gear(void) FGGroundReactions* gr=fdmex->GetGroundReactions(); int Ngear=GroundReactions->GetNumGearUnits(); for (int i=0;iGetGearUnit(i); SGPropertyNode * node = fgGetNode("gear/gear", i, true); - node->getChild("wow", 0, true)->setBoolValue(gr->GetGearUnit(i)->GetWOW()); + node->getChild("wow", 0, true)->setBoolValue( gear->GetWOW()); node->getChild("position-norm", 0, true)->setDoubleValue(FCS->GetGearPos()); - gr->GetGearUnit(i)->SetTirePressure(node->getDoubleValue("tire-pressure-norm")); + gear->SetTirePressure(node->getDoubleValue("tire-pressure-norm")); + if ( gear->GetSteerable() ) + node->setDoubleValue("steering-norm", gear->GetSteerNorm()); } }