From f63d336ebcc5318a83a4db15a73a7c77cc192d53 Mon Sep 17 00:00:00 2001 From: ehofman Date: Sun, 14 Mar 2004 14:57:07 +0000 Subject: [PATCH] Sync. with JSBSim CVS. --- src/FDM/JSBSim/FGAerodynamics.cpp | 5 +- src/FDM/JSBSim/FGAerodynamics.h | 5 +- src/FDM/JSBSim/FGAircraft.cpp | 96 +- src/FDM/JSBSim/FGAircraft.h | 5 - src/FDM/JSBSim/FGAtmosphere.cpp | 24 +- src/FDM/JSBSim/FGAtmosphere.h | 2 - src/FDM/JSBSim/FGAuxiliary.cpp | 6 - src/FDM/JSBSim/FGAuxiliary.h | 2 - src/FDM/JSBSim/FGColumnVector3.cpp | 474 +++----- src/FDM/JSBSim/FGColumnVector3.h | 267 ++++- src/FDM/JSBSim/FGColumnVector4.cpp | 5 +- src/FDM/JSBSim/FGColumnVector4.h | 12 +- src/FDM/JSBSim/FGFCS.cpp | 118 +- src/FDM/JSBSim/FGForce.cpp | 362 +++--- src/FDM/JSBSim/FGForce.h | 1 - src/FDM/JSBSim/FGGroundReactions.h | 3 +- src/FDM/JSBSim/FGInertial.cpp | 8 +- src/FDM/JSBSim/FGInertial.h | 2 - src/FDM/JSBSim/FGInitialCondition.cpp | 116 +- src/FDM/JSBSim/FGInitialCondition.h | 3 - src/FDM/JSBSim/FGJSBBase.cpp | 388 +++---- src/FDM/JSBSim/FGJSBBase.h | 2 + src/FDM/JSBSim/FGLGear.cpp | 1171 ++++++++++---------- src/FDM/JSBSim/FGLGear.h | 6 +- src/FDM/JSBSim/FGMassBalance.cpp | 143 +-- src/FDM/JSBSim/FGMassBalance.h | 78 +- src/FDM/JSBSim/FGMatrix33.cpp | 836 +++++++------- src/FDM/JSBSim/FGMatrix33.h | 380 ++++++- src/FDM/JSBSim/FGNozzle.cpp | 1 + src/FDM/JSBSim/FGOutput.cpp | 1284 +++++++++++----------- src/FDM/JSBSim/FGPosition.cpp | 740 +++++++------ src/FDM/JSBSim/FGPosition.h | 2 - src/FDM/JSBSim/FGPropeller.cpp | 3 + src/FDM/JSBSim/FGPropeller.h | 2 - src/FDM/JSBSim/FGPropertyManager.cpp | 83 +- src/FDM/JSBSim/FGPropertyManager.h | 66 +- src/FDM/JSBSim/FGPropulsion.cpp | 86 +- src/FDM/JSBSim/FGPropulsion.h | 45 +- src/FDM/JSBSim/FGRotation.cpp | 24 +- src/FDM/JSBSim/FGRotation.h | 13 +- src/FDM/JSBSim/FGSimTurbine.cpp | 1 + src/FDM/JSBSim/FGState.cpp | 68 +- src/FDM/JSBSim/FGState.h | 10 - src/FDM/JSBSim/FGTank.cpp | 13 +- src/FDM/JSBSim/FGTank.h | 10 +- src/FDM/JSBSim/FGTranslation.cpp | 21 +- src/FDM/JSBSim/FGTranslation.h | 4 - src/FDM/JSBSim/FGTrim.cpp | 5 +- src/FDM/JSBSim/JSBSim.cxx | 133 ++- src/FDM/JSBSim/filtersjb/FGCondition.cpp | 515 +++++---- src/FDM/JSBSim/filtersjb/FGKinemat.cpp | 147 ++- src/FDM/JSBSim/filtersjb/FGKinemat.h | 9 +- src/FDM/JSBSim/filtersjb/FGSwitch.cpp | 560 +++++----- src/FDM/JSBSim/filtersjb/FGSwitch.h | 12 +- 54 files changed, 4273 insertions(+), 4104 deletions(-) diff --git a/src/FDM/JSBSim/FGAerodynamics.cpp b/src/FDM/JSBSim/FGAerodynamics.cpp index 7617c1f2f..b6687891e 100644 --- a/src/FDM/JSBSim/FGAerodynamics.cpp +++ b/src/FDM/JSBSim/FGAerodynamics.cpp @@ -37,8 +37,11 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGAerodynamics.h" +#include "FGTranslation.h" +#include "FGAircraft.h" +#include "FGState.h" +#include "FGMassBalance.h" #include "FGFactorGroup.h" -#include "FGCoefficient.h" #include "FGPropertyManager.h" namespace JSBSim { diff --git a/src/FDM/JSBSim/FGAerodynamics.h b/src/FDM/JSBSim/FGAerodynamics.h index def5575c6..35c35820a 100644 --- a/src/FDM/JSBSim/FGAerodynamics.h +++ b/src/FDM/JSBSim/FGAerodynamics.h @@ -54,11 +54,8 @@ INCLUDES #include "FGModel.h" #include "FGConfigFile.h" -#include "FGState.h" -#include "FGMassBalance.h" -#include "FGTranslation.h" #include "FGCoefficient.h" -#include "FGFactorGroup.h" +#include "FGColumnVector3.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS diff --git a/src/FDM/JSBSim/FGAircraft.cpp b/src/FDM/JSBSim/FGAircraft.cpp index 3542546ae..c86f382b6 100644 --- a/src/FDM/JSBSim/FGAircraft.cpp +++ b/src/FDM/JSBSim/FGAircraft.cpp @@ -1,49 +1,49 @@ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - + Module: FGAircraft.cpp Author: Jon S. Berndt - Date started: 12/12/98 + Date started: 12/12/98 Purpose: Encapsulates an aircraft Called by: FGFDMExec ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- - + 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. - + FUNCTIONAL DESCRIPTION -------------------------------------------------------------------------------- Models the aircraft reactions and forces. This class is instantiated by the -FGFDMExec class and scheduled as an FDM entry. - +FGFDMExec class and scheduled as an FDM entry. + HISTORY -------------------------------------------------------------------------------- 12/12/98 JSB Created 04/03/99 JSB Changed Aero() method to correct body axis force calculation from wind vector. Fix provided by Tony Peden. 05/03/99 JSB Changed (for the better?) the way configurations are read in. -9/17/99 TP Combined force and moment functions. Added aero reference - point to config file. Added calculations for moments due to +9/17/99 TP Combined force and moment functions. Added aero reference + point to config file. Added calculations for moments due to difference in cg and aero reference point - + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% COMMENTS, REFERENCES, and NOTES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -73,15 +73,9 @@ INCLUDES #include "FGInertial.h" #include "FGGroundReactions.h" #include "FGAerodynamics.h" -#include "FGTranslation.h" -#include "FGRotation.h" -#include "FGAtmosphere.h" #include "FGState.h" #include "FGFDMExec.h" -#include "FGFCS.h" #include "FGPosition.h" -#include "FGAuxiliary.h" -#include "FGOutput.h" #include "FGPropertyManager.h" namespace JSBSim { @@ -139,14 +133,14 @@ bool FGAircraft::Run(void) vMoments += Aerodynamics->GetMoments(); vMoments += Propulsion->GetMoments(); vMoments += GroundReactions->GetMoments(); - + vBodyAccel = vForces/MassBalance->GetMass(); - + vNcg = vBodyAccel/Inertial->gravity(); vNwcg = State->GetTb2s() * vNcg; vNwcg(3) = -1*vNwcg(3) + 1; - + return false; } else { // skip Run() execution this time return true; @@ -166,10 +160,12 @@ bool FGAircraft::Load(FGConfigFile* AC_cfg) { string token = ""; string parameter; - double EW, bixx, biyy, bizz, bixy, bixz; + double EW, bixx, biyy, bizz, bixy, bixz, biyz; double pmWt, pmX, pmY, pmZ; FGColumnVector3 vbaseXYZcg; + bixx = biyy = bizz = bixy = bixz = biyz = 0.0; + AC_cfg->GetNextConfigLine(); while ((token = AC_cfg->GetValue()) != string("/METRICS")) { @@ -182,7 +178,7 @@ bool FGAircraft::Load(FGConfigFile* AC_cfg) if (debug_lvl > 0) cout << " WingSpan: " << WingSpan << endl; } else if (parameter == "AC_WINGINCIDENCE") { *AC_cfg >> WingIncidence; - if (debug_lvl > 0) cout << " Chord: " << cbar << endl; + if (debug_lvl > 0) cout << " Incidence: " << WingIncidence << endl; } else if (parameter == "AC_CHORD") { *AC_cfg >> cbar; if (debug_lvl > 0) cout << " Chord: " << cbar << endl; @@ -200,28 +196,26 @@ bool FGAircraft::Load(FGConfigFile* AC_cfg) if (debug_lvl > 0) cout << " V. Tail Arm: " << VTailArm << endl; } else if (parameter == "AC_IXX") { *AC_cfg >> bixx; - if (debug_lvl > 0) cout << " baseIxx: " << bixx << endl; - MassBalance->SetBaseIxx(bixx); + if (debug_lvl > 0) cout << " baseIxx: " << bixx << " slug-ft2" << endl; } else if (parameter == "AC_IYY") { *AC_cfg >> biyy; - if (debug_lvl > 0) cout << " baseIyy: " << biyy << endl; - MassBalance->SetBaseIyy(biyy); + if (debug_lvl > 0) cout << " baseIyy: " << biyy << " slug-ft2" << endl; } else if (parameter == "AC_IZZ") { *AC_cfg >> bizz; - if (debug_lvl > 0) cout << " baseIzz: " << bizz << endl; - MassBalance->SetBaseIzz(bizz); + if (debug_lvl > 0) cout << " baseIzz: " << bizz << " slug-ft2" << endl; } else if (parameter == "AC_IXY") { *AC_cfg >> bixy; - if (debug_lvl > 0) cout << " baseIxy: " << bixy << endl; - MassBalance->SetBaseIxy(bixy); + if (debug_lvl > 0) cout << " baseIxy: " << bixy << " slug-ft2" << endl; } else if (parameter == "AC_IXZ") { *AC_cfg >> bixz; - if (debug_lvl > 0) cout << " baseIxz: " << bixz << endl; - MassBalance->SetBaseIxz(bixz); + if (debug_lvl > 0) cout << " baseIxz: " << bixz << " slug-ft2" << endl; + } else if (parameter == "AC_IYZ") { + *AC_cfg >> biyz; + if (debug_lvl > 0) cout << " baseIyz: " << biyz << " slug-ft2" << endl; } else if (parameter == "AC_EMPTYWT") { *AC_cfg >> EW; MassBalance->SetEmptyWeight(EW); - if (debug_lvl > 0) cout << " EmptyWeight: " << EW << endl; + if (debug_lvl > 0) cout << " EmptyWeight: " << EW << " lbm" << endl; } else if (parameter == "AC_CGLOC") { *AC_cfg >> vbaseXYZcg(eX) >> vbaseXYZcg(eY) >> vbaseXYZcg(eZ); MassBalance->SetBaseCG(vbaseXYZcg); @@ -244,7 +238,11 @@ bool FGAircraft::Load(FGConfigFile* AC_cfg) << endl; } } - + + MassBalance->SetAircraftBaseInertias(FGMatrix33( bixx, -bixy, -bixz, + -bixy, biyy, -biyz, + -bixz, -biyz, bizz )); + // calculate some derived parameters if (cbar != 0.0) { lbarh = HTailArm/cbar; @@ -253,7 +251,7 @@ bool FGAircraft::Load(FGConfigFile* AC_cfg) vbarh = HTailArm*HTailArea / (cbar*WingArea); vbarv = VTailArm*VTailArea / (cbar*WingArea); } - } + } return true; } @@ -298,17 +296,17 @@ void FGAircraft::bind(void) (PMF)&FGAircraft::GetForces); PropertyManager->Tie("forces/fbz-total-lbs", this,3, (PMF)&FGAircraft::GetForces); - PropertyManager->Tie("metrics/aero-rp-x-ft", this,1, + PropertyManager->Tie("metrics/aero-rp-x-in", this,1, (PMF)&FGAircraft::GetXYZrp); - PropertyManager->Tie("metrics/aero-rp-y-ft", this,2, + PropertyManager->Tie("metrics/aero-rp-y-in", this,2, (PMF)&FGAircraft::GetXYZrp); - PropertyManager->Tie("metrics/aero-rp-z-ft", this,3, + PropertyManager->Tie("metrics/aero-rp-z-in", this,3, (PMF)&FGAircraft::GetXYZrp); - PropertyManager->Tie("metrics/eyepoint-x-ft", this,1, + PropertyManager->Tie("metrics/eyepoint-x-in", this,1, (PMF)&FGAircraft::GetXYZep); - PropertyManager->Tie("metrics/eyepoint-y-ft", this,2, + PropertyManager->Tie("metrics/eyepoint-y-in", this,2, (PMF)&FGAircraft::GetXYZep); - PropertyManager->Tie("metrics/eyepoint-z-ft", this,3, + PropertyManager->Tie("metrics/eyepoint-z-in", this,3, (PMF)&FGAircraft::GetXYZep); PropertyManager->Tie("metrics/visualrefpoint-x-in", this,1, (PMF)&FGAircraft::GetXYZvrp); @@ -340,12 +338,12 @@ void FGAircraft::unbind(void) PropertyManager->Untie("forces/fbx-total-lbs"); PropertyManager->Untie("forces/fby-total-lbs"); PropertyManager->Untie("forces/fbz-total-lbs"); - PropertyManager->Untie("metrics/aero-rp-x-ft"); - PropertyManager->Untie("metrics/aero-rp-y-ft"); - PropertyManager->Untie("metrics/aero-rp-z-ft"); - PropertyManager->Untie("metrics/eyepoint-x-ft"); - PropertyManager->Untie("metrics/eyepoint-y-ft"); - PropertyManager->Untie("metrics/eyepoint-z-ft"); + PropertyManager->Untie("metrics/aero-rp-x-in"); + PropertyManager->Untie("metrics/aero-rp-y-in"); + PropertyManager->Untie("metrics/aero-rp-z-in"); + PropertyManager->Untie("metrics/eyepoint-x-in"); + PropertyManager->Untie("metrics/eyepoint-y-in"); + PropertyManager->Untie("metrics/eyepoint-z-in"); PropertyManager->Untie("metrics/visualrefpoint-x-in"); PropertyManager->Untie("metrics/visualrefpoint-y-in"); PropertyManager->Untie("metrics/visualrefpoint-z-in"); diff --git a/src/FDM/JSBSim/FGAircraft.h b/src/FDM/JSBSim/FGAircraft.h index 4ddcd4687..acf0b54e3 100644 --- a/src/FDM/JSBSim/FGAircraft.h +++ b/src/FDM/JSBSim/FGAircraft.h @@ -53,12 +53,8 @@ INCLUDES #endif #include "FGModel.h" -#include "FGPropulsion.h" #include "FGConfigFile.h" -#include "FGMatrix33.h" #include "FGColumnVector3.h" -#include "FGColumnVector4.h" -#include "FGLGear.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS @@ -169,7 +165,6 @@ private: FGColumnVector3 vXYZrp; FGColumnVector3 vXYZvrp; FGColumnVector3 vXYZep; - FGColumnVector3 vEuler; FGColumnVector3 vDXYZcg; FGColumnVector3 vBodyAccel; FGColumnVector3 vNcg; diff --git a/src/FDM/JSBSim/FGAtmosphere.cpp b/src/FDM/JSBSim/FGAtmosphere.cpp index df7af2fe9..14aa04177 100644 --- a/src/FDM/JSBSim/FGAtmosphere.cpp +++ b/src/FDM/JSBSim/FGAtmosphere.cpp @@ -50,16 +50,9 @@ INCLUDES #include "FGAtmosphere.h" #include "FGState.h" #include "FGFDMExec.h" -#include "FGFCS.h" #include "FGAircraft.h" -#include "FGTranslation.h" -#include "FGRotation.h" #include "FGPosition.h" -#include "FGAuxiliary.h" -#include "FGOutput.h" -#include "FGMatrix33.h" -#include "FGColumnVector3.h" -#include "FGColumnVector4.h" +#include "FGInertial.h" #include "FGPropertyManager.h" namespace JSBSim { @@ -152,8 +145,6 @@ bool FGAtmosphere::Run(void) soundspeed = sqrt(SHRatio*Reng*(*temperature)); - State->Seta(soundspeed); - Debug(2); return false; @@ -308,13 +299,18 @@ void FGAtmosphere::Turbulence(void) vTurbulenceGrad = TurbGain*MagnitudeAccel * vDirection; vBodyTurbGrad = State->GetTl2b()*vTurbulenceGrad; - vTurbPQR(eP) = vBodyTurbGrad(eY)/Aircraft->GetWingSpan(); + + if (Aircraft->GetWingSpan() > 0) { + vTurbPQR(eP) = vBodyTurbGrad(eY)/Aircraft->GetWingSpan(); + } else { + vTurbPQR(eP) = vBodyTurbGrad(eY)/30.0; + } // if (Aircraft->GetHTailArm() != 0.0) // vTurbPQR(eQ) = vBodyTurbGrad(eZ)/Aircraft->GetHTailArm(); // else // vTurbPQR(eQ) = vBodyTurbGrad(eZ)/10.0; - if (Aircraft->GetVTailArm()) + if (Aircraft->GetVTailArm() > 0) vTurbPQR(eR) = vBodyTurbGrad(eX)/Aircraft->GetVTailArm(); else vTurbPQR(eR) = vBodyTurbGrad(eX)/10.0; @@ -356,12 +352,12 @@ void FGAtmosphere::Turbulence(void) vBodyTurbGrad = State->GetTl2b()*vTurbulenceGrad; vTurbPQR(eP) = vBodyTurbGrad(eY)/Aircraft->GetWingSpan(); - if (Aircraft->GetHTailArm() != 0.0) + if (Aircraft->GetHTailArm() > 0) vTurbPQR(eQ) = vBodyTurbGrad(eZ)/Aircraft->GetHTailArm(); else vTurbPQR(eQ) = vBodyTurbGrad(eZ)/10.0; - if (Aircraft->GetVTailArm()) + if (Aircraft->GetVTailArm() > 0) vTurbPQR(eR) = vBodyTurbGrad(eX)/Aircraft->GetVTailArm(); else vTurbPQR(eR) = vBodyTurbGrad(eX)/10.0; diff --git a/src/FDM/JSBSim/FGAtmosphere.h b/src/FDM/JSBSim/FGAtmosphere.h index 2eae721fa..7c8aec696 100644 --- a/src/FDM/JSBSim/FGAtmosphere.h +++ b/src/FDM/JSBSim/FGAtmosphere.h @@ -43,9 +43,7 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGModel.h" -#include "FGMatrix33.h" #include "FGColumnVector3.h" -#include "FGColumnVector4.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS diff --git a/src/FDM/JSBSim/FGAuxiliary.cpp b/src/FDM/JSBSim/FGAuxiliary.cpp index ca86c769e..09b2aefc7 100644 --- a/src/FDM/JSBSim/FGAuxiliary.cpp +++ b/src/FDM/JSBSim/FGAuxiliary.cpp @@ -47,14 +47,8 @@ INCLUDES #include "FGAtmosphere.h" #include "FGState.h" #include "FGFDMExec.h" -#include "FGFCS.h" #include "FGAircraft.h" -#include "FGPosition.h" -#include "FGOutput.h" #include "FGInertial.h" -#include "FGMatrix33.h" -#include "FGColumnVector3.h" -#include "FGColumnVector4.h" #include "FGPropertyManager.h" namespace JSBSim { diff --git a/src/FDM/JSBSim/FGAuxiliary.h b/src/FDM/JSBSim/FGAuxiliary.h index a76976978..5a61290ff 100644 --- a/src/FDM/JSBSim/FGAuxiliary.h +++ b/src/FDM/JSBSim/FGAuxiliary.h @@ -40,9 +40,7 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGModel.h" -#include "FGMatrix33.h" #include "FGColumnVector3.h" -#include "FGColumnVector4.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS diff --git a/src/FDM/JSBSim/FGColumnVector3.cpp b/src/FDM/JSBSim/FGColumnVector3.cpp index 254317631..0e9c1060e 100644 --- a/src/FDM/JSBSim/FGColumnVector3.cpp +++ b/src/FDM/JSBSim/FGColumnVector3.cpp @@ -1,329 +1,145 @@ -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -Module: FGColumnVector3.cpp -Author: Originally by Tony Peden [formatted here (and broken??) by JSB] -Date started: 1998 -Purpose: FGColumnVector3 class -Called by: Various - -FUNCTIONAL DESCRIPTION --------------------------------------------------------------------------------- - -HISTORY --------------------------------------------------------------------------------- -??/??/?? TP Created -03/16/2000 JSB Added exception throwing - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -INCLUDES -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -#include "FGColumnVector3.h" - -namespace JSBSim { - -static const char *IdSrc = "$Id$"; -static const char *IdHdr = ID_COLUMNVECTOR3; - -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -CLASS IMPLEMENTATION -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -FGColumnVector3::FGColumnVector3(void) -{ - rowCtr = 1; - data[0]=0; data[1]=0; data[2]=0; data[3]=0; - - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3::FGColumnVector3(double X, double Y, double Z) -{ - rowCtr = 1; - data[0] = 0; data[eX] = X; data[eY] = Y; data[eZ] = Z; - - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3::~FGColumnVector3(void) -{ - Debug(1); -} - - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3::FGColumnVector3(const FGColumnVector3& b) -{ - data[1] = b.data[1]; - data[2] = b.data[2]; - data[3] = b.data[3]; - rowCtr = 1; - - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3 FGColumnVector3::operator=(const FGColumnVector3& b) -{ - data[1] = b.data[1]; - data[2] = b.data[2]; - data[3] = b.data[3]; - rowCtr = 1; - - return *this; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3 FGColumnVector3::operator+(const FGColumnVector3& C) -{ - FGColumnVector3 Sum; - Sum(1) = C(1) + data[1]; - Sum(2) = C(2) + data[2]; - Sum(3) = C(3) + data[3]; - - return Sum; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGColumnVector3::operator+=(const FGColumnVector3& C) -{ - data[1] += C(1); - data[2] += C(2); - data[3] += C(3); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3 FGColumnVector3::operator*(const double scalar) -{ - FGColumnVector3 Product; - - Product(1) = scalar * data[1]; - Product(2) = scalar * data[2]; - Product(3) = scalar * data[3]; - - return Product; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGColumnVector3::operator*=(const double scalar) -{ - data[1] *= scalar; - data[2] *= scalar; - data[3] *= scalar; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3 FGColumnVector3::operator-(const FGColumnVector3& V) -{ - - FGColumnVector3 Diff; - - Diff(1) = data[1] - V(1); - Diff(2) = data[2] - V(2); - Diff(3) = data[3] - V(3); - - return Diff; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGColumnVector3::operator-=(const FGColumnVector3& V) -{ - data[1] -= V(1); - data[2] -= V(2); - data[3] -= V(3); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3 FGColumnVector3::operator/(const double scalar) -{ - FGColumnVector3 Quotient; - - if (scalar != 0) { - double tmp = 1.0/scalar; - Quotient(1) = data[1] * tmp; - Quotient(2) = data[2] * tmp; - Quotient(3) = data[3] * tmp; - } else { - cerr << "Attempt to divide by zero in method FGColumnVector3::operator/(const double scalar), object " << this << endl; - } - return Quotient; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGColumnVector3::operator/=(const double scalar) -{ - FGColumnVector3 Quotient; - - if (scalar != 0) { - double tmp = 1.0/scalar; - data[1] *= tmp; - data[2] *= tmp; - data[3] *= tmp; - } else { - cerr << "Attempt to divide by zero in method FGColumnVector3::operator/=(const double scalar), object " << this << endl; - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3 operator*(const double scalar, const FGColumnVector3& C) -{ - FGColumnVector3 Product; - - Product(1) = scalar * C(1); - Product(2) = scalar * C(2); - Product(3) = scalar * C(3); - - return Product; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -double FGColumnVector3::Magnitude(void) -{ - double num; - - if ((data[1] == 0.00) && - (data[2] == 0.00) && - (data[3] == 0.00)) - { - return 0.00; - } else { - num = data[1]*data[1]; - num += data[2]*data[2]; - num += data[3]*data[3]; - return sqrt(num); - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3 FGColumnVector3::Normalize(void) -{ - double Mag = Magnitude(); - - if (Mag != 0) { - Mag = 1.0/Mag; - data[1] *= Mag; - data[2] *= Mag; - data[3] *= Mag; - } - - return *this; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3 FGColumnVector3::operator*(const FGColumnVector3& V) -{ - FGColumnVector3 Product; - - Product(1) = data[2] * V(3) - data[3] * V(2); - Product(2) = data[3] * V(1) - data[1] * V(3); - Product(3) = data[1] * V(2) - data[2] * V(1); - - return Product; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGColumnVector3::operator*=(const FGColumnVector3& V) -{ - double a,b,c; - a = data[1]; b=data[2]; c=data[3]; - - data[1] = b * V(3) - c * V(2); - data[2] = c * V(1) - a * V(3); - data[3] = a * V(2) - b * V(1); - -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3 FGColumnVector3::multElementWise(const FGColumnVector3& V) -{ - FGColumnVector3 Product; - - Product(1) = data[1] * V(1); - Product(2) = data[2] * V(2); - Product(3) = data[3] * V(3); - - return Product; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -ostream& operator<<(ostream& os, const FGColumnVector3& col) -{ - os << col(1) << " , " << col(2) << " , " << col(3); - return os; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3& FGColumnVector3::operator<<(const double ff) -{ - data[rowCtr] = ff; - if (++rowCtr > 3 ) - rowCtr = 1; - return *this; -} - -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -// The bitmasked value choices are as follows: -// unset: In this case (the default) JSBSim would only print -// out the normally expected messages, essentially echoing -// the config files as they are read. If the environment -// variable is not set, debug_lvl is set to 1 internally -// 0: This requests JSBSim not to output any messages -// whatsoever. -// 1: This value explicity requests the normal JSBSim -// startup messages -// 2: This value asks for a message to be printed out when -// a class is instantiated -// 4: When this value is set, a message is displayed when a -// FGModel object executes its Run() method -// 8: When this value is set, various runtime state variables -// are printed out periodically -// 16: When set various parameters are sanity checked and -// a message is printed out when they go out of bounds - -void FGColumnVector3::Debug(int from) -{ - if (debug_lvl <= 0) return; - - if (debug_lvl & 1) { // Standard console startup message output - } - if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGColumnVector3" << endl; - if (from == 1) cout << "Destroyed: FGColumnVector3" << endl; - } - if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects - } - if (debug_lvl & 8 ) { // Runtime state variables - } - if (debug_lvl & 16) { // Sanity checking - } - if (debug_lvl & 64) { - if (from == 0) { // Constructor - cout << IdSrc << endl; - cout << IdHdr << endl; - } - } -} - -} // namespace JSBSim +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +Module: FGColumnVector3.cpp +Author: Originally by Tony Peden [formatted here (and broken??) by JSB] +Date started: 1998 +Purpose: FGColumnVector3 class +Called by: Various + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- + +HISTORY +-------------------------------------------------------------------------------- +??/??/?? TP Created +03/16/2000 JSB Added exception throwing + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGColumnVector3.h" + +namespace JSBSim { + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_COLUMNVECTOR3; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +FGColumnVector3::FGColumnVector3(void) +{ + data[0] = data[1] = data[2] = 0.0; + Debug(0); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 FGColumnVector3::operator/(const double scalar) const +{ + if (scalar != 0.0) + return operator*( 1.0/scalar ); + + cerr << "Attempt to divide by zero in method " + "FGColumnVector3::operator/(const double scalar), " + "object " << this << endl; + return FGColumnVector3(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3& FGColumnVector3::operator/=(const double scalar) +{ + if (scalar != 0.0) + operator*=( 1.0/scalar ); + else + cerr << "Attempt to divide by zero in method " + "FGColumnVector3::operator/=(const double scalar), " + "object " << this << endl; + + return *this; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +double FGColumnVector3::Magnitude(void) const +{ + if (data[1] == 0.0 && data[2] == 0.0 && data[3] == 0.0) + return 0.0; + else + return sqrt( Entry(1)*Entry(1) + Entry(2)*Entry(2) + Entry(3)*Entry(3) ); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3& FGColumnVector3::Normalize(void) +{ + double Mag = Magnitude(); + + if (Mag != 0.0) + operator*=( 1.0/Mag ); + + return *this; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 FGColumnVector3::multElementWise(const FGColumnVector3& V) const +{ + return FGColumnVector3(Entry(1) * V(1), Entry(2) * V(2), Entry(3) * V(3)); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +ostream& operator<<(ostream& os, const FGColumnVector3& col) +{ + os << col(1) << " , " << col(2) << " , " << col(3); + return os; +} + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ +// The bitmasked value choices are as follows: +// unset: In this case (the default) JSBSim would only print +// out the normally expected messages, essentially echoing +// the config files as they are read. If the environment +// variable is not set, debug_lvl is set to 1 internally +// 0: This requests JSBSim not to output any messages +// whatsoever. +// 1: This value explicity requests the normal JSBSim +// startup messages +// 2: This value asks for a message to be printed out when +// a class is instantiated +// 4: When this value is set, a message is displayed when a +// FGModel object executes its Run() method +// 8: When this value is set, various runtime state variables +// are printed out periodically +// 16: When set various parameters are sanity checked and +// a message is printed out when they go out of bounds + +void FGColumnVector3::Debug(int from) +{ + if (debug_lvl <= 0) return; + + if (debug_lvl & 1) { // Standard console startup message output + } + if (debug_lvl & 2 ) { // Instantiation/Destruction notification + if (from == 0) cout << "Instantiated: FGColumnVector3" << endl; + if (from == 1) cout << "Destroyed: FGColumnVector3" << endl; + } + if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects + } + if (debug_lvl & 8 ) { // Runtime state variables + } + if (debug_lvl & 16) { // Sanity checking + } + if (debug_lvl & 64) { + if (from == 0) { // Constructor + cout << IdSrc << endl; + cout << IdHdr << endl; + } + } +} + +} // namespace JSBSim diff --git a/src/FDM/JSBSim/FGColumnVector3.h b/src/FDM/JSBSim/FGColumnVector3.h index b2f41f825..f5044709a 100644 --- a/src/FDM/JSBSim/FGColumnVector3.h +++ b/src/FDM/JSBSim/FGColumnVector3.h @@ -6,6 +6,8 @@ Date started: Unknown HISTORY -------------------------------------------------------------------------------- +??/??/???? ?? Initial version and more. +03/06/2004 MF Rework, document and do much inlineing. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SENTRY @@ -31,6 +33,7 @@ INCLUDES SG_USING_STD(cerr); SG_USING_STD(cout); SG_USING_STD(endl); + SG_USING_STD(sqrt); #else # include # if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740) @@ -50,6 +53,7 @@ INCLUDES using std::cerr; using std::cout; using std::endl; + using std::sqrt; # endif using std::string; #endif @@ -84,47 +88,252 @@ CLASS DECLARATION class FGColumnVector3 : public FGJSBBase { public: + /** Default initializer. + + Create a zero vector. + */ FGColumnVector3(void); - FGColumnVector3(double X, double Y, double Z); - FGColumnVector3(const FGColumnVector3& b); - ~FGColumnVector3(void); - - FGColumnVector3 operator=(const FGColumnVector3& b); - - FGColumnVector3 operator*(const double scalar); - FGColumnVector3 operator*(const FGColumnVector3& V); // Cross product operator - FGColumnVector3 operator/(const double scalar); - FGColumnVector3 operator+(const FGColumnVector3& B); // must not return reference - FGColumnVector3 operator-(const FGColumnVector3& B); - - void operator-=(const FGColumnVector3 &B); - void operator+=(const FGColumnVector3 &B); - void operator*=(const FGColumnVector3 &B); - void operator*=(const double scalar); - void operator/=(const double scalar); - FGColumnVector3& operator<<(const double ff); + /** Initialization by given values. + + @param X value of the x-conponent. + @param Y value of the y-conponent. + @param Z value of the z-conponent. + + Create a vector from the doubles given in the arguments. + */ + FGColumnVector3(double X, double Y, double Z) { + data[0] = X; + data[1] = Y; + data[2] = Z; + Debug(0); + } + + /** Copy constructor. + + @param v Vector which is used for initialization. + + Create copy of the vector given in the argument. + */ + FGColumnVector3(const FGColumnVector3& v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + Debug(0); + } + + /** Destructor. + */ + ~FGColumnVector3(void) { Debug(1); } + + + /** 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 data[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) { return data[idx-1]; } + + /** Assignment operator. + + @param b source vector. + + Copy the content of the vector given in the argument into *this. + */ + FGColumnVector3& operator=(const FGColumnVector3& b) { + data[0] = b.data[0]; + data[1] = b.data[1]; + data[2] = b.data[2]; + return *this; + } + + /** Multiplication by a scalar. + + @param scalar scalar value to multiply the vector with. + @return The resulting vector from the multiplication with that scalar. + + Multiply the vector with the scalar given in the argument. + */ + FGColumnVector3 operator*(const double scalar) const { + return FGColumnVector3(scalar*Entry(1), scalar*Entry(2), scalar*Entry(3)); + } + + /** Multiply by 1/scalar. + + @param scalar scalar value to devide the vector through. + @return The resulting vector from the division through that scalar. + + Multiply the vector with the 1/scalar given in the argument. + */ + FGColumnVector3 operator/(const double scalar) const; + + /** Cross product multiplication. + + @param v vector to multiply with. + @return The resulting vector from the cross product multiplication. + + Compute and return the cross product of the current vector with + the given argument. + */ + FGColumnVector3 operator*(const FGColumnVector3& V) const { + return FGColumnVector3( Entry(2) * V(3) - Entry(3) * V(2), + Entry(3) * V(1) - Entry(1) * V(3), + Entry(1) * V(2) - Entry(2) * V(1) ); + } + + /** Addition operator. + */ + FGColumnVector3 operator+(const FGColumnVector3& B) const { + return FGColumnVector3( Entry(1) + B(1), Entry(2) + B(2), Entry(3) + B(3) ); + } + + /** Subtraction operator. + */ + FGColumnVector3 operator-(const FGColumnVector3& B) const { + return FGColumnVector3( Entry(1) - B(1), Entry(2) - B(2), Entry(3) - B(3) ); + } - inline void InitMatrix(void) { data[1]=0; data[2]=0; data[3]=0; } - inline void InitMatrix(double ff) { data[1]=ff; data[2]=ff; data[3]=ff; } + /** Subtract an other vector. + */ + FGColumnVector3& operator-=(const FGColumnVector3 &B) { + Entry(1) -= B(1); + Entry(2) -= B(2); + Entry(3) -= B(3); + return *this; + } - double Magnitude(void); - FGColumnVector3 Normalize(void); + /** Add an other vector. + */ + FGColumnVector3& operator+=(const FGColumnVector3 &B) { + Entry(1) += B(1); + Entry(2) += B(2); + Entry(3) += B(3); + return *this; + } - friend FGColumnVector3 operator*(const double scalar, const FGColumnVector3& A); + /** Scale by a scalar. + */ + FGColumnVector3& operator*=(const double scalar) { + Entry(1) *= scalar; + Entry(2) *= scalar; + Entry(3) *= scalar; + return *this; + } - friend ostream& operator<<(ostream& os, const FGColumnVector3& col); + /** Scale by a 1/scalar. + */ + FGColumnVector3& operator/=(const double scalar); - inline double operator()(int m) const { return data[m]; } - inline double& operator()(int m) { return data[m]; } + void InitMatrix(void) { data[0] = data[1] = data[2] = 0.0; } + void InitMatrix(double a) { data[0] = data[1] = data[2] = a; } + void InitMatrix(double a, double b, double c) { + data[0]=a; data[1]=b; data[2]=c; + } - FGColumnVector3 multElementWise(const FGColumnVector3& V); + /** Length of the vector. + + Compute and return the euclidean norm of this vector. + */ + double Magnitude(void) const; + + /** Normialze. + + Normalize the vector to have the Magnitude() == 1.0. If the vector + is equal to zero it is left untouched. + */ + FGColumnVector3& Normalize(void); + + // ??? Is this something sensible ?? + FGColumnVector3 multElementWise(const FGColumnVector3& V) const; + + // little trick here. + struct AssignRef { + AssignRef(FGColumnVector3& r, int i) : Ref(r), idx(i) {} + AssignRef operator<<(const double ff) { + Ref.Entry(idx) = ff; + return AssignRef(Ref, idx+1); + } + FGColumnVector3& Ref; + int idx; + }; + AssignRef operator<<(const double ff) { + Entry(1) = ff; + return AssignRef(*this, 2); + } private: - double data[4]; - int rowCtr; + double data[3]; + void Debug(int from); }; + +/** Scalar multiplication. + + @param scalar scalar value to multiply with. + @param A Vector to multiply. + + Multiply the Vector with a scalar value. +*/ +inline FGColumnVector3 operator*(double scalar, const FGColumnVector3& A) { + // use already defined operation. + return A*scalar; } + +/** Write vector to a stream. + + @param os Stream to write to. + @param M Matrix to write. + + Write the matrix to a stream. +*/ +ostream& operator<<(ostream& os, const FGColumnVector3& col); + +} // namespace JSBSim + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% #endif diff --git a/src/FDM/JSBSim/FGColumnVector4.cpp b/src/FDM/JSBSim/FGColumnVector4.cpp index de65c0117..b387137d4 100644 --- a/src/FDM/JSBSim/FGColumnVector4.cpp +++ b/src/FDM/JSBSim/FGColumnVector4.cpp @@ -42,7 +42,10 @@ FGColumnVector4::FGColumnVector4(void) FGColumnVector4::FGColumnVector4(double A, double B, double C, double D) { rowCtr = 1; - data[1]=0;data[2]=0;data[3]=0;data[4]=0; + data[1]=A; + data[2]=B; + data[3]=C; + data[4]=D; Debug(0); } diff --git a/src/FDM/JSBSim/FGColumnVector4.h b/src/FDM/JSBSim/FGColumnVector4.h index 347efb372..154945379 100644 --- a/src/FDM/JSBSim/FGColumnVector4.h +++ b/src/FDM/JSBSim/FGColumnVector4.h @@ -31,6 +31,7 @@ INCLUDES SG_USING_STD(cerr); SG_USING_STD(cout); SG_USING_STD(endl); + SG_USING_STD(sqrt); #else # include # if defined (sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740) @@ -50,6 +51,7 @@ INCLUDES using std::cerr; using std::cout; using std::endl; + using std::sqrt; # endif using std::string; #endif @@ -88,22 +90,22 @@ public: FGColumnVector4(double A, double B, double C, double D); FGColumnVector4(const FGColumnVector4& b); ~FGColumnVector4(void); - + FGColumnVector4 operator=(const FGColumnVector4& b); - + FGColumnVector4 operator*(const double scalar); FGColumnVector4 operator/(const double scalar); FGColumnVector4 operator+(const FGColumnVector4& B); // must not return reference FGColumnVector4 operator-(const FGColumnVector4& B); - + void operator-=(const FGColumnVector4 &B); void operator+=(const FGColumnVector4 &B); void operator*=(const double scalar); void operator/=(const double scalar); - + inline double operator()(int m) const { return data[m]; } inline double& operator()(int m) { return data[m]; } - + FGColumnVector4& operator<<(const double ff); inline void InitMatrix(void) { data[1]=0; data[2]=0; data[3]=0; data[4]=0; } diff --git a/src/FDM/JSBSim/FGFCS.cpp b/src/FDM/JSBSim/FGFCS.cpp index bf7778d4d..9cb89f909 100644 --- a/src/FDM/JSBSim/FGFCS.cpp +++ b/src/FDM/JSBSim/FGFCS.cpp @@ -1,52 +1,44 @@ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - + Module: FGFCS.cpp Author: Jon Berndt Date started: 12/12/98 Purpose: Model the flight controls Called by: FDMExec - + ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- - + 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. - + FUNCTIONAL DESCRIPTION -------------------------------------------------------------------------------- This class models the flight controls for a specific airplane - + HISTORY -------------------------------------------------------------------------------- 12/12/98 JSB Created - + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGFCS.h" -#include "FGState.h" #include "FGFDMExec.h" -#include "FGAtmosphere.h" -#include "FGAircraft.h" -#include "FGTranslation.h" -#include "FGRotation.h" -#include "FGPosition.h" -#include "FGAuxiliary.h" -#include "FGOutput.h" #include "FGPropertyManager.h" #include "filtersjb/FGFilter.h" @@ -82,7 +74,7 @@ FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex) LeftBrake = RightBrake = CenterBrake = 0.0; APAttitudeSetPt = APAltitudeSetPt = APHeadingSetPt = APAirspeedSetPt = 0.0; DoNormalize=true; - + eMode = mNone; bind(); @@ -90,7 +82,7 @@ FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex) DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0; DfPos[i] = DsbPos[i] = DspPos[i] = 0.0; } - + for (i=0;i 0) cout << " Control System Name: " << Name << endl; while ((token = AC_cfg->GetValue()) != string("/" + delimiter)) { @@ -382,14 +374,14 @@ bool FGFCS::Load(FGConfigFile* AC_cfg) string nodeName; for (i=0; isize(); i++) { - - if ( (((*Components)[i])->GetType() == "AEROSURFACE_SCALE" - || ((*Components)[i])->GetType() == "KINEMAT") - && ((*Components)[i])->GetOutputNode() ) { + + if ( (((*Components)[i])->GetType() == "AEROSURFACE_SCALE" + || ((*Components)[i])->GetType() == "KINEMAT") + && ((*Components)[i])->GetOutputNode() ) { nodeName = ((*Components)[i])->GetOutputNode()->GetName(); if ( nodeName == "elevator-pos-rad" ) { ToNormalize[iDe]=i; - } else if ( nodeName == "left-aileron-pos-rad" + } else if ( nodeName == "left-aileron-pos-rad" || nodeName == "aileron-pos-rad" ) { ToNormalize[iDaL]=i; } else if ( nodeName == "right-aileron-pos-rad" ) { @@ -404,8 +396,8 @@ bool FGFCS::Load(FGConfigFile* AC_cfg) ToNormalize[iDf]=i; } } - } - + } + if (delimiter == "FLIGHT_CONTROL") bindModel(); eMode = mNone; @@ -443,7 +435,7 @@ string FGFCS::GetComponentName(int idx) break; } return string(""); -} +} //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -526,19 +518,19 @@ void FGFCS::AddThrottle(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGFCS::Normalize(void) { - + //not all of these are guaranteed to be defined for every model //those that are have an index >=0 in the ToNormalize array //ToNormalize is filled in Load() - + if ( ToNormalize[iDe] > -1 ) { DePos[ofNorm] = FCSComponents[ToNormalize[iDe]]->GetOutputPct(); } - + if ( ToNormalize[iDaL] > -1 ) { DaLPos[ofNorm] = FCSComponents[ToNormalize[iDaL]]->GetOutputPct(); } - + if ( ToNormalize[iDaR] > -1 ) { DaRPos[ofNorm] = FCSComponents[ToNormalize[iDaR]]->GetOutputPct(); } @@ -546,19 +538,19 @@ void FGFCS::Normalize(void) { if ( ToNormalize[iDr] > -1 ) { DrPos[ofNorm] = FCSComponents[ToNormalize[iDr]]->GetOutputPct(); } - - if ( ToNormalize[iDsb] > -1 ) { + + if ( ToNormalize[iDsb] > -1 ) { DsbPos[ofNorm] = FCSComponents[ToNormalize[iDsb]]->GetOutputPct(); } - + if ( ToNormalize[iDsp] > -1 ) { DspPos[ofNorm] = FCSComponents[ToNormalize[iDsp]]->GetOutputPct(); } - + if ( ToNormalize[iDf] > -1 ) { DfPos[ofNorm] = FCSComponents[ToNormalize[iDf]]->GetOutputPct(); } - + DePos[ofMag] = fabs(DePos[ofRad]); DaLPos[ofMag] = fabs(DaLPos[ofRad]); DaRPos[ofMag] = fabs(DaRPos[ofRad]); @@ -566,9 +558,9 @@ void FGFCS::Normalize(void) { DsbPos[ofMag] = fabs(DsbPos[ofRad]); DspPos[ofMag] = fabs(DspPos[ofRad]); DfPos[ofMag] = fabs(DfPos[ofRad]); - -} - + +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGFCS::bind(void) @@ -613,7 +605,7 @@ void FGFCS::bind(void) &FGFCS::GetGearCmd, &FGFCS::SetGearCmd, true); - + PropertyManager->Tie("fcs/left-aileron-pos-rad", this,ofRad, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos, @@ -626,7 +618,7 @@ void FGFCS::bind(void) &FGFCS::GetDaLPos, &FGFCS::SetDaLPos, true); - + PropertyManager->Tie("fcs/right-aileron-pos-rad", this,ofRad, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos, @@ -639,20 +631,20 @@ void FGFCS::bind(void) &FGFCS::GetDaRPos, &FGFCS::SetDaRPos, true); - + PropertyManager->Tie("fcs/elevator-pos-rad", this, ofRad, &FGFCS::GetDePos, &FGFCS::SetDePos, true ); PropertyManager->Tie("fcs/elevator-pos-norm", this,ofNorm, - &FGFCS::GetDePos, + &FGFCS::GetDePos, &FGFCS::SetDePos, true ); PropertyManager->Tie("fcs/mag-elevator-pos-rad", this,ofMag, &FGFCS::GetDePos, &FGFCS::SetDePos, true ); - + PropertyManager->Tie("fcs/rudder-pos-rad", this,ofRad, &FGFCS::GetDrPos, &FGFCS::SetDrPos, @@ -665,7 +657,7 @@ void FGFCS::bind(void) &FGFCS::GetDrPos, &FGFCS::SetDrPos, true); - + PropertyManager->Tie("fcs/flap-pos-deg", this,ofRad, &FGFCS::GetDfPos, &FGFCS::SetDfPos, @@ -674,7 +666,7 @@ void FGFCS::bind(void) &FGFCS::GetDfPos, &FGFCS::SetDfPos, true); - + PropertyManager->Tie("fcs/speedbrake-pos-rad", this,ofRad, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos, @@ -687,7 +679,7 @@ void FGFCS::bind(void) &FGFCS::GetDsbPos, &FGFCS::SetDsbPos, true); - + PropertyManager->Tie("fcs/spoiler-pos-rad", this,ofRad, &FGFCS::GetDspPos, &FGFCS::SetDspPos, @@ -700,7 +692,7 @@ void FGFCS::bind(void) &FGFCS::GetDspPos, &FGFCS::SetDspPos, true); - + PropertyManager->Tie("gear/gear-pos-norm", this, &FGFCS::GetGearPos, &FGFCS::SetGearPos, @@ -798,46 +790,46 @@ void FGFCS::bindModel(void) { unsigned i; char tmp[80]; - - + + for (i=0; iTie( tmp,this,i, &FGFCS::GetThrottleCmd, &FGFCS::SetThrottleCmd, true ); - snprintf(tmp,80,"fcs/throttle-pos-norm[%u]",i); + snprintf(tmp,80,"fcs/throttle-pos-norm[%u]",i); PropertyManager->Tie( tmp,this,i, &FGFCS::GetThrottlePos, &FGFCS::SetThrottlePos, true ); if ( MixtureCmd.size() > i ) { - snprintf(tmp,80,"fcs/mixture-cmd-norm[%u]",i); + snprintf(tmp,80,"fcs/mixture-cmd-norm[%u]",i); PropertyManager->Tie( tmp,this,i, &FGFCS::GetMixtureCmd, &FGFCS::SetMixtureCmd, true ); - snprintf(tmp,80,"fcs/mixture-pos-norm[%u]",i); + snprintf(tmp,80,"fcs/mixture-pos-norm[%u]",i); PropertyManager->Tie( tmp,this,i, &FGFCS::GetMixturePos, &FGFCS::SetMixturePos, true ); } if ( PropAdvanceCmd.size() > i ) { - snprintf(tmp,80,"fcs/advance-cmd-norm[%u]",i); + snprintf(tmp,80,"fcs/advance-cmd-norm[%u]",i); PropertyManager->Tie( tmp,this,i, &FGFCS::GetPropAdvanceCmd, &FGFCS::SetPropAdvanceCmd, true ); - snprintf(tmp,80,"fcs/advance-pos-norm[%u]",i); + snprintf(tmp,80,"fcs/advance-pos-norm[%u]",i); PropertyManager->Tie( tmp,this,i, &FGFCS::GetPropAdvance, &FGFCS::SetPropAdvance, true ); } } -} - +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGFCS::unbind(FGPropertyManager *node) @@ -848,8 +840,8 @@ void FGFCS::unbind(FGPropertyManager *node) unbind( (FGPropertyManager*)node->getChild(i) ); } else if( node->getChild(i)->isTied() ) { node->getChild(i)->untie(); - } - } + } + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/FGForce.cpp b/src/FDM/JSBSim/FGForce.cpp index c09a619ae..d134b325f 100644 --- a/src/FDM/JSBSim/FGForce.cpp +++ b/src/FDM/JSBSim/FGForce.cpp @@ -1,182 +1,180 @@ -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - Source: FGForce.cpp - Author: Tony Peden - Date started: 6/10/00 - - ------------- 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 --------------------------------------------------------------------------------- -6/10/00 TP Created - - -FUNCTIONAL DESCRIPTION --------------------------------------------------------------------------------- - -The purpose of this class is to provide storage for computed forces and -encapsulate all the functionality associated with transforming those -forces from their native coord system to the body system. This includes -computing the moments due to the difference between the point of application -and the cg. - -*/ - -#include "FGFDMExec.h" -#include "FGAircraft.h" -#include "FGTranslation.h" -#include "FGMatrix33.h" -#include "FGColumnVector3.h" -#include "FGColumnVector4.h" -#include "FGForce.h" - -namespace JSBSim { - -static const char *IdSrc = "$Id$"; -static const char *IdHdr = ID_FORCE; - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGForce::FGForce(FGFDMExec *FDMExec) : - ttype(tNone), - fdmex(FDMExec) -{ - mT(1,1) = 1; //identity matrix - mT(2,2) = 1; - mT(3,3) = 1; - vSense.InitMatrix(1); - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGForce::~FGForce() -{ - Debug(1); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3& FGForce::GetBodyForces(void) -{ - vFb = Transform()*(vFn.multElementWise(vSense)); - - // Find the distance from this vector's acting location to the cg; this - // needs to be done like this to convert from structural to body coords. - // CG and RP values are in inches - - vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn); - - vM = vMn + vDXYZ*vFb; - - return vFb; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33 FGForce::Transform(void) -{ - switch(ttype) { - case tWindBody: - return fdmex->GetState()->GetTs2b(); - case tLocalBody: - return fdmex->GetState()->GetTl2b(); - case tCustom: - - case tNone: - return mT; - default: - cout << "Unrecognized tranform requested from FGForce::Transform()" << endl; - exit(1); - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw) -{ - if (ttype == tCustom) { - double cp,sp,cr,sr,cy,sy; - - cp=cos(bpitch); sp=sin(bpitch); - cr=cos(broll); sr=sin(broll); - cy=cos(byaw); sy=sin(byaw); - - mT(1,1)=cp*cy; - mT(1,2)=cp*sy; - mT(1,3)=-1*sp; - - mT(2,1)=sr*sp*cy-cr*sy; - mT(2,2)=sr*sp*sy+cr*cy; - mT(2,3)=sr*cp; - - mT(3,1)=cr*sp*cy+sr*sy; - mT(3,2)=cr*sp*sy-sr*cy; - mT(3,3)=cr*cp; - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// The bitmasked value choices are as follows: -// unset: In this case (the default) JSBSim would only print -// out the normally expected messages, essentially echoing -// the config files as they are read. If the environment -// variable is not set, debug_lvl is set to 1 internally -// 0: This requests JSBSim not to output any messages -// whatsoever. -// 1: This value explicity requests the normal JSBSim -// startup messages -// 2: This value asks for a message to be printed out when -// a class is instantiated -// 4: When this value is set, a message is displayed when a -// FGModel object executes its Run() method -// 8: When this value is set, various runtime state variables -// are printed out periodically -// 16: When set various parameters are sanity checked and -// a message is printed out when they go out of bounds - -void FGForce::Debug(int from) -{ - if (debug_lvl <= 0) return; - - if (debug_lvl & 1) { // Standard console startup message output - if (from == 0) { // Constructor - - } - } - if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGForce" << endl; - if (from == 1) cout << "Destroyed: FGForce" << endl; - } - if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects - } - if (debug_lvl & 8 ) { // Runtime state variables - } - if (debug_lvl & 16) { // Sanity checking - } - if (debug_lvl & 64) { - if (from == 0) { // Constructor - cout << IdSrc << endl; - cout << IdHdr << endl; - } - } -} -} +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Source: FGForce.cpp + Author: Tony Peden + Date started: 6/10/00 + + ------------- 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 +-------------------------------------------------------------------------------- +6/10/00 TP Created + + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- + +The purpose of this class is to provide storage for computed forces and +encapsulate all the functionality associated with transforming those +forces from their native coord system to the body system. This includes +computing the moments due to the difference between the point of application +and the cg. + +*/ + +#include "FGFDMExec.h" +#include "FGAircraft.h" +#include "FGTranslation.h" +#include "FGMassBalance.h" +#include "FGState.h" +#include "FGForce.h" + +namespace JSBSim { + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_FORCE; + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGForce::FGForce(FGFDMExec *FDMExec) : + ttype(tNone), + fdmex(FDMExec) +{ + mT(1,1) = 1; //identity matrix + mT(2,2) = 1; + mT(3,3) = 1; + vSense.InitMatrix(1); + Debug(0); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGForce::~FGForce() +{ + Debug(1); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3& FGForce::GetBodyForces(void) +{ + vFb = Transform()*(vFn.multElementWise(vSense)); + + // Find the distance from this vector's acting location to the cg; this + // needs to be done like this to convert from structural to body coords. + // CG and RP values are in inches + + vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn); + + vM = vMn + vDXYZ*vFb; + + return vFb; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33 FGForce::Transform(void) +{ + switch(ttype) { + case tWindBody: + return fdmex->GetState()->GetTs2b(); + case tLocalBody: + return fdmex->GetState()->GetTl2b(); + case tCustom: + case tNone: + return mT; + default: + cout << "Unrecognized tranform requested from FGForce::Transform()" << endl; + exit(1); + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw) +{ + if (ttype == tCustom) { + double cp,sp,cr,sr,cy,sy; + + cp=cos(bpitch); sp=sin(bpitch); + cr=cos(broll); sr=sin(broll); + cy=cos(byaw); sy=sin(byaw); + + mT(1,1)=cp*cy; + mT(1,2)=cp*sy; + mT(1,3)=-1*sp; + + mT(2,1)=sr*sp*cy-cr*sy; + mT(2,2)=sr*sp*sy+cr*cy; + mT(2,3)=sr*cp; + + mT(3,1)=cr*sp*cy+sr*sy; + mT(3,2)=cr*sp*sy-sr*cy; + mT(3,3)=cr*cp; + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// The bitmasked value choices are as follows: +// unset: In this case (the default) JSBSim would only print +// out the normally expected messages, essentially echoing +// the config files as they are read. If the environment +// variable is not set, debug_lvl is set to 1 internally +// 0: This requests JSBSim not to output any messages +// whatsoever. +// 1: This value explicity requests the normal JSBSim +// startup messages +// 2: This value asks for a message to be printed out when +// a class is instantiated +// 4: When this value is set, a message is displayed when a +// FGModel object executes its Run() method +// 8: When this value is set, various runtime state variables +// are printed out periodically +// 16: When set various parameters are sanity checked and +// a message is printed out when they go out of bounds + +void FGForce::Debug(int from) +{ + if (debug_lvl <= 0) return; + + if (debug_lvl & 1) { // Standard console startup message output + if (from == 0) { // Constructor + + } + } + if (debug_lvl & 2 ) { // Instantiation/Destruction notification + if (from == 0) cout << "Instantiated: FGForce" << endl; + if (from == 1) cout << "Destroyed: FGForce" << endl; + } + if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects + } + if (debug_lvl & 8 ) { // Runtime state variables + } + if (debug_lvl & 16) { // Sanity checking + } + if (debug_lvl & 64) { + if (from == 0) { // Constructor + cout << IdSrc << endl; + cout << IdHdr << endl; + } + } +} +} diff --git a/src/FDM/JSBSim/FGForce.h b/src/FDM/JSBSim/FGForce.h index 40911af86..06d587bf3 100644 --- a/src/FDM/JSBSim/FGForce.h +++ b/src/FDM/JSBSim/FGForce.h @@ -61,7 +61,6 @@ INCLUDES #include "FGJSBBase.h" #include "FGMatrix33.h" #include "FGColumnVector3.h" -#include "FGColumnVector4.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS diff --git a/src/FDM/JSBSim/FGGroundReactions.h b/src/FDM/JSBSim/FGGroundReactions.h index 1a34dae3b..db75cf462 100644 --- a/src/FDM/JSBSim/FGGroundReactions.h +++ b/src/FDM/JSBSim/FGGroundReactions.h @@ -52,8 +52,7 @@ INCLUDES #include "FGModel.h" #include "FGConfigFile.h" #include "FGLGear.h" -#include "FGInertial.h" -#include "FGMatrix33.h" +#include "FGColumnVector3.h" #define ID_GROUNDREACTIONS "$Id$" diff --git a/src/FDM/JSBSim/FGInertial.cpp b/src/FDM/JSBSim/FGInertial.cpp index 53516939f..29b489320 100644 --- a/src/FDM/JSBSim/FGInertial.cpp +++ b/src/FDM/JSBSim/FGInertial.cpp @@ -37,6 +37,7 @@ INCLUDES #include "FGInertial.h" #include "FGPosition.h" +#include "FGState.h" #include "FGMassBalance.h" namespace JSBSim { @@ -63,7 +64,7 @@ FGInertial::FGInertial(FGFDMExec* fgex) : FGModel(fgex) vCoriolis.InitMatrix(); vCentrifugal.InitMatrix(); vGravity.InitMatrix(); - + bind(); Debug(0); @@ -110,8 +111,9 @@ bool FGInertial::Run(void) vRadius(eDown) = Position->GetRadius(); vCentrifugal(eDown) = -vOmegaLocal.Magnitude() * vOmegaLocal.Magnitude() * vRadius(eDown); - vForces = State->GetTl2b() * MassBalance->GetMass() * (vCoriolis + vCentrifugal + vGravity); - +// vForces = State->GetTl2b() * MassBalance->GetMass() * (vCoriolis + vCentrifugal + vGravity); + vForces = State->GetTl2b() * MassBalance->GetMass() * vGravity; + return false; } else { return true; diff --git a/src/FDM/JSBSim/FGInertial.h b/src/FDM/JSBSim/FGInertial.h index 93073690c..e32d982bd 100644 --- a/src/FDM/JSBSim/FGInertial.h +++ b/src/FDM/JSBSim/FGInertial.h @@ -51,9 +51,7 @@ INCLUDES #include "FGModel.h" #include "FGConfigFile.h" -#include "FGMatrix33.h" #include "FGColumnVector3.h" -#include "FGColumnVector4.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS diff --git a/src/FDM/JSBSim/FGInitialCondition.cpp b/src/FDM/JSBSim/FGInitialCondition.cpp index 16a912192..9d3e19ec0 100644 --- a/src/FDM/JSBSim/FGInitialCondition.cpp +++ b/src/FDM/JSBSim/FGInitialCondition.cpp @@ -1,59 +1,53 @@ /******************************************************************************* - + Header: FGInitialCondition.cpp Author: Tony Peden Date started: 7/1/99 - + ------------- 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 -------------------------------------------------------------------------------- 7/1/99 TP Created - - + + FUNCTIONAL DESCRIPTION -------------------------------------------------------------------------------- - + The purpose of this class is to take a set of initial conditions and provide a kinematically consistent set of body axis velocity components, euler angles, and altitude. This class does not attempt to trim the model i.e. the sim will most likely start in a very dynamic state (unless, of course, you have chosen your IC's wisely) even after setting it up with this class. - + ******************************************************************************** INCLUDES *******************************************************************************/ #include "FGInitialCondition.h" #include "FGFDMExec.h" -#include "FGState.h" +#include "FGInertial.h" #include "FGAtmosphere.h" #include "FGAerodynamics.h" -#include "FGFCS.h" -#include "FGAircraft.h" -#include "FGTranslation.h" -#include "FGRotation.h" #include "FGPosition.h" -#include "FGAuxiliary.h" -#include "FGOutput.h" #include "FGConfigFile.h" #include "FGPropertyManager.h" @@ -96,7 +90,7 @@ FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) } else { cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl; } - + Debug(0); } @@ -226,7 +220,7 @@ void FGInitialCondition::SetBetaRadIC(double tt) { beta=tt; sbeta=sin(beta); cbeta=cos(beta); getTheta(); - + } //****************************************************************************** @@ -285,7 +279,7 @@ double FGInitialCondition::GetVBodyFpsIC(void) { return v; else { return vt*sbeta - vw; - } + } } //****************************************************************************** @@ -293,7 +287,7 @@ double FGInitialCondition::GetVBodyFpsIC(void) { double FGInitialCondition::GetWBodyFpsIC(void) { if( lastSpeedSet == setvg ) return w; - else + else return vt*salpha*cbeta -ww; } @@ -310,41 +304,41 @@ void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD ) { //****************************************************************************** // positive from left -void FGInitialCondition::SetHeadWindKtsIC(double head){ +void FGInitialCondition::SetHeadWindKtsIC(double head){ whead=head*ktstofps; - lastWindSet=setwhc; + lastWindSet=setwhc; calcWindUVW(); if(lastSpeedSet == setvg) SetVgroundFpsIC(vg); -} +} //****************************************************************************** -void FGInitialCondition::SetCrossWindKtsIC(double cross){ - wcross=cross*ktstofps; - lastWindSet=setwhc; +void FGInitialCondition::SetCrossWindKtsIC(double cross){ + wcross=cross*ktstofps; + lastWindSet=setwhc; calcWindUVW(); if(lastSpeedSet == setvg) SetVgroundFpsIC(vg); -} +} //****************************************************************************** -void FGInitialCondition::SetWindDownKtsIC(double wD) { - wdown=wD; +void FGInitialCondition::SetWindDownKtsIC(double wD) { + wdown=wD; calcWindUVW(); if(lastSpeedSet == setvg) SetVgroundFpsIC(vg); -} +} //****************************************************************************** void FGInitialCondition::SetWindMagKtsIC(double mag) { wmag=mag*ktstofps; lastWindSet=setwmd; - calcWindUVW(); + calcWindUVW(); if(lastSpeedSet == setvg) SetVgroundFpsIC(vg); } @@ -354,7 +348,7 @@ void FGInitialCondition::SetWindMagKtsIC(double mag) { void FGInitialCondition::SetWindDirDegIC(double dir) { wdir=dir*degtorad; lastWindSet=setwmd; - calcWindUVW(); + calcWindUVW(); if(lastSpeedSet == setvg) SetVgroundFpsIC(vg); } @@ -363,7 +357,7 @@ void FGInitialCondition::SetWindDirDegIC(double dir) { //****************************************************************************** void FGInitialCondition::calcWindUVW(void) { - + switch(lastWindSet) { case setwmd: wnorth=wmag*cos(wdir); @@ -375,7 +369,7 @@ void FGInitialCondition::calcWindUVW(void) { break; case setwned: break; - } + } uw=wnorth*ctheta*cpsi + weast*ctheta*spsi - wdown*stheta; @@ -385,14 +379,14 @@ void FGInitialCondition::calcWindUVW(void) { ww=wnorth*(cphi*stheta*cpsi + sphi*spsi) + weast*(cphi*stheta*spsi - sphi*cpsi) + wdown*cphi*ctheta; - - + + /* cout << "FGInitialCondition::calcWindUVW: wnorth, weast, wdown " << wnorth << ", " << weast << ", " << wdown << endl; cout << "FGInitialCondition::calcWindUVW: theta, phi, psi " << theta << ", " << phi << ", " << psi << endl; cout << "FGInitialCondition::calcWindUVW: uw, vw, ww " - << uw << ", " << vw << ", " << ww << endl; */ + << uw << ", " << vw << ", " << ww << endl; */ } @@ -508,9 +502,9 @@ bool FGInitialCondition::getMachFromVcas(double *Mach,double vcas) { bool FGInitialCondition::getAlpha(void) { bool result=false; double guess=theta-gamma; - + if(vt < 0.01) return 0; - + xlo=xhi=0; xmin=fdmex->GetAerodynamics()->GetAlphaCLMin(); xmax=fdmex->GetAerodynamics()->GetAlphaCLMax(); @@ -531,9 +525,9 @@ bool FGInitialCondition::getAlpha(void) { bool FGInitialCondition::getTheta(void) { bool result=false; double guess=alpha+gamma; - + if(vt < 0.01) return 0; - + xlo=xhi=0; xmin=-89;xmax=89; sfunc=&FGInitialCondition::GammaEqOfTheta; @@ -679,7 +673,7 @@ bool FGInitialCondition::solve(double *y,double x) while ((fabs(d) > eps) && (i < 100)) { d=(x3-x1)/d0; x2 = x1-d*d0*f1/(f3-f1); - + f2=(this->*sfunc)(x2)-x; //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl; //cout << " " << f1 << "," << f2 << "," << f3 << endl; @@ -710,19 +704,19 @@ bool FGInitialCondition::solve(double *y,double x) //****************************************************************************** double FGInitialCondition::GetWindDirDegIC(void) { - if(weast != 0.0) + if(weast != 0.0) return atan2(weast,wnorth)*radtodeg; - else if(wnorth > 0) + else if(wnorth > 0) return 0.0; else return 180.0; -} +} //****************************************************************************** bool FGInitialCondition::Load(string rstfile, bool useStoredPath) { - string resetDef; + string resetDef, acpath; string token=""; double temp; @@ -730,20 +724,20 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath) string sep = "/"; # else string sep = ";"; - #endif - + #endif + if( useStoredPath ) { - string acpath = fdmex->GetAircraftPath() + sep + fdmex->GetModelName(); + acpath = fdmex->GetAircraftPath() + sep + fdmex->GetModelName(); resetDef = acpath + sep + rstfile + ".xml"; } else { resetDef = rstfile; - } - + } + FGConfigFile resetfile(resetDef); if (!resetfile.IsOpen()) { cerr << "Failed to open reset file: " << resetDef << endl; return false; - } + } resetfile.GetNextConfigLine(); token = resetfile.GetValue(); @@ -752,13 +746,13 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath) << " does not appear to be a reset file" << endl; return false; } - + resetfile.GetNextConfigLine(); resetfile >> token; while (token != string("/initialize") && token != string("EOF")) { - if (token == "UBODY" ) { resetfile >> temp; SetUBodyFpsIC(temp); } - if (token == "VBODY" ) { resetfile >> temp; SetVBodyFpsIC(temp); } - if (token == "WBODY" ) { resetfile >> temp; SetWBodyFpsIC(temp); } + if (token == "UBODY" ) { resetfile >> temp; SetUBodyFpsIC(temp); } + if (token == "VBODY" ) { resetfile >> temp; SetVBodyFpsIC(temp); } + if (token == "WBODY" ) { resetfile >> temp; SetWBodyFpsIC(temp); } if (token == "LATITUDE" ) { resetfile >> temp; SetLatitudeDegIC(temp); } if (token == "LONGITUDE" ) { resetfile >> temp; SetLongitudeDegIC(temp); } if (token == "PHI" ) { resetfile >> temp; SetRollAngleDegIC(temp); } @@ -780,7 +774,7 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath) } fdmex->RunIC(); - + return true; } @@ -966,7 +960,7 @@ void FGInitialCondition::unbind(void){ /* PropertyManager->Untie("ic/vw-dir-deg"); */ PropertyManager->Untie("ic/roc-fps"); - + /* PropertyManager->Untie("ic/u-fps"); PropertyManager->Untie("ic/v-fps"); PropertyManager->Untie("ic/w-fps"); */ diff --git a/src/FDM/JSBSim/FGInitialCondition.h b/src/FDM/JSBSim/FGInitialCondition.h index 25f2c43ff..fff5c3dcb 100644 --- a/src/FDM/JSBSim/FGInitialCondition.h +++ b/src/FDM/JSBSim/FGInitialCondition.h @@ -49,10 +49,7 @@ INCLUDES #include "FGFDMExec.h" #include "FGJSBBase.h" -#include "FGAtmosphere.h" -#include "FGMatrix33.h" #include "FGColumnVector3.h" -#include "FGColumnVector4.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS diff --git a/src/FDM/JSBSim/FGJSBBase.cpp b/src/FDM/JSBSim/FGJSBBase.cpp index 3c5b6f867..f3188b54e 100644 --- a/src/FDM/JSBSim/FGJSBBase.cpp +++ b/src/FDM/JSBSim/FGJSBBase.cpp @@ -1,189 +1,199 @@ -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - Module: FGJSBBase.cpp - Author: Jon S. Berndt - Date started: 07/01/01 - Purpose: Encapsulates the JSBBase object - - ------------- Copyright (C) 2001 Jon S. Berndt (jsb@hal-pc.org) ------------- - - 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. - -FUNCTIONAL DESCRIPTION --------------------------------------------------------------------------------- - -HISTORY --------------------------------------------------------------------------------- -07/01/01 JSB Created - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -INCLUDES -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -#include "FGJSBBase.h" - -namespace JSBSim { - -static const char *IdSrc = "$Id$"; -static const char *IdHdr = ID_JSBBASE; - -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -CLASS IMPLEMENTATION -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -char FGJSBBase::highint[5] = {27, '[', '1', 'm', '\0' }; -char FGJSBBase::halfint[5] = {27, '[', '2', 'm', '\0' }; -char FGJSBBase::normint[6] = {27, '[', '2', '2', 'm', '\0' }; -char FGJSBBase::reset[5] = {27, '[', '0', 'm', '\0' }; -char FGJSBBase::underon[5] = {27, '[', '4', 'm', '\0' }; -char FGJSBBase::underoff[6] = {27, '[', '2', '4', 'm', '\0' }; -char FGJSBBase::fgblue[6] = {27, '[', '3', '4', 'm', '\0' }; -char FGJSBBase::fgcyan[6] = {27, '[', '3', '6', 'm', '\0' }; -char FGJSBBase::fgred[6] = {27, '[', '3', '1', 'm', '\0' }; -char FGJSBBase::fggreen[6] = {27, '[', '3', '2', 'm', '\0' }; -char FGJSBBase::fgdef[6] = {27, '[', '3', '9', 'm', '\0' }; - -const double FGJSBBase::radtodeg = 57.29578; -const double FGJSBBase::degtorad = 1.745329E-2; -const double FGJSBBase::hptoftlbssec = 550.0; -const double FGJSBBase::psftoinhg = 0.014138; -const double FGJSBBase::fpstokts = 0.592484; -const double FGJSBBase::ktstofps = 1.68781; -const double FGJSBBase::inchtoft = 0.08333333; -const double FGJSBBase::in3tom3 = 1.638706E-5; -double FGJSBBase::Reng = 1716.0; -const double FGJSBBase::SHRatio = 1.40; -const string FGJSBBase::needed_cfg_version = "1.61"; -const string FGJSBBase::JSBSim_version = "0.9.5"; - -std::queue FGJSBBase::Messages; -FGJSBBase::Message FGJSBBase::localMsg; -unsigned int FGJSBBase::messageId = 0; -unsigned int FGJSBBase::frame = 0; - -short FGJSBBase::debug_lvl = 1; - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGJSBBase::FGJSBBase() -{ -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGJSBBase::Message* FGJSBBase::PutMessage(Message* msg) -{ - Messages.push(msg); - return msg; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGJSBBase::Message* FGJSBBase::PutMessage(string text) -{ - Message *msg = new Message(); - msg->text = text; - msg->messageId = messageId++; - msg->subsystem = "FDM"; - msg->type = Message::eText; - Messages.push(msg); - return msg; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGJSBBase::Message* FGJSBBase::PutMessage(string text, bool bVal) -{ - Message *msg = new Message(); - msg->text = text; - msg->messageId = messageId++; - msg->subsystem = "FDM"; - msg->type = Message::eBool; - msg->bVal = bVal; - Messages.push(msg); - return msg; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGJSBBase::Message* FGJSBBase::PutMessage(string text, int iVal) -{ - Message *msg = new Message(); - msg->text = text; - msg->messageId = messageId++; - msg->subsystem = "FDM"; - msg->type = Message::eInteger; - msg->bVal = (iVal != 0); - Messages.push(msg); - return msg; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGJSBBase::Message* FGJSBBase::PutMessage(string text, double dVal) -{ - Message *msg = new Message(); - msg->text = text; - msg->messageId = messageId++; - msg->subsystem = "FDM"; - msg->type = Message::eDouble; - msg->bVal = (dVal != 0.0); - Messages.push(msg); - return msg; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGJSBBase::Message* FGJSBBase::ReadMessage(void) -{ - if (!Messages.empty()) return Messages.front(); - else return NULL; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGJSBBase::Message* FGJSBBase::ProcessMessage(void) -{ - if (!Messages.empty()) - localMsg = *(Messages.front()); - else - return NULL; - Messages.pop(); - return &localMsg; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGJSBBase::disableHighLighting(void) { - highint[0]='\0'; - halfint[0]='\0'; - normint[0]='\0'; - reset[0]='\0'; - underon[0]='\0'; - underoff[0]='\0'; - fgblue[0]='\0'; - fgcyan[0]='\0'; - fgred[0]='\0'; - fggreen[0]='\0'; - fgdef[0]='\0'; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -} // namespace JSBSim - +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGJSBBase.cpp + Author: Jon S. Berndt + Date started: 07/01/01 + Purpose: Encapsulates the JSBBase object + + ------------- Copyright (C) 2001 Jon S. Berndt (jsb@hal-pc.org) ------------- + + 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. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- + +HISTORY +-------------------------------------------------------------------------------- +07/01/01 JSB Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGJSBBase.h" + +namespace JSBSim { + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_JSBBASE; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +char FGJSBBase::highint[5] = {27, '[', '1', 'm', '\0' }; +char FGJSBBase::halfint[5] = {27, '[', '2', 'm', '\0' }; +char FGJSBBase::normint[6] = {27, '[', '2', '2', 'm', '\0' }; +char FGJSBBase::reset[5] = {27, '[', '0', 'm', '\0' }; +char FGJSBBase::underon[5] = {27, '[', '4', 'm', '\0' }; +char FGJSBBase::underoff[6] = {27, '[', '2', '4', 'm', '\0' }; +char FGJSBBase::fgblue[6] = {27, '[', '3', '4', 'm', '\0' }; +char FGJSBBase::fgcyan[6] = {27, '[', '3', '6', 'm', '\0' }; +char FGJSBBase::fgred[6] = {27, '[', '3', '1', 'm', '\0' }; +char FGJSBBase::fggreen[6] = {27, '[', '3', '2', 'm', '\0' }; +char FGJSBBase::fgdef[6] = {27, '[', '3', '9', 'm', '\0' }; + +const double FGJSBBase::radtodeg = 57.29578; +const double FGJSBBase::degtorad = 1.745329E-2; +const double FGJSBBase::hptoftlbssec = 550.0; +const double FGJSBBase::psftoinhg = 0.014138; +const double FGJSBBase::fpstokts = 0.592484; +const double FGJSBBase::ktstofps = 1.68781; +const double FGJSBBase::inchtoft = 0.08333333; +const double FGJSBBase::in3tom3 = 1.638706E-5; +double FGJSBBase::Reng = 1716.0; +const double FGJSBBase::SHRatio = 1.40; + +// Note that definition of lbtoslug by the inverse of slugtolb and not +// to a different constant you can also get from some tables will make +// lbtoslug*slugtolb == 1 up to the magnitude of roundoff. So converting from +// slug to lb and back will yield to the original value you started with up +// to the magnitude of roundoff. +// Taken from units gnu commandline tool +const double FGJSBBase::slugtolb = 32.174049; +const double FGJSBBase::lbtoslug = 1.0/slugtolb; + +const string FGJSBBase::needed_cfg_version = "1.61"; +const string FGJSBBase::JSBSim_version = "0.9.5"; + +std::queue FGJSBBase::Messages; +FGJSBBase::Message FGJSBBase::localMsg; +unsigned int FGJSBBase::messageId = 0; +unsigned int FGJSBBase::frame = 0; + +short FGJSBBase::debug_lvl = 1; + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGJSBBase::FGJSBBase() +{ +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGJSBBase::Message* FGJSBBase::PutMessage(Message* msg) +{ + Messages.push(msg); + return msg; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGJSBBase::Message* FGJSBBase::PutMessage(string text) +{ + Message *msg = new Message(); + msg->text = text; + msg->messageId = messageId++; + msg->subsystem = "FDM"; + msg->type = Message::eText; + Messages.push(msg); + return msg; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGJSBBase::Message* FGJSBBase::PutMessage(string text, bool bVal) +{ + Message *msg = new Message(); + msg->text = text; + msg->messageId = messageId++; + msg->subsystem = "FDM"; + msg->type = Message::eBool; + msg->bVal = bVal; + Messages.push(msg); + return msg; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGJSBBase::Message* FGJSBBase::PutMessage(string text, int iVal) +{ + Message *msg = new Message(); + msg->text = text; + msg->messageId = messageId++; + msg->subsystem = "FDM"; + msg->type = Message::eInteger; + msg->bVal = (iVal != 0); + Messages.push(msg); + return msg; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGJSBBase::Message* FGJSBBase::PutMessage(string text, double dVal) +{ + Message *msg = new Message(); + msg->text = text; + msg->messageId = messageId++; + msg->subsystem = "FDM"; + msg->type = Message::eDouble; + msg->bVal = (dVal != 0.0); + Messages.push(msg); + return msg; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGJSBBase::Message* FGJSBBase::ReadMessage(void) +{ + if (!Messages.empty()) return Messages.front(); + else return NULL; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGJSBBase::Message* FGJSBBase::ProcessMessage(void) +{ + if (!Messages.empty()) + localMsg = *(Messages.front()); + else + return NULL; + Messages.pop(); + return &localMsg; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGJSBBase::disableHighLighting(void) { + highint[0]='\0'; + halfint[0]='\0'; + normint[0]='\0'; + reset[0]='\0'; + underon[0]='\0'; + underoff[0]='\0'; + fgblue[0]='\0'; + fgcyan[0]='\0'; + fgred[0]='\0'; + fggreen[0]='\0'; + fgdef[0]='\0'; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +} // namespace JSBSim + diff --git a/src/FDM/JSBSim/FGJSBBase.h b/src/FDM/JSBSim/FGJSBBase.h index 9f05b313f..78b842a85 100644 --- a/src/FDM/JSBSim/FGJSBBase.h +++ b/src/FDM/JSBSim/FGJSBBase.h @@ -240,6 +240,8 @@ protected: static const double in3tom3; static double Reng; // Specific Gas Constant,ft^2/(sec^2*R) static const double SHRatio; + static const double lbtoslug; + static const double slugtolb; static const string needed_cfg_version; static const string JSBSim_version; }; diff --git a/src/FDM/JSBSim/FGLGear.cpp b/src/FDM/JSBSim/FGLGear.cpp index 4bdb1f2fe..c9c418350 100644 --- a/src/FDM/JSBSim/FGLGear.cpp +++ b/src/FDM/JSBSim/FGLGear.cpp @@ -1,585 +1,586 @@ -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - Module: FGLGear.cpp - Author: Jon S. Berndt - Norman H. Princen - Date started: 11/18/99 - Purpose: Encapsulates the landing gear elements - Called by: FGAircraft - - ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- - - 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. - -FUNCTIONAL DESCRIPTION --------------------------------------------------------------------------------- - -HISTORY --------------------------------------------------------------------------------- -11/18/99 JSB Created -01/30/01 NHP Extended gear model to properly simulate steering and braking - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -INCLUDES -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -#include "FGLGear.h" -//#include - -namespace JSBSim { - -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -DEFINITIONS -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -GLOBAL DATA -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -static const char *IdSrc = "$Id$"; -static const char *IdHdr = ID_LGEAR; - -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -CLASS IMPLEMENTATION -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex) -{ - string tmp; - - *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3) - >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff - >> rollingFCoeff >> sSteerType >> sBrakeGroup - >> maxSteerAngle >> sRetractable; - - if (sBrakeGroup == "LEFT" ) eBrakeGrp = bgLeft; - else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight; - else if (sBrakeGroup == "CENTER") eBrakeGrp = bgCenter; - else if (sBrakeGroup == "NOSE" ) eBrakeGrp = bgNose; - else if (sBrakeGroup == "TAIL" ) eBrakeGrp = bgTail; - else if (sBrakeGroup == "NONE" ) eBrakeGrp = bgNone; - else { - cerr << "Improper braking group specification in config file: " - << sBrakeGroup << " is undefined." << endl; - } - - if (sSteerType == "STEERABLE") eSteerType = stSteer; - else if (sSteerType == "FIXED" ) eSteerType = stFixed; - else if (sSteerType == "CASTERED" ) eSteerType = stCaster; - else { - 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; - -// Add some AI here to determine if gear is located properly according to its -// brake group type ?? - - State = Exec->GetState(); - Aircraft = Exec->GetAircraft(); - Position = Exec->GetPosition(); - Rotation = Exec->GetRotation(); - FCS = Exec->GetFCS(); - MassBalance = Exec->GetMassBalance(); - - WOW = lastWOW = true; // should the value be initialized to true? - ReportEnable = true; - FirstContact = false; - StartedGroundRun = false; - TakeoffReported = LandingReported = false; - LandingDistanceTraveled = TakeoffDistanceTraveled = TakeoffDistanceTraveled50ft = 0.0; - MaximumStrutForce = MaximumStrutTravel = 0.0; - SinkRate = GroundSpeed = 0.0; - - vWhlBodyVec = MassBalance->StructuralToBody(vXYZ); - - vLocalGear = State->GetTb2l() * vWhlBodyVec; - - compressLength = 0.0; - compressSpeed = 0.0; - brakePct = 0.0; - maxCompLen = 0.0; - - WheelSlip = lastWheelSlip = 0.0; - - compressLength = 0.0; - compressSpeed = 0.0; - brakePct = 0.0; - maxCompLen = 0.0; - - TirePressureNorm = 1.0; - - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGLGear::FGLGear(const FGLGear& lgear) -{ - State = lgear.State; - Aircraft = lgear.Aircraft; - Position = lgear.Position; - Rotation = lgear.Rotation; - Exec = lgear.Exec; - FCS = lgear.FCS; - MassBalance = lgear.MassBalance; - - vXYZ = lgear.vXYZ; - vMoment = lgear.vMoment; - vWhlBodyVec = lgear.vWhlBodyVec; - vLocalGear = lgear.vLocalGear; - - WOW = lgear.WOW; - lastWOW = lgear.lastWOW; - ReportEnable = lgear.ReportEnable; - FirstContact = lgear.FirstContact; - StartedGroundRun = lgear.StartedGroundRun; - LandingDistanceTraveled = lgear.LandingDistanceTraveled; - TakeoffDistanceTraveled = lgear.TakeoffDistanceTraveled; - TakeoffDistanceTraveled50ft = lgear.TakeoffDistanceTraveled50ft; - MaximumStrutForce = lgear.MaximumStrutForce; - MaximumStrutTravel = lgear.MaximumStrutTravel; - - kSpring = lgear.kSpring; - bDamp = lgear.bDamp; - compressLength = lgear.compressLength; - compressSpeed = lgear.compressSpeed; - staticFCoeff = lgear.staticFCoeff; - dynamicFCoeff = lgear.dynamicFCoeff; - rollingFCoeff = lgear.rollingFCoeff; - brakePct = lgear.brakePct; - maxCompLen = lgear.maxCompLen; - SinkRate = lgear.SinkRate; - GroundSpeed = lgear.GroundSpeed; - LandingReported = lgear.LandingReported; - TakeoffReported = lgear.TakeoffReported; - name = lgear.name; - sSteerType = lgear.sSteerType; - sRetractable = lgear.sRetractable; - eSteerType = lgear.eSteerType; - sBrakeGroup = lgear.sBrakeGroup; - eBrakeGrp = lgear.eBrakeGrp; - maxSteerAngle = lgear.maxSteerAngle; - isRetractable = lgear.isRetractable; - GearUp = lgear.GearUp; - GearDown = lgear.GearDown; - WheelSlip = lgear.WheelSlip; - lastWheelSlip = lgear.lastWheelSlip; - TirePressureNorm = lgear.TirePressureNorm; - Servicable = lgear.Servicable; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGLGear::~FGLGear() -{ - Debug(1); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -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(); - - if (isRetractable) { - if (FCS->GetGearPos() < 0.01) { - GearUp = true; - GearDown = false; - } else if (FCS->GetGearPos() > 0.99) { - GearDown = true; - GearUp = false; - } else { - GearUp = false; - GearDown = false; - } - } else { - GearUp = false; - GearDown = true; - } - - if (GearDown) { - - vWhlBodyVec = MassBalance->StructuralToBody(vXYZ); - -// vWhlBodyVec now stores the vector from the cg to this wheel - - vLocalGear = State->GetTb2l() * vWhlBodyVec; - -// vLocalGear now stores the vector from the cg to the wheel in local coords. - - compressLength = vLocalGear(eZ) - Position->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 -// "hangs down" below the CG greater than the altitude, then the compressLength -// will be positive - i.e. the gear will have made contact. - - if (compressLength > 0.00) { - - WOW = true; // Weight-On-Wheels is true - -// The next equation should really use the vector to the contact patch of the tire -// including the strut compression and not vWhlBodyVec. Will fix this later. -// As it stands, now, the following equation takes the aircraft body-frame -// rotational rate and calculates the cross-product with the vector from the CG -// to the wheel, thus producing the instantaneous velocity vector of the tire -// in Body coords. The frame is also converted to local coordinates. When the -// aircraft local-frame velocity is added to this quantity, the total velocity of -// the wheel in local frame is then known. Subsequently, the compression speed -// (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(); - - compressSpeed = vWhlVelVec(eZ); - -// If this is the first time the wheel has made contact, remember some values -// for later printout. - - if (!FirstContact) { - FirstContact = true; - SinkRate = compressSpeed; - GroundSpeed = Position->GetVel().Magnitude(); - TakeoffReported = false; - } - -// If the takeoff run is starting, initialize. - - if ((Position->GetVel().Magnitude() > 0.1) && - (FCS->GetBrake(bgLeft) == 0) && - (FCS->GetBrake(bgRight) == 0) && - (FCS->GetThrottlePos(0) == 1) && !StartedGroundRun) - { - TakeoffDistanceTraveled = 0; - TakeoffDistanceTraveled50ft = 0; - StartedGroundRun = true; - } - -// The following needs work regarding friction coefficients and braking and -// steering The BrakeFCoeff formula assumes that an anti-skid system is used. -// It also assumes that we won't be turning and braking at the same time. -// Will fix this later. -// [JSB] The braking force coefficients include normal rolling coefficient + -// a percentage of the static friction coefficient based on braking applied. - - switch (eBrakeGrp) { - case bgLeft: - BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) + - staticFCoeff*FCS->GetBrake(bgLeft) ); - break; - case bgRight: - BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) + - staticFCoeff*FCS->GetBrake(bgRight) ); - break; - case bgCenter: - BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) + - staticFCoeff*FCS->GetBrake(bgCenter) ); - break; - case bgNose: - BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) + - staticFCoeff*FCS->GetBrake(bgCenter) ); - break; - case bgTail: - BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) + - staticFCoeff*FCS->GetBrake(bgCenter) ); - break; - case bgNone: - BrakeFCoeff = rollingFCoeff; - break; - default: - cerr << "Improper brake group membership detected for this gear." << endl; - 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); - RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel; - SideWhlVel = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel; - -// Calculate tire slip angle. - - 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; - } else { - WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel); - } -/* - if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) { - WheelSlip = 0.0; - } else if (RollingWhlVel < 1.0) { - WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel); - deltaSlip = WheelSlip - lastWheelSlip; - if (fabs(deltaSlip) > maxdeltaSlip) { - if (WheelSlip > lastWheelSlip) { - WheelSlip = lastWheelSlip + maxdeltaSlip; - } else if (WheelSlip < lastWheelSlip) { - WheelSlip = lastWheelSlip - maxdeltaSlip; - } - } - } else { - WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel); - } - - if ((WheelSlip < 0.0 && lastWheelSlip > 0.0) || - (WheelSlip > 0.0 && lastWheelSlip < 0.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. - - 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 + - staticFCoeff*(40.0 - fabs(WheelSlip))/20.0)*fabs(WheelSlip)/WheelSlip; - } else { - FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip; - } - -// 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 -// possibly give a "rebound damping factor" that differs from the compression -// case. - - vLocalForce(eZ) = min(-compressLength * kSpring - - compressSpeed * bDamp, (double)0.0); - - MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ))); - MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength)); - -// Compute the forces in the wheel ground plane. - - RollingForce = 0; - if (fabs(RollingWhlVel) > 1E-3) { - RollingForce = (1.0 - TirePressureNorm) * 30 - + vLocalForce(eZ) * BrakeFCoeff - * fabs(RollingWhlVel)/RollingWhlVel; - } - SideForce = vLocalForce(eZ) * FCoeff; - -// Transform these forces back to the local reference frame. - - vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel; - vLocalForce(eY) = SideForce*CosWheel + RollingForce*SinWheel; - -// Note to Jon: At this point the forces will be too big when the airplane is -// stopped or rolling to a stop. We need to make sure that the gear forces just -// balance out the non-gear forces when the airplane is stopped. That way the -// airplane won't start to accelerate until the non-gear/ forces are larger than -// the gear forces. I think that the proper fix should go into FGAircraft::FMGear. -// This routine would only compute the local strut forces and return them to -// FMGear. All of the gear forces would get adjusted in FMGear using the total -// non-gear forces. Then the gear moments would be calculated. If strange things -// start happening to the airplane during testing as it rolls to a stop, then we -// need to implement this change. I ran out of time to do it now but have the -// equations. - -// Transform the forces back to the body frame and compute the moment. - - vForce = State->GetTl2b() * vLocalForce; - vMoment = vWhlBodyVec * vForce; - - } else { // Gear is NOT compressed - - WOW = false; - - if (Position->GetDistanceAGL() > 200.0) { - FirstContact = false; - StartedGroundRun = false; - LandingReported = false; - LandingDistanceTraveled = 0.0; - MaximumStrutForce = MaximumStrutTravel = 0.0; - } - - compressLength = 0.0; // reset compressLength to zero for data output validity - } - - if (FirstContact) LandingDistanceTraveled += Position->GetVground()*deltaT; - - if (StartedGroundRun) { - TakeoffDistanceTraveled50ft += Position->GetVground()*deltaT; - if (WOW) TakeoffDistanceTraveled += Position->GetVground()*deltaT; - } - - if (ReportEnable && Position->GetVground() <= 0.05 && !LandingReported) { - if (debug_lvl > 0) Report(erLand); - } - - if (ReportEnable && !TakeoffReported && - (vLocalGear(eZ) - Position->GetDistanceAGL()) < -50.0) - { - if (debug_lvl > 0) Report(erTakeoff); - } - - if (lastWOW != WOW) { - PutMessage("GEAR_CONTACT: " + name, WOW); - } - - lastWOW = WOW; - -// Crash detection logic (really out-of-bounds detection) - - if (compressLength > 500.0 || - vForce.Magnitude() > 100000000.0 || - vMoment.Magnitude() > 5000000000.0 || - SinkRate > 1.4666*30) - { - PutMessage("Crash Detected: Simulation FREEZE."); - Exec->Freeze(); - } - } - return vForce; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGLGear::Report(ReportType repType) -{ - switch(repType) { - case erLand: - cout << endl << "Touchdown report for " << name << endl; - cout << " Sink rate at contact: " << SinkRate << " fps, " - << SinkRate*0.3048 << " mps" << endl; - cout << " Contact ground speed: " << GroundSpeed*.5925 << " knots, " - << GroundSpeed*0.3048 << " mps" << endl; - cout << " Maximum contact force: " << MaximumStrutForce << " lbs, " - << MaximumStrutForce*4.448 << " Newtons" << endl; - cout << " Maximum strut travel: " << MaximumStrutTravel*12.0 << " inches, " - << MaximumStrutTravel*30.48 << " cm" << endl; - cout << " Distance traveled: " << LandingDistanceTraveled << " ft, " - << LandingDistanceTraveled*0.3048 << " meters" << endl; - LandingReported = true; - break; - case erTakeoff: - cout << endl << "Takeoff report for " << name << endl; - cout << " Distance traveled: " << TakeoffDistanceTraveled - << " ft, " << TakeoffDistanceTraveled*0.3048 << " meters" << endl; - cout << " Distance traveled (over 50'): " << TakeoffDistanceTraveled50ft - << " ft, " << TakeoffDistanceTraveled50ft*0.3048 << " meters" << endl; - TakeoffReported = true; - break; - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// The bitmasked value choices are as follows: -// unset: In this case (the default) JSBSim would only print -// out the normally expected messages, essentially echoing -// the config files as they are read. If the environment -// variable is not set, debug_lvl is set to 1 internally -// 0: This requests JSBSim not to output any messages -// whatsoever. -// 1: This value explicity requests the normal JSBSim -// startup messages -// 2: This value asks for a message to be printed out when -// a class is instantiated -// 4: When this value is set, a message is displayed when a -// FGModel object executes its Run() method -// 8: When this value is set, various runtime state variables -// are printed out periodically -// 16: When set various parameters are sanity checked and -// a message is printed out when they go out of bounds - -void FGLGear::Debug(int from) -{ - if (debug_lvl <= 0) return; - - if (debug_lvl & 1) { // Standard console startup message output - if (from == 0) { // Constructor - cout << " Name: " << name << endl; - cout << " Location: " << vXYZ << endl; - cout << " Spring Constant: " << kSpring << endl; - cout << " Damping Constant: " << bDamp << endl; - cout << " Dynamic Friction: " << dynamicFCoeff << endl; - cout << " Static Friction: " << staticFCoeff << endl; - cout << " Rolling Friction: " << rollingFCoeff << endl; - cout << " Steering Type: " << sSteerType << endl; - cout << " Grouping: " << sBrakeGroup << endl; - cout << " Max Steer Angle: " << maxSteerAngle << endl; - cout << " Retractable: " << sRetractable << endl; - } - } - if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGLGear" << endl; - if (from == 1) cout << "Destroyed: FGLGear" << endl; - } - if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects - } - if (debug_lvl & 8 ) { // Runtime state variables - } - if (debug_lvl & 16) { // Sanity checking - } - if (debug_lvl & 64) { - if (from == 0) { // Constructor - cout << IdSrc << endl; - cout << IdHdr << endl; - } - } -} - -} // namespace JSBSim - +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGLGear.cpp + Author: Jon S. Berndt + Norman H. Princen + Date started: 11/18/99 + Purpose: Encapsulates the landing gear elements + Called by: FGAircraft + + ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- + + 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. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- + +HISTORY +-------------------------------------------------------------------------------- +11/18/99 JSB Created +01/30/01 NHP Extended gear model to properly simulate steering and braking + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGLGear.h" + +namespace JSBSim { + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +GLOBAL DATA +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_LGEAR; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex) +{ + string tmp; + + *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3) + >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff + >> rollingFCoeff >> sSteerType >> sBrakeGroup + >> maxSteerAngle >> sRetractable; + + if (sBrakeGroup == "LEFT" ) eBrakeGrp = bgLeft; + else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight; + else if (sBrakeGroup == "CENTER") eBrakeGrp = bgCenter; + else if (sBrakeGroup == "NOSE" ) eBrakeGrp = bgNose; + else if (sBrakeGroup == "TAIL" ) eBrakeGrp = bgTail; + else if (sBrakeGroup == "NONE" ) eBrakeGrp = bgNone; + else { + cerr << "Improper braking group specification in config file: " + << sBrakeGroup << " is undefined." << endl; + } + + if (sSteerType == "STEERABLE") eSteerType = stSteer; + else if (sSteerType == "FIXED" ) eSteerType = stFixed; + else if (sSteerType == "CASTERED" ) eSteerType = stCaster; + else { + 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; + +// Add some AI here to determine if gear is located properly according to its +// brake group type ?? + + State = Exec->GetState(); + Aircraft = Exec->GetAircraft(); + Position = Exec->GetPosition(); + Rotation = Exec->GetRotation(); + FCS = Exec->GetFCS(); + MassBalance = Exec->GetMassBalance(); + + WOW = lastWOW = true; // should the value be initialized to true? + ReportEnable = true; + FirstContact = false; + StartedGroundRun = false; + TakeoffReported = LandingReported = false; + LandingDistanceTraveled = TakeoffDistanceTraveled = TakeoffDistanceTraveled50ft = 0.0; + MaximumStrutForce = MaximumStrutTravel = 0.0; + SideForce = RollingForce = 0.0; + SinkRate = GroundSpeed = 0.0; + + vWhlBodyVec = MassBalance->StructuralToBody(vXYZ); + + vLocalGear = State->GetTb2l() * vWhlBodyVec; + + compressLength = 0.0; + compressSpeed = 0.0; + brakePct = 0.0; + maxCompLen = 0.0; + + WheelSlip = lastWheelSlip = 0.0; + + compressLength = 0.0; + compressSpeed = 0.0; + brakePct = 0.0; + maxCompLen = 0.0; + + TirePressureNorm = 1.0; + + Debug(0); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGLGear::FGLGear(const FGLGear& lgear) +{ + State = lgear.State; + Aircraft = lgear.Aircraft; + Position = lgear.Position; + Rotation = lgear.Rotation; + Exec = lgear.Exec; + FCS = lgear.FCS; + MassBalance = lgear.MassBalance; + + vXYZ = lgear.vXYZ; + vMoment = lgear.vMoment; + vWhlBodyVec = lgear.vWhlBodyVec; + vLocalGear = lgear.vLocalGear; + + WOW = lgear.WOW; + lastWOW = lgear.lastWOW; + ReportEnable = lgear.ReportEnable; + FirstContact = lgear.FirstContact; + StartedGroundRun = lgear.StartedGroundRun; + LandingDistanceTraveled = lgear.LandingDistanceTraveled; + TakeoffDistanceTraveled = lgear.TakeoffDistanceTraveled; + TakeoffDistanceTraveled50ft = lgear.TakeoffDistanceTraveled50ft; + MaximumStrutForce = lgear.MaximumStrutForce; + MaximumStrutTravel = lgear.MaximumStrutTravel; + SideForce = lgear.SideForce; + RollingForce = lgear.RollingForce; + + kSpring = lgear.kSpring; + bDamp = lgear.bDamp; + compressLength = lgear.compressLength; + compressSpeed = lgear.compressSpeed; + staticFCoeff = lgear.staticFCoeff; + dynamicFCoeff = lgear.dynamicFCoeff; + rollingFCoeff = lgear.rollingFCoeff; + brakePct = lgear.brakePct; + maxCompLen = lgear.maxCompLen; + SinkRate = lgear.SinkRate; + GroundSpeed = lgear.GroundSpeed; + LandingReported = lgear.LandingReported; + TakeoffReported = lgear.TakeoffReported; + name = lgear.name; + sSteerType = lgear.sSteerType; + sRetractable = lgear.sRetractable; + eSteerType = lgear.eSteerType; + sBrakeGroup = lgear.sBrakeGroup; + eBrakeGrp = lgear.eBrakeGrp; + maxSteerAngle = lgear.maxSteerAngle; + isRetractable = lgear.isRetractable; + GearUp = lgear.GearUp; + GearDown = lgear.GearDown; + WheelSlip = lgear.WheelSlip; + lastWheelSlip = lgear.lastWheelSlip; + TirePressureNorm = lgear.TirePressureNorm; + Servicable = lgear.Servicable; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGLGear::~FGLGear() +{ + Debug(1); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3& FGLGear::Force(void) +{ + double SteerGain = 0; + double SinWheel, CosWheel; + double deltaSlip; + double deltaT = State->Getdt()*Aircraft->GetRate(); + + vForce.InitMatrix(); + vMoment.InitMatrix(); + + if (isRetractable) { + if (FCS->GetGearPos() < 0.01) { + GearUp = true; + GearDown = false; + } else if (FCS->GetGearPos() > 0.99) { + GearDown = true; + GearUp = false; + } else { + GearUp = false; + GearDown = false; + } + } else { + GearUp = false; + GearDown = true; + } + + if (GearDown) { + + vWhlBodyVec = MassBalance->StructuralToBody(vXYZ); + +// vWhlBodyVec now stores the vector from the cg to this wheel + + vLocalGear = State->GetTb2l() * vWhlBodyVec; + +// vLocalGear now stores the vector from the cg to the wheel in local coords. + + compressLength = vLocalGear(eZ) - Position->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 +// "hangs down" below the CG greater than the altitude, then the compressLength +// will be positive - i.e. the gear will have made contact. + + if (compressLength > 0.00) { + + WOW = true; // Weight-On-Wheels is true + +// The next equation should really use the vector to the contact patch of the tire +// including the strut compression and not vWhlBodyVec. Will fix this later. +// As it stands, now, the following equation takes the aircraft body-frame +// rotational rate and calculates the cross-product with the vector from the CG +// to the wheel, thus producing the instantaneous velocity vector of the tire +// in Body coords. The frame is also converted to local coordinates. When the +// aircraft local-frame velocity is added to this quantity, the total velocity of +// the wheel in local frame is then known. Subsequently, the compression speed +// (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(); + compressSpeed = vWhlVelVec(eZ); + +// If this is the first time the wheel has made contact, remember some values +// for later printout. + + if (!FirstContact) { + FirstContact = true; + SinkRate = compressSpeed; + GroundSpeed = Position->GetVel().Magnitude(); + TakeoffReported = false; + } + +// If the takeoff run is starting, initialize. + + if ((Position->GetVel().Magnitude() > 0.1) && + (FCS->GetBrake(bgLeft) == 0) && + (FCS->GetBrake(bgRight) == 0) && + (FCS->GetThrottlePos(0) == 1) && !StartedGroundRun) + { + TakeoffDistanceTraveled = 0; + TakeoffDistanceTraveled50ft = 0; + StartedGroundRun = true; + } + +// The following needs work regarding friction coefficients and braking and +// steering The BrakeFCoeff formula assumes that an anti-skid system is used. +// It also assumes that we won't be turning and braking at the same time. +// Will fix this later. +// [JSB] The braking force coefficients include normal rolling coefficient + +// a percentage of the static friction coefficient based on braking applied. + + switch (eBrakeGrp) { + case bgLeft: + BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) + + staticFCoeff*FCS->GetBrake(bgLeft) ); + break; + case bgRight: + BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) + + staticFCoeff*FCS->GetBrake(bgRight) ); + break; + case bgCenter: + BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) + + staticFCoeff*FCS->GetBrake(bgCenter) ); + break; + case bgNose: + BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) + + staticFCoeff*FCS->GetBrake(bgCenter) ); + break; + case bgTail: + BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) + + staticFCoeff*FCS->GetBrake(bgCenter) ); + break; + case bgNone: + BrakeFCoeff = rollingFCoeff; + break; + default: + cerr << "Improper brake group membership detected for this gear." << endl; + 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); + RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel; + SideWhlVel = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel; + +// Calculate tire slip angle. + + 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; + } else { + WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel); + } +/* + double maxdeltaSlip = 0.5*deltaT; + + if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) { + WheelSlip = 0.0; + } else if (RollingWhlVel < 1.0) { + WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel); + deltaSlip = WheelSlip - lastWheelSlip; + if (fabs(deltaSlip) > maxdeltaSlip) { + if (WheelSlip > lastWheelSlip) { + WheelSlip = lastWheelSlip + maxdeltaSlip; + } else if (WheelSlip < lastWheelSlip) { + WheelSlip = lastWheelSlip - maxdeltaSlip; + } + } + } else { + WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel); + } + + if ((WheelSlip < 0.0 && lastWheelSlip > 0.0) || + (WheelSlip > 0.0 && lastWheelSlip < 0.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. + + 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 + + staticFCoeff*(40.0 - fabs(WheelSlip))/20.0)*fabs(WheelSlip)/WheelSlip; + } else { + FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip; + } + +// 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 +// possibly give a "rebound damping factor" that differs from the compression +// case. + + vLocalForce(eZ) = min(-compressLength * kSpring + - compressSpeed * bDamp, (double)0.0); + + MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ))); + MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength)); + +// Compute the forces in the wheel ground plane. + + RollingForce = 0; + if (fabs(RollingWhlVel) > 1E-3) { + RollingForce = (1.0 - TirePressureNorm) * 30 + + vLocalForce(eZ) * BrakeFCoeff + * fabs(RollingWhlVel)/RollingWhlVel; + } + SideForce = vLocalForce(eZ) * FCoeff; + +// Transform these forces back to the local reference frame. + + vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel; + vLocalForce(eY) = SideForce*CosWheel + RollingForce*SinWheel; + +// Note to Jon: At this point the forces will be too big when the airplane is +// stopped or rolling to a stop. We need to make sure that the gear forces just +// balance out the non-gear forces when the airplane is stopped. That way the +// airplane won't start to accelerate until the non-gear/ forces are larger than +// the gear forces. I think that the proper fix should go into FGAircraft::FMGear. +// This routine would only compute the local strut forces and return them to +// FMGear. All of the gear forces would get adjusted in FMGear using the total +// non-gear forces. Then the gear moments would be calculated. If strange things +// start happening to the airplane during testing as it rolls to a stop, then we +// need to implement this change. I ran out of time to do it now but have the +// equations. + +// Transform the forces back to the body frame and compute the moment. + + vForce = State->GetTl2b() * vLocalForce; + vMoment = vWhlBodyVec * vForce; + + } else { // Gear is NOT compressed + + WOW = false; + + if (Position->GetDistanceAGL() > 200.0) { + FirstContact = false; + StartedGroundRun = false; + LandingReported = false; + LandingDistanceTraveled = 0.0; + MaximumStrutForce = MaximumStrutTravel = 0.0; + } + + compressLength = 0.0; // reset compressLength to zero for data output validity + } + + if (FirstContact) LandingDistanceTraveled += Position->GetVground()*deltaT; + + if (StartedGroundRun) { + TakeoffDistanceTraveled50ft += Position->GetVground()*deltaT; + if (WOW) TakeoffDistanceTraveled += Position->GetVground()*deltaT; + } + + if (ReportEnable && Position->GetVground() <= 0.05 && !LandingReported) { + if (debug_lvl > 0) Report(erLand); + } + + if (ReportEnable && !TakeoffReported && + (vLocalGear(eZ) - Position->GetDistanceAGL()) < -50.0) + { + if (debug_lvl > 0) Report(erTakeoff); + } + + if (lastWOW != WOW) { + PutMessage("GEAR_CONTACT: " + name, WOW); + } + + lastWOW = WOW; + +// Crash detection logic (really out-of-bounds detection) + + if (compressLength > 500.0 || + vForce.Magnitude() > 100000000.0 || + vMoment.Magnitude() > 5000000000.0 || + SinkRate > 1.4666*30) + { + PutMessage("Crash Detected: Simulation FREEZE."); + Exec->Freeze(); + } + } + return vForce; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGLGear::Report(ReportType repType) +{ + switch(repType) { + case erLand: + cout << endl << "Touchdown report for " << name << endl; + cout << " Sink rate at contact: " << SinkRate << " fps, " + << SinkRate*0.3048 << " mps" << endl; + cout << " Contact ground speed: " << GroundSpeed*.5925 << " knots, " + << GroundSpeed*0.3048 << " mps" << endl; + cout << " Maximum contact force: " << MaximumStrutForce << " lbs, " + << MaximumStrutForce*4.448 << " Newtons" << endl; + cout << " Maximum strut travel: " << MaximumStrutTravel*12.0 << " inches, " + << MaximumStrutTravel*30.48 << " cm" << endl; + cout << " Distance traveled: " << LandingDistanceTraveled << " ft, " + << LandingDistanceTraveled*0.3048 << " meters" << endl; + LandingReported = true; + break; + case erTakeoff: + cout << endl << "Takeoff report for " << name << endl; + cout << " Distance traveled: " << TakeoffDistanceTraveled + << " ft, " << TakeoffDistanceTraveled*0.3048 << " meters" << endl; + cout << " Distance traveled (over 50'): " << TakeoffDistanceTraveled50ft + << " ft, " << TakeoffDistanceTraveled50ft*0.3048 << " meters" << endl; + TakeoffReported = true; + break; + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// The bitmasked value choices are as follows: +// unset: In this case (the default) JSBSim would only print +// out the normally expected messages, essentially echoing +// the config files as they are read. If the environment +// variable is not set, debug_lvl is set to 1 internally +// 0: This requests JSBSim not to output any messages +// whatsoever. +// 1: This value explicity requests the normal JSBSim +// startup messages +// 2: This value asks for a message to be printed out when +// a class is instantiated +// 4: When this value is set, a message is displayed when a +// FGModel object executes its Run() method +// 8: When this value is set, various runtime state variables +// are printed out periodically +// 16: When set various parameters are sanity checked and +// a message is printed out when they go out of bounds + +void FGLGear::Debug(int from) +{ + if (debug_lvl <= 0) return; + + if (debug_lvl & 1) { // Standard console startup message output + if (from == 0) { // Constructor + cout << " Name: " << name << endl; + cout << " Location: " << vXYZ << endl; + cout << " Spring Constant: " << kSpring << endl; + cout << " Damping Constant: " << bDamp << endl; + cout << " Dynamic Friction: " << dynamicFCoeff << endl; + cout << " Static Friction: " << staticFCoeff << endl; + cout << " Rolling Friction: " << rollingFCoeff << endl; + cout << " Steering Type: " << sSteerType << endl; + cout << " Grouping: " << sBrakeGroup << endl; + cout << " Max Steer Angle: " << maxSteerAngle << endl; + cout << " Retractable: " << sRetractable << endl; + } + } + if (debug_lvl & 2 ) { // Instantiation/Destruction notification + if (from == 0) cout << "Instantiated: FGLGear" << endl; + if (from == 1) cout << "Destroyed: FGLGear" << endl; + } + if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects + } + if (debug_lvl & 8 ) { // Runtime state variables + } + if (debug_lvl & 16) { // Sanity checking + } + if (debug_lvl & 64) { + if (from == 0) { // Constructor + cout << IdSrc << endl; + cout << IdHdr << endl; + } + } +} + +} // namespace JSBSim + diff --git a/src/FDM/JSBSim/FGLGear.h b/src/FDM/JSBSim/FGLGear.h index 397892b85..ef13cf5e2 100644 --- a/src/FDM/JSBSim/FGLGear.h +++ b/src/FDM/JSBSim/FGLGear.h @@ -42,13 +42,11 @@ INCLUDES # include #endif +#include "FGJSBBase.h" +#include "FGFDMExec.h" #include #include "FGConfigFile.h" -#include "FGMatrix33.h" #include "FGColumnVector3.h" -#include "FGColumnVector4.h" -#include "FGFDMExec.h" -#include "FGJSBBase.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS diff --git a/src/FDM/JSBSim/FGMassBalance.cpp b/src/FDM/JSBSim/FGMassBalance.cpp index 582f1d957..f925c8fcc 100644 --- a/src/FDM/JSBSim/FGMassBalance.cpp +++ b/src/FDM/JSBSim/FGMassBalance.cpp @@ -39,6 +39,7 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGMassBalance.h" +#include "FGPropulsion.h" #include "FGPropertyManager.h" namespace JSBSim { @@ -55,11 +56,13 @@ FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex) { Name = "FGMassBalance"; Weight = EmptyWeight = Mass = 0.0; - Ixx = Iyy = Izz = Ixy = Ixz = 0.0; - baseIxx = baseIyy = baseIzz = baseIxy = baseIxz = 0.0; - vbaseXYZcg(eX) = 0.0; - vbaseXYZcg(eY) = 0.0; - vbaseXYZcg(eZ) = 0.0; + + vbaseXYZcg.InitMatrix(0.0); + baseJ.InitMatrix(); + mJ.InitMatrix(); + mJinv.InitMatrix(); + pmJ.InitMatrix(); + bind(); Debug(0); @@ -77,24 +80,54 @@ FGMassBalance::~FGMassBalance() bool FGMassBalance::Run(void) { + double denom, k1, k2, k3, k4, k5, k6; + double Ixx, Iyy, Izz, Ixy, Ixz, Iyz; + if (!FGModel::Run()) { Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetPointMassWeight(); - Mass = Weight / Inertial->gravity(); + Mass = lbtoslug*Weight; -// Calculate new CG here. +// Calculate new CG vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg + GetPointMassMoment() ) / Weight; -// Calculate new moments of inertia here - - Ixx = baseIxx + Propulsion->GetTanksIxx(vXYZcg) + GetPMIxx(); - Iyy = baseIyy + Propulsion->GetTanksIyy(vXYZcg) + GetPMIyy(); - Izz = baseIzz + Propulsion->GetTanksIzz(vXYZcg) + GetPMIzz(); - Ixy = baseIxy + Propulsion->GetTanksIxy(vXYZcg) + GetPMIxy(); - Ixz = baseIxz + Propulsion->GetTanksIxz(vXYZcg) + GetPMIxz(); +// Calculate new total moments of inertia + + // At first it is the base configuration inertia matrix ... + mJ = baseJ; + // ... with the additional term originating from the parallel axis theorem. + mJ += GetPointmassInertia( lbtoslug * EmptyWeight, vbaseXYZcg ); + // Then add the contributions from the additional pointmasses. + mJ += CalculatePMInertias(); + mJ += Propulsion->CalculateTankInertias(); + + Ixx = mJ(1,1); + Iyy = mJ(2,2); + Izz = mJ(3,3); + Ixy = -mJ(1,2); + Ixz = -mJ(1,3); + Iyz = -mJ(2,3); + +// Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation") + + k1 = (Iyy*Izz - Iyz*Iyz); + k2 = (Iyz*Ixz + Ixy*Izz); + k3 = (Ixy*Iyz + Iyy*Ixz); + + denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 ); + k1 = k1*denom; + k2 = k2*denom; + k3 = k3*denom; + k4 = (Izz*Ixx - Ixz*Ixz)*denom; + k5 = (Ixy*Ixz + Iyz*Ixx)*denom; + k6 = (Ixx*Iyy - Ixy*Ixy)*denom; + + mJinv.InitMatrix( k1, k2, k3, + k2, k4, k5, + k3, k5, k6 ); Debug(2); @@ -108,7 +141,7 @@ bool FGMassBalance::Run(void) void FGMassBalance::AddPointMass(double weight, double X, double Y, double Z) { - PointMassLoc.push_back(*(new FGColumnVector3(X, Y, Z))); + PointMassLoc.push_back(FGColumnVector3(X, Y, Z)); PointMassWeight.push_back(weight); } @@ -138,62 +171,19 @@ FGColumnVector3& FGMassBalance::GetPointMassMoment(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGMassBalance::GetPMIxx(void) +FGMatrix33& FGMassBalance::CalculatePMInertias(void) { - double I = 0.0; - for (unsigned int i=0; igravity()); - return I; -} + unsigned int size; -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + size = PointMassLoc.size(); + if (size == 0) return pmJ; -double FGMassBalance::GetPMIyy(void) -{ - double I = 0.0; - for (unsigned int i=0; igravity()); - return I; -} + pmJ = FGMatrix33(); -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + for (unsigned int i=0; igravity()); - return I; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -double FGMassBalance::GetPMIxy(void) -{ - double I = 0.0; - for (unsigned int i=0; igravity()); - return I; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -double FGMassBalance::GetPMIxz(void) -{ - double I = 0.0; - for (unsigned int i=0; igravity()); - return I; + return pmJ; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -232,22 +222,12 @@ FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGMassBalance::bind(void) -{ +{ typedef double (FGMassBalance::*PMF)(int) const; PropertyManager->Tie("inertia/mass-slugs", this, &FGMassBalance::GetMass); PropertyManager->Tie("inertia/weight-lbs", this, &FGMassBalance::GetWeight); - PropertyManager->Tie("inertia/ixx-lbsft2", this, - &FGMassBalance::GetIxx); - PropertyManager->Tie("inertia/iyy-lbsft2", this, - &FGMassBalance::GetIyy); - PropertyManager->Tie("inertia/izz-lbsft2", this, - &FGMassBalance::GetIzz); - PropertyManager->Tie("inertia/ixy-lbsft2", this, - &FGMassBalance::GetIxy); - PropertyManager->Tie("inertia/ixz-lbsft2", this, - &FGMassBalance::GetIxz); PropertyManager->Tie("inertia/cg-x-ft", this,1, (PMF)&FGMassBalance::GetXYZcg); PropertyManager->Tie("inertia/cg-y-ft", this,2, @@ -262,11 +242,6 @@ void FGMassBalance::unbind(void) { PropertyManager->Untie("inertia/mass-slugs"); PropertyManager->Untie("inertia/weight-lbs"); - PropertyManager->Untie("inertia/ixx-lbsft2"); - PropertyManager->Untie("inertia/iyy-lbsft2"); - PropertyManager->Untie("inertia/izz-lbsft2"); - PropertyManager->Untie("inertia/ixy-lbsft2"); - PropertyManager->Untie("inertia/ixz-lbsft2"); PropertyManager->Untie("inertia/cg-x-ft"); PropertyManager->Untie("inertia/cg-y-ft"); PropertyManager->Untie("inertia/cg-z-ft"); @@ -301,8 +276,8 @@ void FGMassBalance::Debug(int from) } } if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGPiston" << endl; - if (from == 1) cout << "Destroyed: FGPiston" << endl; + if (from == 0) cout << "Instantiated: FGMassBalance" << endl; + if (from == 1) cout << "Destroyed: FGMassBalance" << endl; } if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects } diff --git a/src/FDM/JSBSim/FGMassBalance.h b/src/FDM/JSBSim/FGMassBalance.h index c53288219..af6c5f597 100644 --- a/src/FDM/JSBSim/FGMassBalance.h +++ b/src/FDM/JSBSim/FGMassBalance.h @@ -39,7 +39,8 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGModel.h" -#include "FGPropulsion.h" +#include "FGColumnVector3.h" +#include "FGMatrix33.h" #include /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -76,39 +77,53 @@ public: inline double GetMass(void) const {return Mass;} inline double GetWeight(void) const {return Weight;} - inline double GetIxx(void) const {return Ixx;} - inline double GetIyy(void) const {return Iyy;} - inline double GetIzz(void) const {return Izz;} - inline double GetIxy(void) const {return Ixy;} - inline double GetIxz(void) const {return Ixz;} inline FGColumnVector3& GetXYZcg(void) {return vXYZcg;} inline double GetXYZcg(int axis) const {return vXYZcg(axis);} + /** Computes the inertia contribution of a pointmass. + Computes and returns the inertia matrix of a pointmass of mass + slugs at the given vector r in the structural frame. The units + should be for the mass in slug and the vector in the structural + frame as usual in inches. + @param slugs the mass of this single pointmass given in slugs + @param r the location of this single pointmass in the structural frame + */ + FGMatrix33 GetPointmassInertia(double slugs, const FGColumnVector3& r) const + { + FGColumnVector3 v = StructuralToBody( r ); + FGColumnVector3 sv = slugs*v; + double xx = sv(1)*v(1); + double yy = sv(2)*v(2); + double zz = sv(3)*v(3); + double xy = -sv(1)*v(2); + double xz = -sv(1)*v(3); + double yz = -sv(2)*v(3); + return FGMatrix33( yy+zz, xy, xz, + xy, xx+zz, yz, + xz, yz, xx+yy ); + } + /** Conversion from the structural frame to the body frame. - * Converts the argument \parm r given in the reference frame - * coordinate system to the body frame. The units of the structural - * frame are assumed to be in inches. The unit of the result is in - * ft. + Converts the location given in the structural frame + coordinate system to the body frame. The units of the structural + frame are assumed to be in inches. The unit of the result is in + ft. + @param r vector coordinate in the structural reference frame (X positive + aft, measurements in inches). + @return vector coordinate in the body frame, in feet. */ FGColumnVector3 StructuralToBody(const FGColumnVector3& r) const; inline void SetEmptyWeight(double EW) { EmptyWeight = EW;} - inline void SetBaseIxx(double bixx) { baseIxx = bixx;} - inline void SetBaseIyy(double biyy) { baseIyy = biyy;} - inline void SetBaseIzz(double bizz) { baseIzz = bizz;} - inline void SetBaseIxy(double bixy) { baseIxy = bixy;} - inline void SetBaseIxz(double bixz) { baseIxz = bixz;} - inline void SetBaseCG(const FGColumnVector3& CG) {vbaseXYZcg = CG;} - + inline void SetBaseCG(const FGColumnVector3& CG) {vbaseXYZcg = vXYZcg = CG;} + void AddPointMass(double weight, double X, double Y, double Z); double GetPointMassWeight(void); FGColumnVector3& GetPointMassMoment(void); - double GetPMIxx(void); - double GetPMIyy(void); - double GetPMIzz(void); - double GetPMIxy(void); - double GetPMIxz(void); - + FGMatrix33& GetJ(void) {return mJ;} + FGMatrix33& GetJinv(void) {return mJinv;} + void SetAircraftBaseInertias(FGMatrix33 BaseJ) {baseJ = BaseJ;} + void bind(void); void unbind(void); @@ -116,22 +131,19 @@ private: double Weight; double EmptyWeight; double Mass; - double Ixx; - double Iyy; - double Izz; - double Ixy; - double Ixz; - double baseIxx; - double baseIyy; - double baseIzz; - double baseIxy; - double baseIxz; + FGMatrix33 mJ; + FGMatrix33 mJinv; + FGMatrix33 pmJ; + FGMatrix33 baseJ; FGColumnVector3 vXYZcg; FGColumnVector3 vXYZtank; FGColumnVector3 vbaseXYZcg; + FGColumnVector3 vPMxyz; vector PointMassLoc; vector PointMassWeight; FGColumnVector3 PointMassCG; + FGMatrix33& CalculatePMInertias(void); + void Debug(int from); }; } diff --git a/src/FDM/JSBSim/FGMatrix33.cpp b/src/FDM/JSBSim/FGMatrix33.cpp index 65e046e48..ddd442b5d 100644 --- a/src/FDM/JSBSim/FGMatrix33.cpp +++ b/src/FDM/JSBSim/FGMatrix33.cpp @@ -1,445 +1,391 @@ -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -Module: FGMatrix33.cpp -Author: Originally by Tony Peden [formatted here (and broken??) by JSB] -Date started: 1998 -Purpose: FGMatrix33 class -Called by: Various - -FUNCTIONAL DESCRIPTION --------------------------------------------------------------------------------- - -HISTORY --------------------------------------------------------------------------------- -??/??/?? TP Created -03/16/2000 JSB Added exception throwing - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -INCLUDES -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -#include "FGMatrix33.h" -#include "FGColumnVector3.h" - -namespace JSBSim { - -static const char *IdSrc = "$Id$"; -static const char *IdHdr = ID_MATRIX33; - -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -CLASS IMPLEMENTATION -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33::FGMatrix33(void) -{ - InitMatrix(); - rowCtr = colCtr = 1; - - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33::FGMatrix33(int r, int c) -{ - InitMatrix(); - rowCtr = colCtr = 1; - - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33::FGMatrix33(const FGMatrix33& M) -{ - rowCtr = colCtr = 1; - - data[1][1] = M.data[1][1]; - data[1][2] = M.data[1][2]; - data[1][3] = M.data[1][3]; - data[2][1] = M.data[2][1]; - data[2][2] = M.data[2][2]; - data[2][3] = M.data[2][3]; - data[3][1] = M.data[3][1]; - data[3][2] = M.data[3][2]; - data[3][3] = M.data[3][3]; - - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33::~FGMatrix33(void) -{ - rowCtr = colCtr = 1; - - Debug(1); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -ostream& operator<<(ostream& os, const FGMatrix33& M) -{ - for (unsigned int i=1; i<=M.Rows(); i++) { - for (unsigned int j=1; j<=M.Cols(); j++) { - if (i == M.Rows() && j == M.Cols()) - os << M.data[i][j]; - else - os << M.data[i][j] << ", "; - } - } - return os; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33& FGMatrix33::operator<<(const double ff) -{ - data[rowCtr][colCtr] = ff; - if (++colCtr > Cols()) { - colCtr = 1; - if (++rowCtr > Rows()) - rowCtr = 1; - } - return *this; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -istream& operator>>(istream& is, FGMatrix33& M) -{ - for (unsigned int i=1; i<=M.Rows(); i++) { - for (unsigned int j=1; j<=M.Cols(); j++) { - is >> M.data[i][j]; - } - } - return is; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33& FGMatrix33::operator=(const FGMatrix33& M) -{ - if (&M != this) { - data[1][1] = M.data[1][1]; - data[1][2] = M.data[1][2]; - data[1][3] = M.data[1][3]; - data[2][1] = M.data[2][1]; - data[2][2] = M.data[2][2]; - data[2][3] = M.data[2][3]; - data[3][1] = M.data[3][1]; - data[3][2] = M.data[3][2]; - data[3][3] = M.data[3][3]; - } - return *this; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGMatrix33::InitMatrix(double value) -{ - if (data) { - data[1][1] = value; - data[1][2] = value; - data[1][3] = value; - data[2][1] = value; - data[2][2] = value; - data[2][3] = value; - data[3][1] = value; - data[3][2] = value; - data[3][3] = value; - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGMatrix33::InitMatrix(void) -{ - this->InitMatrix(0); -} - -// ***************************************************************************** -// binary operators ************************************************************ -// ***************************************************************************** - -FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) -{ - FGMatrix33 Diff; - - Diff(1,1) = data[1][1] - M(1,1); - Diff(1,2) = data[1][2] - M(1,2); - Diff(1,3) = data[1][3] - M(1,3); - Diff(2,1) = data[2][1] - M(2,1); - Diff(2,2) = data[2][2] - M(2,2); - Diff(2,3) = data[2][3] - M(2,3); - Diff(3,1) = data[3][1] - M(3,1); - Diff(3,2) = data[3][2] - M(3,2); - Diff(3,3) = data[3][3] - M(3,3); - - return Diff; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGMatrix33::operator-=(const FGMatrix33 &M) -{ - data[1][1] -= M(1,1); - data[1][2] -= M(1,2); - data[1][3] -= M(1,3); - data[2][1] -= M(2,1); - data[2][2] -= M(2,2); - data[2][3] -= M(2,3); - data[3][1] -= M(3,1); - data[3][2] -= M(3,2); - data[3][3] -= M(3,3); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) -{ - FGMatrix33 Sum; - - Sum(1,1) = data[1][1] + M(1,1); - Sum(1,2) = data[1][2] + M(1,2); - Sum(1,3) = data[1][3] + M(1,3); - Sum(2,1) = data[2][1] + M(2,1); - Sum(2,2) = data[2][2] + M(2,2); - Sum(2,3) = data[2][3] + M(2,3); - Sum(3,1) = data[3][1] + M(3,1); - Sum(3,2) = data[3][2] + M(3,2); - Sum(3,3) = data[3][3] + M(3,3); - - return Sum; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGMatrix33::operator+=(const FGMatrix33 &M) -{ - data[1][1] += M(1,1); - data[1][2] += M(1,2); - data[1][3] += M(1,3); - data[2][1] += M(2,1); - data[2][2] += M(2,2); - data[2][3] += M(2,3); - data[3][1] += M(3,1); - data[3][2] += M(3,2); - data[3][3] += M(3,3); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33 FGMatrix33::operator*(const double scalar) -{ - FGMatrix33 Product; - - Product(1,1) = data[1][1] * scalar; - Product(1,2) = data[1][2] * scalar; - Product(1,3) = data[1][3] * scalar; - Product(2,1) = data[2][1] * scalar; - Product(2,2) = data[2][2] * scalar; - Product(2,3) = data[2][3] * scalar; - Product(3,1) = data[3][1] * scalar; - Product(3,2) = data[3][2] * scalar; - Product(3,3) = data[3][3] * scalar; - - return Product; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33 operator*(double scalar, FGMatrix33 &M) -{ - FGMatrix33 Product; - - Product(1,1) = M(1,1) * scalar; - Product(1,2) = M(1,2) * scalar; - Product(1,3) = M(1,3) * scalar; - Product(2,1) = M(2,1) * scalar; - Product(2,2) = M(2,2) * scalar; - Product(2,3) = M(2,3) * scalar; - Product(3,1) = M(3,1) * scalar; - Product(3,2) = M(3,2) * scalar; - Product(3,3) = M(3,3) * scalar; - - return Product; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGMatrix33::operator*=(const double scalar) -{ - data[1][1] *= scalar; - data[1][2] *= scalar; - data[1][3] *= scalar; - data[2][1] *= scalar; - data[2][2] *= scalar; - data[2][3] *= scalar; - data[3][1] *= scalar; - data[3][2] *= scalar; - data[3][3] *= scalar; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) -{ - FGMatrix33 Product; - - Product(1,1) = data[1][1]*M(1,1) + data[1][2]*M(2,1) + data[1][3]*M(3,1); - Product(1,2) = data[1][1]*M(1,2) + data[1][2]*M(2,2) + data[1][3]*M(3,2); - Product(1,3) = data[1][1]*M(1,3) + data[1][2]*M(2,3) + data[1][3]*M(3,3); - Product(2,1) = data[2][1]*M(1,1) + data[2][2]*M(2,1) + data[2][3]*M(3,1); - Product(2,2) = data[2][1]*M(1,2) + data[2][2]*M(2,2) + data[2][3]*M(3,2); - Product(2,3) = data[2][1]*M(1,3) + data[2][2]*M(2,3) + data[2][3]*M(3,3); - Product(3,1) = data[3][1]*M(1,1) + data[3][2]*M(2,1) + data[3][3]*M(3,1); - Product(3,2) = data[3][1]*M(1,2) + data[3][2]*M(2,2) + data[3][3]*M(3,2); - Product(3,3) = data[3][1]*M(1,3) + data[3][2]*M(2,3) + data[3][3]*M(3,3); - - return Product; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGMatrix33::operator*=(const FGMatrix33& M) -{ - double a,b,c; - - a = data[1][1]; b=data[1][2]; c=data[1][3]; - data[1][1] = a*M(1,1) + b*M(2,1) + c*M(3,1); - data[1][2] = a*M(1,2) + b*M(2,2) + c*M(3,2); - data[1][3] = a*M(1,3) + b*M(2,3) + c*M(3,3); - - a = data[2][1]; b=data[2][2]; c=data[2][3]; - data[2][1] = a*M(1,1) + b*M(2,1) + c*M(3,1); - data[2][2] = a*M(1,2) + b*M(2,2) + c*M(3,2); - data[2][3] = a*M(1,3) + b*M(2,3) + c*M(3,3); - - a = data[3][1]; b=data[3][2]; c=data[3][3]; - data[3][1] = a*M(1,1) + b*M(2,1) + c*M(3,1); - data[3][2] = a*M(1,2) + b*M(2,2) + c*M(3,2); - data[3][3] = a*M(1,3) + b*M(2,3) + c*M(3,3); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGMatrix33 FGMatrix33::operator/(const double scalar) -{ - FGMatrix33 Quot; - - if ( scalar != 0 ) { - double tmp = 1.0/scalar; - Quot(1,1) = data[1][1] * tmp; - Quot(1,2) = data[1][2] * tmp; - Quot(1,3) = data[1][3] * tmp; - Quot(2,1) = data[2][1] * tmp; - Quot(2,2) = data[2][2] * tmp; - Quot(2,3) = data[2][3] * tmp; - Quot(3,1) = data[3][1] * tmp; - Quot(3,2) = data[3][2] * tmp; - Quot(3,3) = data[3][3] * tmp; - } else { - MatrixException mE; - mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)"; - throw mE; - } - return Quot; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGMatrix33::operator/=(const double scalar) -{ - if ( scalar != 0 ) { - double tmp = 1.0/scalar; - data[1][1] *= tmp; - data[1][2] *= tmp; - data[1][3] *= tmp; - data[2][1] *= tmp; - data[2][2] *= tmp; - data[2][3] *= tmp; - data[3][1] *= tmp; - data[3][2] *= tmp; - data[3][3] *= tmp; - } else { - MatrixException mE; - mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)"; - throw mE; - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGMatrix33::T(void) -{ - for (unsigned int i=1; i<=3; i++) { - for (unsigned int j=i+1; j<=3; j++) { - double tmp = data[i][j]; - data[i][j] = data[j][i]; - data[j][i] = tmp; - } - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& Col) -{ - FGColumnVector3 Product; - - Product(1) = data[1][1]*Col(1) + data[1][2]*Col(2) + data[1][3]*Col(3); - Product(2) = data[2][1]*Col(1) + data[2][2]*Col(2) + data[2][3]*Col(3); - Product(3) = data[3][1]*Col(1) + data[3][2]*Col(2) + data[3][3]*Col(3); - - return Product; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// The bitmasked value choices are as follows: -// unset: In this case (the default) JSBSim would only print -// out the normally expected messages, essentially echoing -// the config files as they are read. If the environment -// variable is not set, debug_lvl is set to 1 internally -// 0: This requests JSBSim not to output any messages -// whatsoever. -// 1: This value explicity requests the normal JSBSim -// startup messages -// 2: This value asks for a message to be printed out when -// a class is instantiated -// 4: When this value is set, a message is displayed when a -// FGModel object executes its Run() method -// 8: When this value is set, various runtime state variables -// are printed out periodically -// 16: When set various parameters are sanity checked and -// a message is printed out when they go out of bounds - -void FGMatrix33::Debug(int from) -{ - if (debug_lvl <= 0) return; - - if (debug_lvl & 1) { // Standard console startup message output - if (from == 0) { // Constructor - - } - } - if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGMatrix33" << endl; - if (from == 1) cout << "Destroyed: FGMatrix33" << endl; - } - if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects - } - if (debug_lvl & 8 ) { // Runtime state variables - } - if (debug_lvl & 16) { // Sanity checking - } - if (debug_lvl & 64) { - if (from == 0) { // Constructor - cout << IdSrc << endl; - cout << IdHdr << endl; - } - } -} -} +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +Module: FGMatrix33.cpp +Author: Tony Peden, Jon Berndt, Mathias Frolich +Date started: 1998 +Purpose: FGMatrix33 class +Called by: Various + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- + +HISTORY +-------------------------------------------------------------------------------- +??/??/?? TP Created +03/16/2000 JSB Added exception throwing + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGMatrix33.h" +#include "FGColumnVector3.h" + +namespace JSBSim { + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_MATRIX33; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33::FGMatrix33(void) +{ + data[0] = data[1] = data[2] = data[3] = data[4] = data[5] = + data[6] = data[7] = data[8] = 0.0; + + Debug(0); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +ostream& operator<<(ostream& os, const FGMatrix33& M) +{ + for (unsigned int i=1; i<=M.Rows(); i++) { + for (unsigned int j=1; j<=M.Cols(); j++) { + if (i == M.Rows() && j == M.Cols()) + os << M(i,j); + else + os << M(i,j) << ", "; + } + } + return os; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +istream& operator>>(istream& is, FGMatrix33& M) +{ + for (unsigned int i=1; i<=M.Rows(); i++) { + for (unsigned int j=1; j<=M.Cols(); j++) { + is >> M(i,j); + } + } + return is; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +double FGMatrix33::Determinant(void) const { + return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1) + + Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1) + - Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33 FGMatrix33::Inverse(void) const { + // Compute the inverse of a general matrix using Cramers rule. + // I guess googling for cramers rule gives tons of references + // for this. :) + double rdet = 1.0/Determinant(); + + double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2)); + double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3)); + double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1)); + double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3)); + double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1)); + double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2)); + double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2)); + double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3)); + double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1)); + + return FGMatrix33( i11, i12, i13, + i21, i22, i23, + i31, i32, i33 ); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGMatrix33::InitMatrix(void) +{ + data[0] = data[1] = data[2] = data[3] = data[4] = data[5] = + data[6] = data[7] = data[8] = 0.0; +} + +// ***************************************************************************** +// binary operators ************************************************************ +// ***************************************************************************** + +FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const +{ + return FGMatrix33( Entry(1,1) - M(1,1), + Entry(1,2) - M(1,2), + Entry(1,3) - M(1,3), + Entry(2,1) - M(2,1), + Entry(2,2) - M(2,2), + Entry(2,3) - M(2,3), + Entry(3,1) - M(3,1), + Entry(3,2) - M(3,2), + Entry(3,3) - M(3,3) ); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M) +{ + data[0] -= M.data[0]; + data[1] -= M.data[1]; + data[2] -= M.data[2]; + data[3] -= M.data[3]; + data[4] -= M.data[4]; + data[5] -= M.data[5]; + data[6] -= M.data[6]; + data[7] -= M.data[7]; + data[8] -= M.data[8]; + + return *this; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const +{ + return FGMatrix33( Entry(1,1) + M(1,1), + Entry(1,2) + M(1,2), + Entry(1,3) + M(1,3), + Entry(2,1) + M(2,1), + Entry(2,2) + M(2,2), + Entry(2,3) + M(2,3), + Entry(3,1) + M(3,1), + Entry(3,2) + M(3,2), + Entry(3,3) + M(3,3) ); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M) +{ + Entry(1,1) += M(1,1); + Entry(1,2) += M(1,2); + Entry(1,3) += M(1,3); + Entry(2,1) += M(2,1); + Entry(2,2) += M(2,2); + Entry(2,3) += M(2,3); + Entry(3,1) += M(3,1); + Entry(3,2) += M(3,2); + Entry(3,3) += M(3,3); + + return *this; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33 FGMatrix33::operator*(const double scalar) const +{ + return FGMatrix33( scalar * Entry(1,1), + scalar * Entry(1,2), + scalar * Entry(1,3), + scalar * Entry(2,1), + scalar * Entry(2,2), + scalar * Entry(2,3), + scalar * Entry(3,1), + scalar * Entry(3,2), + scalar * Entry(3,3) ); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33 operator*(double scalar, FGMatrix33 &M) +{ + return FGMatrix33( scalar * M(1,1), + scalar * M(1,2), + scalar * M(1,3), + scalar * M(2,1), + scalar * M(2,2), + scalar * M(2,3), + scalar * M(3,1), + scalar * M(3,2), + scalar * M(3,3) ); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33& FGMatrix33::operator*=(const double scalar) +{ + Entry(1,1) *= scalar; + Entry(1,2) *= scalar; + Entry(1,3) *= scalar; + Entry(2,1) *= scalar; + Entry(2,2) *= scalar; + Entry(2,3) *= scalar; + Entry(3,1) *= scalar; + Entry(3,2) *= scalar; + Entry(3,3) *= scalar; + + return *this; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const +{ + // FIXME: Make compiler friendlier + FGMatrix33 Product; + + Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1); + Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2); + Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3); + Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1); + Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2); + Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3); + Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1); + Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2); + Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3); + + return Product; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M) +{ + // FIXME: Make compiler friendlier + double a,b,c; + + a = Entry(1,1); b=Entry(1,2); c=Entry(1,3); + Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1); + Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2); + Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3); + + a = Entry(2,1); b=Entry(2,2); c=Entry(2,3); + Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1); + Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2); + Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3); + + a = Entry(3,1); b=Entry(3,2); c=Entry(3,3); + Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1); + Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2); + Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3); + + return *this; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33 FGMatrix33::operator/(const double scalar) const +{ + FGMatrix33 Quot; + + if ( scalar != 0 ) { + double tmp = 1.0/scalar; + Quot(1,1) = Entry(1,1) * tmp; + Quot(1,2) = Entry(1,2) * tmp; + Quot(1,3) = Entry(1,3) * tmp; + Quot(2,1) = Entry(2,1) * tmp; + Quot(2,2) = Entry(2,2) * tmp; + Quot(2,3) = Entry(2,3) * tmp; + Quot(3,1) = Entry(3,1) * tmp; + Quot(3,2) = Entry(3,2) * tmp; + Quot(3,3) = Entry(3,3) * tmp; + } else { + MatrixException mE; + mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)"; + throw mE; + } + return Quot; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33& FGMatrix33::operator/=(const double scalar) +{ + if ( scalar != 0 ) { + double tmp = 1.0/scalar; + Entry(1,1) *= tmp; + Entry(1,2) *= tmp; + Entry(1,3) *= tmp; + Entry(2,1) *= tmp; + Entry(2,2) *= tmp; + Entry(2,3) *= tmp; + Entry(3,1) *= tmp; + Entry(3,2) *= tmp; + Entry(3,3) *= tmp; + } else { + MatrixException mE; + mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)"; + throw mE; + } + return *this; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGMatrix33::T(void) +{ + for (unsigned int i=1; i<=3; i++) { + for (unsigned int j=i+1; j<=3; j++) { + double tmp = Entry(i,j); + Entry(i,j) = Entry(j,i); + Entry(j,i) = tmp; + } + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const { + double tmp1 = v(1)*Entry(1,1); + double tmp2 = v(1)*Entry(2,1); + double tmp3 = v(1)*Entry(3,1); + + tmp1 += v(2)*Entry(1,2); + tmp2 += v(2)*Entry(2,2); + tmp3 += v(2)*Entry(3,2); + + tmp1 += v(3)*Entry(1,3); + tmp2 += v(3)*Entry(2,3); + tmp3 += v(3)*Entry(3,3); + + return FGColumnVector3( tmp1, tmp2, tmp3 ); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// The bitmasked value choices are as follows: +// unset: In this case (the default) JSBSim would only print +// out the normally expected messages, essentially echoing +// the config files as they are read. If the environment +// variable is not set, debug_lvl is set to 1 internally +// 0: This requests JSBSim not to output any messages +// whatsoever. +// 1: This value explicity requests the normal JSBSim +// startup messages +// 2: This value asks for a message to be printed out when +// a class is instantiated +// 4: When this value is set, a message is displayed when a +// FGModel object executes its Run() method +// 8: When this value is set, various runtime state variables +// are printed out periodically +// 16: When set various parameters are sanity checked and +// a message is printed out when they go out of bounds + +void FGMatrix33::Debug(int from) +{ + if (debug_lvl <= 0) return; + + if (debug_lvl & 1) { // Standard console startup message output + if (from == 0) { // Constructor + + } + } + if (debug_lvl & 2 ) { // Instantiation/Destruction notification + if (from == 0) cout << "Instantiated: FGMatrix33" << endl; + if (from == 1) cout << "Destroyed: FGMatrix33" << endl; + } + if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects + } + if (debug_lvl & 8 ) { // Runtime state variables + } + if (debug_lvl & 16) { // Sanity checking + } + if (debug_lvl & 64) { + if (from == 0) { // Constructor + cout << IdSrc << endl; + cout << IdHdr << endl; + } + } +} +} diff --git a/src/FDM/JSBSim/FGMatrix33.h b/src/FDM/JSBSim/FGMatrix33.h index bbf168be1..dc68b7358 100644 --- a/src/FDM/JSBSim/FGMatrix33.h +++ b/src/FDM/JSBSim/FGMatrix33.h @@ -1,13 +1,14 @@ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Header: FGMatrix33.h -Author: Originally by Tony Peden [formatted and adapted here by Jon Berndt] +Author: Tony Peden, Jon Berndt, Mathias Frolich Date started: Unknown HISTORY -------------------------------------------------------------------------------- ??/??/?? TP Created 03/16/2000 JSB Added exception throwing +03/06/2004 MF Rework of the code to make it a bit compiler friendlier %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SENTRY @@ -94,7 +95,8 @@ public: CLASS DOCUMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -/** Handles matrix math operations. + /** Handles matrix math operations. + @author Tony Peden, Jon Berndt, Mathias Froelich */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -104,49 +106,365 @@ DECLARATION: FGMatrix33 class FGMatrix33 : public FGJSBBase { public: + + enum { + eRows = 3, + eColumns = 3 + }; + + /** Default initializer. + + Create a zero matrix. + */ FGMatrix33(void); - FGMatrix33(int r, int c); - FGMatrix33(const FGMatrix33& A); - ~FGMatrix33(void); - FGMatrix33& operator=(const FGMatrix33& A); - inline double operator()(unsigned int row, unsigned int col) const {return data[row][col];} - inline double& operator()(unsigned int row, unsigned int col) {return data[row][col];} + /** Copy constructor. + + @param M Matrix which is used for initialization. + + Create copy of the matrix given in the argument. + */ + FGMatrix33(const FGMatrix33& M) { + Entry(1,1) = M.Entry(1,1); + Entry(2,1) = M.Entry(2,1); + Entry(3,1) = M.Entry(3,1); + Entry(1,2) = M.Entry(1,2); + Entry(2,2) = M.Entry(2,2); + Entry(3,2) = M.Entry(3,2); + Entry(1,3) = M.Entry(1,3); + Entry(2,3) = M.Entry(2,3); + Entry(3,3) = M.Entry(3,3); + + Debug(0); + } + + /** Initialization by given values. + + @param m11 value of the 1,1 Matrix element. + @param m12 value of the 1,2 Matrix element. + @param m13 value of the 1,3 Matrix element. + @param m21 value of the 2,1 Matrix element. + @param m22 value of the 2,2 Matrix element. + @param m23 value of the 2,3 Matrix element. + @param m31 value of the 3,1 Matrix element. + @param m32 value of the 3,2 Matrix element. + @param m33 value of the 3,3 Matrix element. + + Create a matrix from the doubles given in the arguments. + */ + FGMatrix33(double m11, double m12, double m13, + double m21, double m22, double m23, + double m31, double m32, double m33) { + Entry(1,1) = m11; + Entry(2,1) = m21; + Entry(3,1) = m31; + Entry(1,2) = m12; + Entry(2,2) = m22; + Entry(3,2) = m32; + Entry(1,3) = m13; + Entry(2,3) = m23; + Entry(3,3) = m33; + + Debug(0); + } + + /** Destructor. + */ + ~FGMatrix33(void) { Debug(1); } + + /** Read access the entries of the matrix. + @param row Row index. + @param col Column index. + + @return the value of the matrix entry at the given row and + column indices. Indices are counted starting with 1. + */ + double operator()(unsigned int row, unsigned int col) const { + return Entry(row, col); + } + + /** Write access the entries of the matrix. + Note that the indices given in the arguments are unchecked. + + @param row Row index. + @param col Column index. + + @return a reference to the matrix entry at the given row and + column indices. Indices are counted starting with 1. + */ + double& operator()(unsigned int row, unsigned int col) { + return Entry(row, col); + } + + /** Read access the entries of the matrix. + This function is just a shortcut for the @ref double& + operator()(unsigned int row, unsigned int col) function. It is + used internally to access the elements in a more convenient way. + + Note that the indices given in the arguments are unchecked. + + @param row Row index. + @param col Column index. + + @return the value of the matrix entry at the given row and + column indices. Indices are counted starting with 1. + */ + double Entry(unsigned int row, unsigned int col) const { + return data[(col-1)*eRows+row-1]; + } + + /** Write access the entries of the matrix. + This function is just a shortcut for the @ref double& + operator()(unsigned int row, unsigned int col) function. It is + used internally to access the elements in a more convenient way. - FGColumnVector3 operator*(const FGColumnVector3& Col); + Note that the indices given in the arguments are unchecked. - inline unsigned int Rows(void) const { return 3; } - inline unsigned int Cols(void) const { return 3; } + @param row Row index. + @param col Column index. + @return a reference to the matrix entry at the given row and + column indices. Indices are counted starting with 1. + */ + double& Entry(unsigned int row, unsigned int col) { + return data[(col-1)*eRows+row-1]; + } + + /** Number of rows in the matrix. + @return the number of rows in the matrix. + */ + unsigned int Rows(void) const { return eRows; } + + /** Number of cloumns in the matrix. + @return the number of columns in the matrix. + */ + unsigned int Cols(void) const { return eColumns; } + + /** Transposed matrix. + This function only returns the transpose of this matrix. This matrix itself + remains unchanged. + @return the transposed matrix. + */ + FGMatrix33 Transposed(void) const { + return FGMatrix33( Entry(1,1), Entry(2,1), Entry(3,1), + Entry(1,2), Entry(2,2), Entry(3,2), + Entry(1,3), Entry(2,3), Entry(3,3) ); + } + + /** Transposes this matrix. + This function only transposes this matrix. Nothing is returned. + */ void T(void); + +/** Initialize the matrix. + This function initializes a matrix to all 0.0. + */ void InitMatrix(void); - void InitMatrix(double value); - - //friend FGMatrix33 operator*(double scalar,FGMatrix33& A); - FGMatrix33 operator-(const FGMatrix33& B); - FGMatrix33 operator+(const FGMatrix33& B); - FGMatrix33 operator*(const FGMatrix33& B); - FGMatrix33 operator*(const double scalar); - FGMatrix33 operator/(const double scalar); - FGMatrix33& operator<<(const double ff); +/** Initialize the matrix. + This function initializes a matrix to user specified values. + */ + void InitMatrix(double m11, double m12, double m13, + double m21, double m22, double m23, + double m31, double m32, double m33) { + Entry(1,1) = m11; + Entry(2,1) = m21; + Entry(3,1) = m31; + Entry(1,2) = m12; + Entry(2,2) = m22; + Entry(3,2) = m32; + Entry(1,3) = m13; + Entry(2,3) = m23; + Entry(3,3) = m33; + } + + /** Determinant of the matrix. + @return the determinant of the matrix. + */ + double Determinant(void) const; + + /** Return if the matrix is invertible. + Checks and returns if the matrix is nonsingular and thus + invertible. This is done by simply computing the determinant and + check if it is zero. Note that this test does not cover any + instabilities caused by nearly singular matirces using finite + arithmetics. It only checks exact singularity. + */ + bool Invertible(void) const { return 0.0 != Determinant(); } + + /** Return the inverse of the matrix. + Computes and returns if the inverse of the matrix. It is computed + by Cramers Rule. Also there are no checks performed if the matrix + is invertible. If you are not sure that it really is check this + with the @ref Invertible() call before. + */ + FGMatrix33 Inverse(void) const; + + /** Assignment operator. + + @param A source matrix. + + Copy the content of the matrix given in the argument into *this. + */ + FGMatrix33& operator=(const FGMatrix33& A) { + data[0] = A.data[0]; + data[1] = A.data[1]; + data[2] = A.data[2]; + data[3] = A.data[3]; + data[4] = A.data[4]; + data[5] = A.data[5]; + data[6] = A.data[6]; + data[7] = A.data[7]; + data[8] = A.data[8]; + return *this; + } + + /** Matrix vector multiplication. + + @param v vector to multiply with. + @return matric vector product. + + Compute and return the product of the current matrix with the + vector given in the argument. + */ + FGColumnVector3 operator*(const FGColumnVector3& v) const; + + /** Matrix subtraction. + + @param B matrix to add to. + @return difference of the matrices. + + Compute and return the sum of the current matrix and the matrix + B given in the argument. + */ + FGMatrix33 operator-(const FGMatrix33& B) const; + + /** Matrix addition. + + @param B matrix to add to. + @return sum of the matrices. + + Compute and return the sum of the current matrix and the matrix + B given in the argument. + */ + FGMatrix33 operator+(const FGMatrix33& B) const; + + /** Matrix product. + + @param B matrix to add to. + @return product of the matrices. + + Compute and return the product of the current matrix and the matrix + B given in the argument. + */ + FGMatrix33 operator*(const FGMatrix33& B) const; + + /** Multiply the matrix with a scalar. + + @param scalar scalar factor to multiply with. + @return scaled matrix. + + Compute and return the product of the current matrix with the + scalar value scalar given in the argument. + */ + FGMatrix33 operator*(const double scalar) const; + + /** Multiply the matrix with 1.0/scalar. + + @param scalar scalar factor to divide through. + @return scaled matrix. + + Compute and return the product of the current matrix with the + scalar value 1.0/scalar, where scalar is given in the argument. + */ + FGMatrix33 operator/(const double scalar) const; + + /** In place matrix subtraction. + + @param B matrix to subtract. + @return reference to the current matrix. + + Compute the diffence from the current matrix and the matrix B + given in the argument. + */ + FGMatrix33& operator-=(const FGMatrix33 &B); + + /** In place matrix addition. + + @param B matrix to add. + @return reference to the current matrix. + + Compute the sum of the current matrix and the matrix B + given in the argument. + */ + FGMatrix33& operator+=(const FGMatrix33 &B); - friend ostream& operator<<(ostream& os, const FGMatrix33& M); - friend istream& operator>>(istream& is, FGMatrix33& M); + /** In place matrix multiplication. - void operator-=(const FGMatrix33 &B); - void operator+=(const FGMatrix33 &B); - void operator*=(const FGMatrix33 &B); - void operator*=(const double scalar); - void operator/=(const double scalar); + @param B matrix to multiply with. + @return reference to the current matrix. -protected: - double data[4][4]; + Compute the product of the current matrix and the matrix B + given in the argument. + */ + FGMatrix33& operator*=(const FGMatrix33 &B); + + /** In place matrix scale. + + @param scalar scalar value to multiply with. + @return reference to the current matrix. + + Compute the product of the current matrix and the scalar value scalar + given in the argument. + */ + FGMatrix33& operator*=(const double scalar); + + /** In place matrix scale. + + @param scalar scalar value to divide through. + @return reference to the current matrix. + + Compute the product of the current matrix and the scalar value + 1.0/scalar, where scalar is given in the argument. + */ + FGMatrix33& operator/=(const double scalar); private: - void TransposeSquare(void); - unsigned int rowCtr, colCtr; + double data[eRows*eColumns]; + void Debug(int from); }; + +/** Scalar multiplication. + + @param scalar scalar value to multiply with. + @param A Matrix to multiply. + + Multiply the Matrix with a scalar value. +*/ +inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) { + // use already defined operation. + return A*scalar; } + +/** Write matrix to a stream. + + @param os Stream to write to. + @param M Matrix to write. + + Write the matrix to a stream. +*/ +ostream& operator<<(ostream& os, const FGMatrix33& M); + +/** Read matrix from a stream. + + @param os Stream to read from. + @param M Matrix to initialize with the values from the stream. + + Read matrix from a stream. +*/ +istream& operator>>(istream& is, FGMatrix33& M); + +} // namespace JSBSim + #endif diff --git a/src/FDM/JSBSim/FGNozzle.cpp b/src/FDM/JSBSim/FGNozzle.cpp index 12ca06aa1..4faa4ea74 100644 --- a/src/FDM/JSBSim/FGNozzle.cpp +++ b/src/FDM/JSBSim/FGNozzle.cpp @@ -43,6 +43,7 @@ INCLUDES #endif #include "FGNozzle.h" +#include "FGAtmosphere.h" namespace JSBSim { diff --git a/src/FDM/JSBSim/FGOutput.cpp b/src/FDM/JSBSim/FGOutput.cpp index c15d2f188..54231a5ec 100644 --- a/src/FDM/JSBSim/FGOutput.cpp +++ b/src/FDM/JSBSim/FGOutput.cpp @@ -1,640 +1,644 @@ -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - Module: FGOutput.cpp - Author: Jon Berndt - Date started: 12/02/98 - Purpose: Manage output of sim parameters to file or stdout - Called by: FGSimExec - - ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- - - 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. - -FUNCTIONAL DESCRIPTION --------------------------------------------------------------------------------- -This is the place where you create output routines to dump data for perusal -later. - -HISTORY --------------------------------------------------------------------------------- -12/02/98 JSB Created - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -INCLUDES -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -#include "FGOutput.h" -#include "FGState.h" -#include "FGFDMExec.h" -#include "FGAtmosphere.h" -#include "FGFCS.h" -#include "FGAerodynamics.h" -#include "FGGroundReactions.h" -#include "FGAircraft.h" -#include "FGMassBalance.h" -#include "FGTranslation.h" -#include "FGRotation.h" -#include "FGPosition.h" -#include "FGAuxiliary.h" - -namespace JSBSim { - -static const char *IdSrc = "$Id$"; -static const char *IdHdr = ID_OUTPUT; - -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -CLASS IMPLEMENTATION -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -FGOutput::FGOutput(FGFDMExec* fdmex) : FGModel(fdmex) -{ - Name = "FGOutput"; - sFirstPass = dFirstPass = true; - socket = 0; - Type = otNone; - Filename = ""; - SubSystems = 0; - enabled = true; - outputInFileName = ""; - - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGOutput::~FGOutput() -{ - if (socket) delete socket; - for (int i=0; iGetCoefficientStrings(); - } - if (SubSystems & ssFCS) { - outstream << ", "; - outstream << FCS->GetComponentStrings(); - } - if (SubSystems & ssGroundReactions) { - outstream << ", "; - outstream << GroundReactions->GetGroundReactionStrings(); - } - if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) { - outstream << ", "; - outstream << Propulsion->GetPropulsionStrings(); - } - if (OutputProperties.size() > 0) { - for (int i=0;iGetName(); - } - } - - outstream << endl; - dFirstPass = false; - } - - outstream << State->Getsim_time(); - if (SubSystems & ssSimulation) { - } - if (SubSystems & ssAerosurfaces) { - outstream << ", "; - outstream << FCS->GetDaCmd() << ", "; - outstream << FCS->GetDeCmd() << ", "; - outstream << FCS->GetDrCmd() << ", "; - outstream << FCS->GetDfCmd() << ", "; - outstream << FCS->GetDaLPos() << ", "; - outstream << FCS->GetDaRPos() << ", "; - outstream << FCS->GetDePos() << ", "; - outstream << FCS->GetDrPos() << ", "; - outstream << FCS->GetDfPos(); - } - if (SubSystems & ssRates) { - outstream << ", "; - outstream << Rotation->GetPQR() << ", "; - outstream << Rotation->GetPQRdot(); - } - if (SubSystems & ssVelocities) { - outstream << ", "; - outstream << Translation->Getqbar() << ", "; - outstream << Translation->GetVt() << ", "; - outstream << Translation->GetUVW() << ", "; - outstream << Translation->GetAeroUVW() << ", "; - outstream << Position->GetVel(); - } - if (SubSystems & ssForces) { - outstream << ", "; - outstream << Aerodynamics->GetvFs() << ", "; - outstream << Aerodynamics->GetLoD() << ", "; - outstream << Aircraft->GetForces() << ", "; - outstream << Inertial->GetGravity() << ", "; - outstream << Inertial->GetCoriolis() << ", "; - outstream << Inertial->GetCentrifugal(); - } - if (SubSystems & ssMoments) { - outstream << ", "; - outstream << Aircraft->GetMoments(); - } - if (SubSystems & ssAtmosphere) { - outstream << ", "; - outstream << Atmosphere->GetDensity() << ", "; - outstream << Atmosphere->GetWindNED(); - } - if (SubSystems & ssMassProps) { - outstream << ", "; - outstream << MassBalance->GetIxx() << ", "; - outstream << MassBalance->GetIyy() << ", "; - outstream << MassBalance->GetIzz() << ", "; - outstream << MassBalance->GetIxz() << ", "; - outstream << MassBalance->GetMass() << ", "; - outstream << MassBalance->GetXYZcg(); - } - if (SubSystems & ssPosition) { - outstream << ", "; - outstream << Position->Geth() << ", "; - outstream << Rotation->GetEuler() << ", "; - outstream << Translation->Getalpha() << ", "; - outstream << Translation->Getbeta() << ", "; - outstream << Position->GetLatitude() << ", "; - outstream << Position->GetLongitude() << ", "; - outstream << Position->GetDistanceAGL() << ", "; - outstream << Position->GetRunwayRadius(); - } - if (SubSystems & ssCoefficients) { - outstream << ", "; - outstream << Aerodynamics->GetCoefficientValues(); - } - if (SubSystems & ssFCS) { - outstream << ", "; - outstream << FCS->GetComponentValues(); - } - if (SubSystems & ssGroundReactions) { - outstream << ", "; - outstream << GroundReactions->GetGroundReactionValues(); - } - if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) { - outstream << ", "; - outstream << Propulsion->GetPropulsionValues(); - } - - for (int i=0;igetDoubleValue(); - } - - outstream << endl; - outstream.flush(); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGOutput::SocketOutput(void) -{ - string asciiData; - - if (socket == NULL) return; - if (!socket->GetConnectStatus()) return; - - socket->Clear(); - if (sFirstPass) { - socket->Append(""); - socket->Append("Time"); - socket->Append("Altitude"); - socket->Append("Phi"); - socket->Append("Tht"); - socket->Append("Psi"); - socket->Append("Rho"); - socket->Append("Vtotal"); - socket->Append("UBody"); - socket->Append("VBody"); - socket->Append("WBody"); - socket->Append("UAero"); - socket->Append("VAero"); - socket->Append("WAero"); - socket->Append("Vn"); - socket->Append("Ve"); - socket->Append("Vd"); - socket->Append("Udot"); - socket->Append("Vdot"); - socket->Append("Wdot"); - socket->Append("P"); - socket->Append("Q"); - socket->Append("R"); - socket->Append("PDot"); - socket->Append("QDot"); - socket->Append("RDot"); - socket->Append("Fx"); - socket->Append("Fy"); - socket->Append("Fz"); - socket->Append("Latitude"); - socket->Append("Longitude"); - socket->Append("QBar"); - socket->Append("Alpha"); - socket->Append("L"); - socket->Append("M"); - socket->Append("N"); - socket->Append("Throttle Position"); - socket->Append("Left Aileron Position"); - socket->Append("Right Aileron Position"); - socket->Append("Elevator Position"); - socket->Append("Rudder Position"); - sFirstPass = false; - socket->Send(); - } - - socket->Clear(); - socket->Append(State->Getsim_time()); - socket->Append(Position->Geth()); - socket->Append(Rotation->Getphi()); - socket->Append(Rotation->Gettht()); - socket->Append(Rotation->Getpsi()); - socket->Append(Atmosphere->GetDensity()); - socket->Append(Translation->GetVt()); - socket->Append(Translation->GetUVW(eU)); - socket->Append(Translation->GetUVW(eV)); - socket->Append(Translation->GetUVW(eW)); - socket->Append(Translation->GetAeroUVW(eU)); - socket->Append(Translation->GetAeroUVW(eV)); - socket->Append(Translation->GetAeroUVW(eW)); - socket->Append(Position->GetVn()); - socket->Append(Position->GetVe()); - socket->Append(Position->GetVd()); - socket->Append(Translation->GetUVWdot(eU)); - socket->Append(Translation->GetUVWdot(eV)); - socket->Append(Translation->GetUVWdot(eW)); - socket->Append(Rotation->GetPQR(eP)); - socket->Append(Rotation->GetPQR(eQ)); - socket->Append(Rotation->GetPQR(eR)); - socket->Append(Rotation->GetPQRdot(eP)); - socket->Append(Rotation->GetPQRdot(eQ)); - socket->Append(Rotation->GetPQRdot(eR)); - socket->Append(Aircraft->GetForces(eX)); - socket->Append(Aircraft->GetForces(eY)); - socket->Append(Aircraft->GetForces(eZ)); - socket->Append(Position->GetLatitude()); - socket->Append(Position->GetLongitude()); - socket->Append(Translation->Getqbar()); - socket->Append(Translation->Getalpha()); - socket->Append(Aircraft->GetMoments(eL)); - socket->Append(Aircraft->GetMoments(eM)); - socket->Append(Aircraft->GetMoments(eN)); - socket->Append(FCS->GetThrottlePos(0)); - socket->Append(FCS->GetDaLPos()); - socket->Append(FCS->GetDaRPos()); - socket->Append(FCS->GetDePos()); - socket->Append(FCS->GetDrPos()); - socket->Send(); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGOutput::SocketStatusOutput(string out_str) -{ - string asciiData; - - if (socket == NULL) return; - - socket->Clear(); - asciiData = string("") + out_str; - socket->Append(asciiData.c_str()); - socket->Send(); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -bool FGOutput::Load(FGConfigFile* AC_cfg) -{ - string token="", parameter="", separator=""; - string name="", fname=""; - int OutRate = 0; - FGConfigFile* Output_cfg; - string property; - -# ifndef macintosh - separator = "/"; -# else - separator = ";"; -# endif - - name = AC_cfg->GetValue("NAME"); - fname = AC_cfg->GetValue("FILE"); - token = AC_cfg->GetValue("TYPE"); - Output->SetType(token); - -#if defined( FG_WITH_JSBSIM_SOCKET ) || !defined( FGFS ) - if (token == "SOCKET") { - socket = new FGfdmSocket("localhost",1138); - } -#endif - - if (!fname.empty()) { - outputInFileName = FDMExec->GetAircraftPath() + separator - + FDMExec->GetModelName() + separator + fname + ".xml"; - Output_cfg = new FGConfigFile(outputInFileName); - if (!Output_cfg->IsOpen()) { - cerr << "Could not open file: " << outputInFileName << endl; - return false; - } - } else { - Output_cfg = AC_cfg; - } - Output->SetFilename(name); - - while ((token = Output_cfg->GetValue()) != string("/OUTPUT")) { - *Output_cfg >> parameter; - if (parameter == "RATE_IN_HZ") { - *Output_cfg >> OutRate; - } - if (parameter == "SIMULATION") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssSimulation; - } - if (parameter == "AEROSURFACES") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssAerosurfaces; - } - if (parameter == "RATES") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssRates; - } - if (parameter == "VELOCITIES") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssVelocities; - } - if (parameter == "FORCES") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssForces; - } - if (parameter == "MOMENTS") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssMoments; - } - if (parameter == "ATMOSPHERE") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssAtmosphere; - } - if (parameter == "MASSPROPS") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssMassProps; - } - if (parameter == "POSITION") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssPosition; - } - if (parameter == "COEFFICIENTS") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssCoefficients; - } - if (parameter == "GROUND_REACTIONS") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssGroundReactions; - } - if (parameter == "FCS") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssFCS; - } - if (parameter == "PROPULSION") { - *Output_cfg >> parameter; - if (parameter == "ON") SubSystems += ssPropulsion; - } - if (parameter == "PROPERTY") { - *Output_cfg >> property; - OutputProperties.push_back(PropertyManager->GetNode(property)); - } - - if (parameter == "EOF") break; - } - - OutRate = OutRate>120?120:(OutRate<0?0:OutRate); - rate = (int)(0.5 + 1.0/(State->Getdt()*OutRate)); - - Debug(2); - - return true; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// The bitmasked value choices are as follows: -// unset: In this case (the default) JSBSim would only print -// out the normally expected messages, essentially echoing -// the config files as they are read. If the environment -// variable is not set, debug_lvl is set to 1 internally -// 0: This requests JSBSim not to output any messages -// whatsoever. -// 1: This value explicity requests the normal JSBSim -// startup messages -// 2: This value asks for a message to be printed out when -// a class is instantiated -// 4: When this value is set, a message is displayed when a -// FGModel object executes its Run() method -// 8: When this value is set, various runtime state variables -// are printed out periodically -// 16: When set various parameters are sanity checked and -// a message is printed out when they go out of bounds - -void FGOutput::Debug(int from) -{ - string scratch=""; - - if (debug_lvl <= 0) return; - - if (debug_lvl & 1) { // Standard console startup message output - if (from == 0) { // Constructor - - } - if (from == 2) { - if (outputInFileName.empty()) - cout << " " << "Output parameters read inline" << endl; - else - cout << " Output parameters read from file: " << outputInFileName << endl; - - if (Filename == "cout" || Filename == "COUT") { - scratch = " Log output goes to screen console"; - } else if (!Filename.empty()) { - scratch = " Log output goes to file: " + Filename; - } - switch (Type) { - case otCSV: - cout << scratch << " in CSV format output at rate " << 120/rate << " Hz" << endl; - break; - case otNone: - cout << " No log output" << endl; - break; - } - - if (SubSystems & ssSimulation) cout << " Simulation parameters logged" << endl; - if (SubSystems & ssAerosurfaces) cout << " Aerosurface parameters logged" << endl; - if (SubSystems & ssRates) cout << " Rate parameters logged" << endl; - if (SubSystems & ssVelocities) cout << " Velocity parameters logged" << endl; - if (SubSystems & ssForces) cout << " Force parameters logged" << endl; - if (SubSystems & ssMoments) cout << " Moments parameters logged" << endl; - if (SubSystems & ssAtmosphere) cout << " Atmosphere parameters logged" << endl; - if (SubSystems & ssMassProps) cout << " Mass parameters logged" << endl; - if (SubSystems & ssCoefficients) cout << " Coefficient parameters logged" << endl; - if (SubSystems & ssPosition) cout << " Position parameters logged" << endl; - if (SubSystems & ssGroundReactions) cout << " Ground parameters logged" << endl; - if (SubSystems & ssFCS) cout << " FCS parameters logged" << endl; - if (SubSystems & ssPropulsion) cout << " Propulsion parameters logged" << endl; - } - } - if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGOutput" << endl; - if (from == 1) cout << "Destroyed: FGOutput" << endl; - } - if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects - } - if (debug_lvl & 8 ) { // Runtime state variables - } - if (debug_lvl & 16) { // Sanity checking - } - if (debug_lvl & 64) { - if (from == 0) { // Constructor - cout << IdSrc << endl; - cout << IdHdr << endl; - } - } -} -} +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGOutput.cpp + Author: Jon Berndt + Date started: 12/02/98 + Purpose: Manage output of sim parameters to file or stdout + Called by: FGSimExec + + ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- + + 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. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- +This is the place where you create output routines to dump data for perusal +later. + +HISTORY +-------------------------------------------------------------------------------- +12/02/98 JSB Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGOutput.h" +#include "FGState.h" +#include "FGFDMExec.h" +#include "FGAtmosphere.h" +#include "FGFCS.h" +#include "FGAerodynamics.h" +#include "FGGroundReactions.h" +#include "FGAircraft.h" +#include "FGMassBalance.h" +#include "FGTranslation.h" +#include "FGRotation.h" +#include "FGPosition.h" +#include "FGAuxiliary.h" +#include "FGInertial.h" + +namespace JSBSim { + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_OUTPUT; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +FGOutput::FGOutput(FGFDMExec* fdmex) : FGModel(fdmex) +{ + Name = "FGOutput"; + sFirstPass = dFirstPass = true; + socket = 0; + Type = otNone; + Filename = ""; + SubSystems = 0; + enabled = true; + outputInFileName = ""; + + Debug(0); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGOutput::~FGOutput() +{ + if (socket) delete socket; + for (unsigned int i=0; iGetCoefficientStrings(); + if (scratch.length() != 0) outstream << ", " << scratch; + } + if (SubSystems & ssFCS) { + scratch = FCS->GetComponentStrings(); + if (scratch.length() != 0) outstream << ", " << scratch; + } + if (SubSystems & ssGroundReactions) { + outstream << ", "; + outstream << GroundReactions->GetGroundReactionStrings(); + } + if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) { + outstream << ", "; + outstream << Propulsion->GetPropulsionStrings(); + } + if (OutputProperties.size() > 0) { + for (unsigned int i=0;iGetName(); + } + } + + outstream << endl; + dFirstPass = false; + } + + outstream << State->Getsim_time(); + if (SubSystems & ssSimulation) { + } + if (SubSystems & ssAerosurfaces) { + outstream << ", "; + outstream << FCS->GetDaCmd() << ", "; + outstream << FCS->GetDeCmd() << ", "; + outstream << FCS->GetDrCmd() << ", "; + outstream << FCS->GetDfCmd() << ", "; + outstream << FCS->GetDaLPos() << ", "; + outstream << FCS->GetDaRPos() << ", "; + outstream << FCS->GetDePos() << ", "; + outstream << FCS->GetDrPos() << ", "; + outstream << FCS->GetDfPos(); + } + if (SubSystems & ssRates) { + outstream << ", "; + outstream << Rotation->GetPQR() << ", "; + outstream << Rotation->GetPQRdot(); + } + if (SubSystems & ssVelocities) { + outstream << ", "; + outstream << Translation->Getqbar() << ", "; + outstream << Translation->GetVt() << ", "; + outstream << Translation->GetUVW() << ", "; + outstream << Translation->GetAeroUVW() << ", "; + outstream << Position->GetVel(); + } + if (SubSystems & ssForces) { + outstream << ", "; + outstream << Aerodynamics->GetvFs() << ", "; + outstream << Aerodynamics->GetLoD() << ", "; + outstream << Aircraft->GetForces() << ", "; + outstream << Inertial->GetGravity() << ", "; + outstream << Inertial->GetCoriolis() << ", "; + outstream << Inertial->GetCentrifugal(); + } + if (SubSystems & ssMoments) { + outstream << ", "; + outstream << Aircraft->GetMoments(); + } + if (SubSystems & ssAtmosphere) { + outstream << ", "; + outstream << Atmosphere->GetDensity() << ", "; + outstream << Atmosphere->GetWindNED(); + } + if (SubSystems & ssMassProps) { + outstream << ", "; + outstream << MassBalance->GetJ() << ", "; + outstream << MassBalance->GetMass() << ", "; + outstream << MassBalance->GetXYZcg(); + } + if (SubSystems & ssPosition) { + outstream << ", "; + outstream << Position->Geth() << ", "; + outstream << Rotation->GetEuler() << ", "; + outstream << Translation->Getalpha() << ", "; + outstream << Translation->Getbeta() << ", "; + outstream << Position->GetLatitude() << ", "; + outstream << Position->GetLongitude() << ", "; + outstream << Position->GetDistanceAGL() << ", "; + outstream << Position->GetRunwayRadius(); + } + if (SubSystems & ssCoefficients) { + scratch = Aerodynamics->GetCoefficientValues(); + if (scratch.length() != 0) outstream << ", " << scratch; + } + if (SubSystems & ssFCS) { + scratch = FCS->GetComponentValues(); + if (scratch.length() != 0) outstream << ", " << scratch; + } + if (SubSystems & ssGroundReactions) { + outstream << ", "; + outstream << GroundReactions->GetGroundReactionValues(); + } + if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) { + outstream << ", "; + outstream << Propulsion->GetPropulsionValues(); + } + + for (unsigned int i=0;igetDoubleValue(); + } + + outstream << endl; + outstream.flush(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGOutput::SocketOutput(void) +{ + string asciiData; + + if (socket == NULL) return; + if (!socket->GetConnectStatus()) return; + + socket->Clear(); + if (sFirstPass) { + socket->Append(""); + socket->Append("Time"); + socket->Append("Altitude"); + socket->Append("Phi"); + socket->Append("Tht"); + socket->Append("Psi"); + socket->Append("Rho"); + socket->Append("Vtotal"); + socket->Append("UBody"); + socket->Append("VBody"); + socket->Append("WBody"); + socket->Append("UAero"); + socket->Append("VAero"); + socket->Append("WAero"); + socket->Append("Vn"); + socket->Append("Ve"); + socket->Append("Vd"); + socket->Append("Udot"); + socket->Append("Vdot"); + socket->Append("Wdot"); + socket->Append("P"); + socket->Append("Q"); + socket->Append("R"); + socket->Append("PDot"); + socket->Append("QDot"); + socket->Append("RDot"); + socket->Append("Fx"); + socket->Append("Fy"); + socket->Append("Fz"); + socket->Append("Latitude"); + socket->Append("Longitude"); + socket->Append("QBar"); + socket->Append("Alpha"); + socket->Append("L"); + socket->Append("M"); + socket->Append("N"); + socket->Append("Throttle Position"); + socket->Append("Left Aileron Position"); + socket->Append("Right Aileron Position"); + socket->Append("Elevator Position"); + socket->Append("Rudder Position"); + sFirstPass = false; + socket->Send(); + } + + socket->Clear(); + socket->Append(State->Getsim_time()); + socket->Append(Position->Geth()); + socket->Append(Rotation->Getphi()); + socket->Append(Rotation->Gettht()); + socket->Append(Rotation->Getpsi()); + socket->Append(Atmosphere->GetDensity()); + socket->Append(Translation->GetVt()); + socket->Append(Translation->GetUVW(eU)); + socket->Append(Translation->GetUVW(eV)); + socket->Append(Translation->GetUVW(eW)); + socket->Append(Translation->GetAeroUVW(eU)); + socket->Append(Translation->GetAeroUVW(eV)); + socket->Append(Translation->GetAeroUVW(eW)); + socket->Append(Position->GetVn()); + socket->Append(Position->GetVe()); + socket->Append(Position->GetVd()); + socket->Append(Translation->GetUVWdot(eU)); + socket->Append(Translation->GetUVWdot(eV)); + socket->Append(Translation->GetUVWdot(eW)); + socket->Append(Rotation->GetPQR(eP)); + socket->Append(Rotation->GetPQR(eQ)); + socket->Append(Rotation->GetPQR(eR)); + socket->Append(Rotation->GetPQRdot(eP)); + socket->Append(Rotation->GetPQRdot(eQ)); + socket->Append(Rotation->GetPQRdot(eR)); + socket->Append(Aircraft->GetForces(eX)); + socket->Append(Aircraft->GetForces(eY)); + socket->Append(Aircraft->GetForces(eZ)); + socket->Append(Position->GetLatitude()); + socket->Append(Position->GetLongitude()); + socket->Append(Translation->Getqbar()); + socket->Append(Translation->Getalpha()); + socket->Append(Aircraft->GetMoments(eL)); + socket->Append(Aircraft->GetMoments(eM)); + socket->Append(Aircraft->GetMoments(eN)); + socket->Append(FCS->GetThrottlePos(0)); + socket->Append(FCS->GetDaLPos()); + socket->Append(FCS->GetDaRPos()); + socket->Append(FCS->GetDePos()); + socket->Append(FCS->GetDrPos()); + socket->Send(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGOutput::SocketStatusOutput(string out_str) +{ + string asciiData; + + if (socket == NULL) return; + + socket->Clear(); + asciiData = string("") + out_str; + socket->Append(asciiData.c_str()); + socket->Send(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGOutput::Load(FGConfigFile* AC_cfg) +{ + string token="", parameter="", separator=""; + string name="", fname=""; + int OutRate = 0; + FGConfigFile* Output_cfg; + string property; + +# ifndef macintosh + separator = "/"; +# else + separator = ";"; +# endif + + name = AC_cfg->GetValue("NAME"); + fname = AC_cfg->GetValue("FILE"); + token = AC_cfg->GetValue("TYPE"); + Output->SetType(token); + +#if defined( FG_WITH_JSBSIM_SOCKET ) || !defined( FGFS ) + if (token == "SOCKET") { + socket = new FGfdmSocket("localhost",1138); + } +#endif + + if (!fname.empty()) { + outputInFileName = FDMExec->GetAircraftPath() + separator + + FDMExec->GetModelName() + separator + fname + ".xml"; + Output_cfg = new FGConfigFile(outputInFileName); + if (!Output_cfg->IsOpen()) { + cerr << "Could not open file: " << outputInFileName << endl; + return false; + } + } else { + Output_cfg = AC_cfg; + } + Output->SetFilename(name); + + while ((token = Output_cfg->GetValue()) != string("/OUTPUT")) { + *Output_cfg >> parameter; + if (parameter == "RATE_IN_HZ") { + *Output_cfg >> OutRate; + } + if (parameter == "SIMULATION") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssSimulation; + } + if (parameter == "AEROSURFACES") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssAerosurfaces; + } + if (parameter == "RATES") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssRates; + } + if (parameter == "VELOCITIES") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssVelocities; + } + if (parameter == "FORCES") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssForces; + } + if (parameter == "MOMENTS") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssMoments; + } + if (parameter == "ATMOSPHERE") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssAtmosphere; + } + if (parameter == "MASSPROPS") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssMassProps; + } + if (parameter == "POSITION") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssPosition; + } + if (parameter == "COEFFICIENTS") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssCoefficients; + } + if (parameter == "GROUND_REACTIONS") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssGroundReactions; + } + if (parameter == "FCS") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssFCS; + } + if (parameter == "PROPULSION") { + *Output_cfg >> parameter; + if (parameter == "ON") SubSystems += ssPropulsion; + } + if (parameter == "PROPERTY") { + *Output_cfg >> property; + OutputProperties.push_back(PropertyManager->GetNode(property)); + } + + if (parameter == "EOF") break; + } + + OutRate = OutRate>120?120:(OutRate<0?0:OutRate); + rate = (int)(0.5 + 1.0/(State->Getdt()*OutRate)); + + Debug(2); + + return true; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// The bitmasked value choices are as follows: +// unset: In this case (the default) JSBSim would only print +// out the normally expected messages, essentially echoing +// the config files as they are read. If the environment +// variable is not set, debug_lvl is set to 1 internally +// 0: This requests JSBSim not to output any messages +// whatsoever. +// 1: This value explicity requests the normal JSBSim +// startup messages +// 2: This value asks for a message to be printed out when +// a class is instantiated +// 4: When this value is set, a message is displayed when a +// FGModel object executes its Run() method +// 8: When this value is set, various runtime state variables +// are printed out periodically +// 16: When set various parameters are sanity checked and +// a message is printed out when they go out of bounds + +void FGOutput::Debug(int from) +{ + string scratch=""; + + if (debug_lvl <= 0) return; + + if (debug_lvl & 1) { // Standard console startup message output + if (from == 0) { // Constructor + + } + if (from == 2) { + if (outputInFileName.empty()) + cout << " " << "Output parameters read inline" << endl; + else + cout << " Output parameters read from file: " << outputInFileName << endl; + + if (Filename == "cout" || Filename == "COUT") { + scratch = " Log output goes to screen console"; + } else if (!Filename.empty()) { + scratch = " Log output goes to file: " + Filename; + } + switch (Type) { + case otCSV: + cout << scratch << " in CSV format output at rate " << 120/rate << " Hz" << endl; + break; + case otNone: + cout << " No log output" << endl; + break; + } + + if (SubSystems & ssSimulation) cout << " Simulation parameters logged" << endl; + if (SubSystems & ssAerosurfaces) cout << " Aerosurface parameters logged" << endl; + if (SubSystems & ssRates) cout << " Rate parameters logged" << endl; + if (SubSystems & ssVelocities) cout << " Velocity parameters logged" << endl; + if (SubSystems & ssForces) cout << " Force parameters logged" << endl; + if (SubSystems & ssMoments) cout << " Moments parameters logged" << endl; + if (SubSystems & ssAtmosphere) cout << " Atmosphere parameters logged" << endl; + if (SubSystems & ssMassProps) cout << " Mass parameters logged" << endl; + if (SubSystems & ssCoefficients) cout << " Coefficient parameters logged" << endl; + if (SubSystems & ssPosition) cout << " Position parameters logged" << endl; + if (SubSystems & ssGroundReactions) cout << " Ground parameters logged" << endl; + if (SubSystems & ssFCS) cout << " FCS parameters logged" << endl; + if (SubSystems & ssPropulsion) cout << " Propulsion parameters logged" << endl; + } + } + if (debug_lvl & 2 ) { // Instantiation/Destruction notification + if (from == 0) cout << "Instantiated: FGOutput" << endl; + if (from == 1) cout << "Destroyed: FGOutput" << endl; + } + if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects + } + if (debug_lvl & 8 ) { // Runtime state variables + } + if (debug_lvl & 16) { // Sanity checking + } + if (debug_lvl & 64) { + if (from == 0) { // Constructor + cout << IdSrc << endl; + cout << IdHdr << endl; + } + } +} +} diff --git a/src/FDM/JSBSim/FGPosition.cpp b/src/FDM/JSBSim/FGPosition.cpp index 932c511b2..e33dba821 100644 --- a/src/FDM/JSBSim/FGPosition.cpp +++ b/src/FDM/JSBSim/FGPosition.cpp @@ -1,373 +1,367 @@ -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - Module: FGPosition.cpp - Author: Jon S. Berndt - Date started: 01/05/99 - Purpose: Integrate the EOM to determine instantaneous position - Called by: FGFDMExec - - ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- - - 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. - -FUNCTIONAL DESCRIPTION --------------------------------------------------------------------------------- -This class encapsulates the integration of rates and accelerations to get the -current position of the aircraft. - -HISTORY --------------------------------------------------------------------------------- -01/05/99 JSB Created - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -COMMENTS, REFERENCES, and NOTES -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -[1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling - Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate - School, January 1994 -[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices", - JSC 12960, July 1977 -[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at - NASA-Ames", NASA CR-2497, January 1975 -[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics", - Wiley & Sons, 1979 ISBN 0-471-03032-5 -[5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons, - 1982 ISBN 0-471-08936-2 - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -INCLUDES -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -#ifdef FGFS -# include -# ifdef SG_HAVE_STD_INCLUDES -# include -# include -# else -# include -# include -# endif -#else -# if defined(sgi) && !defined(__GNUC__) -# include -# if (_COMPILER_VERSION < 740) -# include -# else -# include -# endif -# else -# include -# include -# endif -#endif - -#include "FGPosition.h" -#include "FGAtmosphere.h" -#include "FGState.h" -#include "FGFDMExec.h" -#include "FGFCS.h" -#include "FGAircraft.h" -#include "FGMassBalance.h" -#include "FGTranslation.h" -#include "FGRotation.h" -#include "FGAuxiliary.h" -#include "FGOutput.h" -#include "FGPropertyManager.h" - - -namespace JSBSim { - -static const char *IdSrc = "$Id$"; -static const char *IdHdr = ID_POSITION; - -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -CLASS IMPLEMENTATION -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -extern double globalTriNormal[3]; -extern double globalSceneryAltitude; -extern double globalSeaLevelRadius; - -FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex) -{ - Name = "FGPosition"; - LongitudeDot = LatitudeDot = RadiusDot = 0.0; - - for (int i=0;i<4;i++) { - LatitudeDot_prev[i] = 0.0; - LongitudeDot_prev[i] = 0.0; - RadiusDot_prev[i] = 0.0; - } - - vVRPoffset.InitMatrix(); - - Longitude = Latitude = 0.0; - LongitudeVRP = LatitudeVRP = 0.0; - gamma = Vt = Vground = 0.0; - hoverbmac = hoverbcg = 0.0; - psigt = 0.0; - bind(); - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGPosition::~FGPosition(void) -{ - unbind(); - Debug(1); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -bool FGPosition::InitModel(void) -{ - FGModel::InitModel(); - - h = 3.0; // Est. height of aircraft cg off runway - SeaLevelRadius = Inertial->RefRadius(); // For initialization ONLY - Radius = SeaLevelRadius + h; - RunwayRadius = SeaLevelRadius; - DistanceAGL = Radius - RunwayRadius; // Geocentric - vRunwayNormal(3) = -1.0; // Initialized for standalone mode - b = 1; - return true; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -/* -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 reference radius. - In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass. -*/ - -bool FGPosition::Run(void) -{ - double cosLat; - double hdot_Vt; - - if (!FGModel::Run()) { - GetState(); - - Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) ); - psigt = atan2(vVel(eEast), vVel(eNorth)); - if (psigt < 0.0) - psigt += 2*M_PI; - - Radius = h + SeaLevelRadius; - - cosLat = cos(Latitude); - if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat); - LatitudeDot = vVel(eNorth) / Radius; - RadiusDot = -vVel(eDown); - - Longitude += State->Integrate(FGState::TRAPZ, dt*rate, LongitudeDot, LongitudeDot_prev); - Latitude += State->Integrate(FGState::TRAPZ, dt*rate, LatitudeDot, LatitudeDot_prev); - Radius += State->Integrate(FGState::TRAPZ, dt*rate, RadiusDot, RadiusDot_prev); - - h = Radius - SeaLevelRadius; // Geocentric - - vVRPoffset = State->GetTb2l() * MassBalance->StructuralToBody(Aircraft->GetXYZvrp()); - - // vVRP - the vector to the Visual Reference Point - now contains the - // offset from the CG to the VRP, in units of feet, in the Local coordinate - // frame, where X points north, Y points East, and Z points down. This needs - // to be converted to Lat/Lon/Alt, now. - - if (cosLat != 0) - LongitudeVRP = vVRPoffset(eEast) / (Radius * cosLat) + Longitude; - - LatitudeVRP = vVRPoffset(eNorth) / Radius + Latitude; - hVRP = h - vVRPoffset(eDown); -/* -cout << "Lat/Lon/Alt : " << Latitude << " / " << Longitude << " / " << h << endl; -cout << "Lat/Lon/Alt VRP: " << LatitudeVRP << " / " << LongitudeVRP << " / " << hVRP << endl << endl; -*/ - DistanceAGL = Radius - RunwayRadius; // Geocentric - - hoverbcg = DistanceAGL/b; - - vMac = State->GetTb2l()*MassBalance->StructuralToBody(Aircraft->GetXYZrp()); - hoverbmac = (DistanceAGL + vMac(3)) / b; - - if (Vt > 0) { - hdot_Vt = RadiusDot/Vt; - if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt); - } else { - gamma = 0.0; - } - - return false; - - } else { - return true; - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGPosition::GetState(void) -{ - dt = State->Getdt(); - - Vt = Translation->GetVt(); - vVel = State->GetTb2l() * Translation->GetUVW(); - vVelDot = State->GetTb2l() * Translation->GetUVWdot(); - - b = Aircraft->GetWingSpan(); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGPosition::Seth(double tt) -{ - h = tt; - Radius = h + SeaLevelRadius; - DistanceAGL = Radius - RunwayRadius; // Geocentric - hoverbcg = DistanceAGL/b; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGPosition::SetDistanceAGL(double tt) -{ - DistanceAGL=tt; - Radius = RunwayRadius + DistanceAGL; - h = Radius - SeaLevelRadius; - hoverbcg = DistanceAGL/b; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGPosition::bind(void) -{ - PropertyManager->Tie("velocities/v-north-fps", this, - &FGPosition::GetVn); - PropertyManager->Tie("velocities/v-east-fps", this, - &FGPosition::GetVe); - PropertyManager->Tie("velocities/v-down-fps", this, - &FGPosition::GetVd); - PropertyManager->Tie("velocities/vg-fps", this, - &FGPosition::GetVground); - PropertyManager->Tie("flight-path/psi-gt-rad", this, - &FGPosition::GetGroundTrack); - PropertyManager->Tie("position/h-sl-ft", this, - &FGPosition::Geth, - &FGPosition::Seth, - true); - PropertyManager->Tie("velocities/h-dot-fps", this, - &FGPosition::Gethdot); - PropertyManager->Tie("position/lat-gc-rad", this, - &FGPosition::GetLatitude, - &FGPosition::SetLatitude); - PropertyManager->Tie("position/lat-dot-gc-rad", this, - &FGPosition::GetLatitudeDot); - PropertyManager->Tie("position/long-gc-rad", this, - &FGPosition::GetLongitude, - &FGPosition::SetLongitude, - true); - PropertyManager->Tie("position/long-dot-gc-rad", this, - &FGPosition::GetLongitudeDot); - PropertyManager->Tie("metrics/runway-radius", this, - &FGPosition::GetRunwayRadius, - &FGPosition::SetRunwayRadius); - PropertyManager->Tie("position/h-agl-ft", this, - &FGPosition::GetDistanceAGL, - &FGPosition::SetDistanceAGL); - PropertyManager->Tie("position/radius-to-vehicle-ft", this, - &FGPosition::GetRadius); - PropertyManager->Tie("flight-path/gamma-rad", this, - &FGPosition::GetGamma, - &FGPosition::SetGamma); - PropertyManager->Tie("aero/h_b-cg-ft", this, - &FGPosition::GetHOverBCG); - PropertyManager->Tie("aero/h_b-mac-ft", this, - &FGPosition::GetHOverBMAC); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGPosition::unbind(void) -{ - PropertyManager->Untie("velocities/v-north-fps"); - PropertyManager->Untie("velocities/v-east-fps"); - PropertyManager->Untie("velocities/v-down-fps"); - PropertyManager->Untie("velocities/vg-fps"); - PropertyManager->Untie("flight-path/psi-gt-rad"); - PropertyManager->Untie("position/h-sl-ft"); - PropertyManager->Untie("velocities/h-dot-fps"); - PropertyManager->Untie("position/lat-gc-rad"); - PropertyManager->Untie("position/lat-dot-gc-rad"); - PropertyManager->Untie("position/long-gc-rad"); - PropertyManager->Untie("position/long-dot-gc-rad"); - PropertyManager->Untie("metrics/runway-radius"); - PropertyManager->Untie("position/h-agl-ft"); - PropertyManager->Untie("position/radius-to-vehicle-ft"); - PropertyManager->Untie("flight-path/gamma-rad"); - PropertyManager->Untie("aero/h_b-cg-ft"); - PropertyManager->Untie("aero/h_b-mac-ft"); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// The bitmasked value choices are as follows: -// unset: In this case (the default) JSBSim would only print -// out the normally expected messages, essentially echoing -// the config files as they are read. If the environment -// variable is not set, debug_lvl is set to 1 internally -// 0: This requests JSBSim not to output any messages -// whatsoever. -// 1: This value explicity requests the normal JSBSim -// startup messages -// 2: This value asks for a message to be printed out when -// a class is instantiated -// 4: When this value is set, a message is displayed when a -// FGModel object executes its Run() method -// 8: When this value is set, various runtime state variables -// are printed out periodically -// 16: When set various parameters are sanity checked and -// a message is printed out when they go out of bounds - -void FGPosition::Debug(int from) -{ - if (debug_lvl <= 0) return; - - if (debug_lvl & 1) { // Standard console startup message output - if (from == 0) { // Constructor - - } - } - if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGPosition" << endl; - if (from == 1) cout << "Destroyed: FGPosition" << endl; - } - if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects - } - if (debug_lvl & 8 ) { // Runtime state variables - } - if (debug_lvl & 16) { // Sanity checking - } - if (debug_lvl & 64) { - if (from == 0) { // Constructor - cout << IdSrc << endl; - cout << IdHdr << endl; - } - } -} -} +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGPosition.cpp + Author: Jon S. Berndt + Date started: 01/05/99 + Purpose: Integrate the EOM to determine instantaneous position + Called by: FGFDMExec + + ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- + + 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. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- +This class encapsulates the integration of rates and accelerations to get the +current position of the aircraft. + +HISTORY +-------------------------------------------------------------------------------- +01/05/99 JSB Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +COMMENTS, REFERENCES, and NOTES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +[1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling + Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate + School, January 1994 +[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices", + JSC 12960, July 1977 +[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at + NASA-Ames", NASA CR-2497, January 1975 +[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics", + Wiley & Sons, 1979 ISBN 0-471-03032-5 +[5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons, + 1982 ISBN 0-471-08936-2 + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifdef FGFS +# include +# ifdef SG_HAVE_STD_INCLUDES +# include +# include +# else +# include +# include +# endif +#else +# if defined(sgi) && !defined(__GNUC__) +# include +# if (_COMPILER_VERSION < 740) +# include +# else +# include +# endif +# else +# include +# include +# endif +#endif + +#include "FGPosition.h" +#include "FGState.h" +#include "FGFDMExec.h" +#include "FGAircraft.h" +#include "FGMassBalance.h" +#include "FGTranslation.h" +#include "FGRotation.h" +#include "FGInertial.h" +#include "FGPropertyManager.h" + +namespace JSBSim { + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_POSITION; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex) +{ + Name = "FGPosition"; + LongitudeDot = LatitudeDot = RadiusDot = 0.0; + + for (int i=0;i<4;i++) { + LatitudeDot_prev[i] = 0.0; + LongitudeDot_prev[i] = 0.0; + RadiusDot_prev[i] = 0.0; + } + + vVRPoffset.InitMatrix(); + + Longitude = Latitude = 0.0; + LongitudeVRP = LatitudeVRP = 0.0; + gamma = Vt = Vground = 0.0; + hoverbmac = hoverbcg = 0.0; + psigt = 0.0; + bind(); + Debug(0); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGPosition::~FGPosition(void) +{ + unbind(); + Debug(1); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGPosition::InitModel(void) +{ + FGModel::InitModel(); + + h = 3.0; // Est. height of aircraft cg off runway + SeaLevelRadius = Inertial->RefRadius(); // For initialization ONLY + Radius = SeaLevelRadius + h; + RunwayRadius = SeaLevelRadius; + DistanceAGL = Radius - RunwayRadius; // Geocentric + vRunwayNormal(3) = -1.0; // Initialized for standalone mode + b = 1; + return true; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +/* +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 reference radius. + In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass. +*/ + +bool FGPosition::Run(void) +{ + double cosLat; + double hdot_Vt; + + if (!FGModel::Run()) { + GetState(); + + Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) ); + + if (vVel(eNorth) == 0) psigt = 0; + else psigt = atan2(vVel(eEast), vVel(eNorth)); + + if (psigt < 0.0) psigt += 2*M_PI; + + Radius = h + SeaLevelRadius; + + cosLat = cos(Latitude); + if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat); + LatitudeDot = vVel(eNorth) / Radius; + RadiusDot = -vVel(eDown); + + Longitude += State->Integrate(FGState::TRAPZ, dt*rate, LongitudeDot, LongitudeDot_prev); + Latitude += State->Integrate(FGState::TRAPZ, dt*rate, LatitudeDot, LatitudeDot_prev); + Radius += State->Integrate(FGState::TRAPZ, dt*rate, RadiusDot, RadiusDot_prev); + + h = Radius - SeaLevelRadius; // Geocentric + + vVRPoffset = State->GetTb2l() * MassBalance->StructuralToBody(Aircraft->GetXYZvrp()); + + // vVRP - the vector to the Visual Reference Point - now contains the + // offset from the CG to the VRP, in units of feet, in the Local coordinate + // frame, where X points north, Y points East, and Z points down. This needs + // to be converted to Lat/Lon/Alt, now. + + if (cosLat != 0) + LongitudeVRP = vVRPoffset(eEast) / (Radius * cosLat) + Longitude; + + LatitudeVRP = vVRPoffset(eNorth) / Radius + Latitude; + hVRP = h - vVRPoffset(eDown); +/* +cout << "Lat/Lon/Alt : " << Latitude << " / " << Longitude << " / " << h << endl; +cout << "Lat/Lon/Alt VRP: " << LatitudeVRP << " / " << LongitudeVRP << " / " << hVRP << endl << endl; +*/ + DistanceAGL = Radius - RunwayRadius; // Geocentric + + hoverbcg = DistanceAGL/b; + + vMac = State->GetTb2l()*MassBalance->StructuralToBody(Aircraft->GetXYZrp()); + hoverbmac = (DistanceAGL + vMac(3)) / b; + + if (Vt > 0) { + hdot_Vt = RadiusDot/Vt; + if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt); + } else { + gamma = 0.0; + } + + return false; + + } else { + return true; + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPosition::GetState(void) +{ + dt = State->Getdt(); + + Vt = Translation->GetVt(); + vVel = State->GetTb2l() * Translation->GetUVW(); + vVelDot = State->GetTb2l() * Translation->GetUVWdot(); + + b = Aircraft->GetWingSpan(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPosition::Seth(double tt) +{ + h = tt; + Radius = h + SeaLevelRadius; + DistanceAGL = Radius - RunwayRadius; // Geocentric + hoverbcg = DistanceAGL/b; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPosition::SetDistanceAGL(double tt) +{ + DistanceAGL=tt; + Radius = RunwayRadius + DistanceAGL; + h = Radius - SeaLevelRadius; + hoverbcg = DistanceAGL/b; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPosition::bind(void) +{ + PropertyManager->Tie("velocities/v-north-fps", this, + &FGPosition::GetVn); + PropertyManager->Tie("velocities/v-east-fps", this, + &FGPosition::GetVe); + PropertyManager->Tie("velocities/v-down-fps", this, + &FGPosition::GetVd); + PropertyManager->Tie("velocities/vg-fps", this, + &FGPosition::GetVground); + PropertyManager->Tie("flight-path/psi-gt-rad", this, + &FGPosition::GetGroundTrack); + PropertyManager->Tie("position/h-sl-ft", this, + &FGPosition::Geth, + &FGPosition::Seth, + true); + PropertyManager->Tie("velocities/h-dot-fps", this, + &FGPosition::Gethdot); + PropertyManager->Tie("position/lat-gc-rad", this, + &FGPosition::GetLatitude, + &FGPosition::SetLatitude); + PropertyManager->Tie("position/lat-dot-gc-rad", this, + &FGPosition::GetLatitudeDot); + PropertyManager->Tie("position/long-gc-rad", this, + &FGPosition::GetLongitude, + &FGPosition::SetLongitude, + true); + PropertyManager->Tie("position/long-dot-gc-rad", this, + &FGPosition::GetLongitudeDot); + PropertyManager->Tie("metrics/runway-radius", this, + &FGPosition::GetRunwayRadius, + &FGPosition::SetRunwayRadius); + PropertyManager->Tie("position/h-agl-ft", this, + &FGPosition::GetDistanceAGL, + &FGPosition::SetDistanceAGL); + PropertyManager->Tie("position/radius-to-vehicle-ft", this, + &FGPosition::GetRadius); + PropertyManager->Tie("flight-path/gamma-rad", this, + &FGPosition::GetGamma, + &FGPosition::SetGamma); + PropertyManager->Tie("aero/h_b-cg-ft", this, + &FGPosition::GetHOverBCG); + PropertyManager->Tie("aero/h_b-mac-ft", this, + &FGPosition::GetHOverBMAC); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPosition::unbind(void) +{ + PropertyManager->Untie("velocities/v-north-fps"); + PropertyManager->Untie("velocities/v-east-fps"); + PropertyManager->Untie("velocities/v-down-fps"); + PropertyManager->Untie("velocities/vg-fps"); + PropertyManager->Untie("flight-path/psi-gt-rad"); + PropertyManager->Untie("position/h-sl-ft"); + PropertyManager->Untie("velocities/h-dot-fps"); + PropertyManager->Untie("position/lat-gc-rad"); + PropertyManager->Untie("position/lat-dot-gc-rad"); + PropertyManager->Untie("position/long-gc-rad"); + PropertyManager->Untie("position/long-dot-gc-rad"); + PropertyManager->Untie("metrics/runway-radius"); + PropertyManager->Untie("position/h-agl-ft"); + PropertyManager->Untie("position/radius-to-vehicle-ft"); + PropertyManager->Untie("flight-path/gamma-rad"); + PropertyManager->Untie("aero/h_b-cg-ft"); + PropertyManager->Untie("aero/h_b-mac-ft"); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// The bitmasked value choices are as follows: +// unset: In this case (the default) JSBSim would only print +// out the normally expected messages, essentially echoing +// the config files as they are read. If the environment +// variable is not set, debug_lvl is set to 1 internally +// 0: This requests JSBSim not to output any messages +// whatsoever. +// 1: This value explicity requests the normal JSBSim +// startup messages +// 2: This value asks for a message to be printed out when +// a class is instantiated +// 4: When this value is set, a message is displayed when a +// FGModel object executes its Run() method +// 8: When this value is set, various runtime state variables +// are printed out periodically +// 16: When set various parameters are sanity checked and +// a message is printed out when they go out of bounds + +void FGPosition::Debug(int from) +{ + if (debug_lvl <= 0) return; + + if (debug_lvl & 1) { // Standard console startup message output + if (from == 0) { // Constructor + + } + } + if (debug_lvl & 2 ) { // Instantiation/Destruction notification + if (from == 0) cout << "Instantiated: FGPosition" << endl; + if (from == 1) cout << "Destroyed: FGPosition" << endl; + } + if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects + } + if (debug_lvl & 8 ) { // Runtime state variables + } + if (debug_lvl & 16) { // Sanity checking + } + if (debug_lvl & 64) { + if (from == 0) { // Constructor + cout << IdSrc << endl; + cout << IdHdr << endl; + } + } +} +} diff --git a/src/FDM/JSBSim/FGPosition.h b/src/FDM/JSBSim/FGPosition.h index bac37730a..009ebc177 100644 --- a/src/FDM/JSBSim/FGPosition.h +++ b/src/FDM/JSBSim/FGPosition.h @@ -39,9 +39,7 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGModel.h" -#include "FGMatrix33.h" #include "FGColumnVector3.h" -#include "FGColumnVector4.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS diff --git a/src/FDM/JSBSim/FGPropeller.cpp b/src/FDM/JSBSim/FGPropeller.cpp index da3fbd41d..3ad669e14 100644 --- a/src/FDM/JSBSim/FGPropeller.cpp +++ b/src/FDM/JSBSim/FGPropeller.cpp @@ -36,7 +36,10 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGPropeller.h" +#include "FGTranslation.h" +#include "FGRotation.h" #include "FGFCS.h" +#include "FGAtmosphere.h" namespace JSBSim { diff --git a/src/FDM/JSBSim/FGPropeller.h b/src/FDM/JSBSim/FGPropeller.h index 8d905772c..25575c555 100644 --- a/src/FDM/JSBSim/FGPropeller.h +++ b/src/FDM/JSBSim/FGPropeller.h @@ -40,8 +40,6 @@ INCLUDES #include "FGThruster.h" #include "FGTable.h" -#include "FGTranslation.h" -#include "FGRotation.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS diff --git a/src/FDM/JSBSim/FGPropertyManager.cpp b/src/FDM/JSBSim/FGPropertyManager.cpp index 84eb46656..2b3843b89 100644 --- a/src/FDM/JSBSim/FGPropertyManager.cpp +++ b/src/FDM/JSBSim/FGPropertyManager.cpp @@ -1,22 +1,22 @@ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - + Header: FGPropertyManager.cpp Author: Tony Peden Based on work originally by David Megginson Date: 2/2002 - + ------------- Copyright (C) 2002 ------------- - + 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. @@ -28,7 +28,6 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#include #include "FGPropertyManager.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -52,19 +51,19 @@ namespace JSBSim { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% string FGPropertyManager::mkPropertyName(string name, bool lowercase) { - + /* do this two pass to avoid problems with characters getting skipped because the index changed */ unsigned i; for(i=0;igetNode(path.c_str(), create); - if(node == 0) - cout << "FGPropertyManager::GetNode() No node found for " + if(node == 0) + cout << "FGPropertyManager::GetNode() No node found for " << path << endl; return (FGPropertyManager*)node; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGPropertyManager* +FGPropertyManager* FGPropertyManager::GetNode (const string &relpath, int index, bool create) { return (FGPropertyManager*)getNode(relpath.c_str(),index,create); -} +} //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -102,7 +101,7 @@ bool FGPropertyManager::HasNode (const string &path) string FGPropertyManager::GetName( void ) { return string( getName() ); -} +} //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -113,23 +112,23 @@ string FGPropertyManager::GetFullyQualifiedName(void) { bool atroot=false; while( !atroot ) { stack.push_back( tmpn->getDisplayName(true) ); - if( !tmpn->getParent() ) + if( !tmpn->getParent() ) atroot=true; - else + else tmpn=tmpn->getParent(); } - + string fqname=""; for(unsigned i=stack.size()-1;i>0;i--) { fqname+= stack[i]; fqname+= "/"; } fqname+= stack[0]; - return fqname; + return fqname; + +} + -} - - //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% bool FGPropertyManager::GetBool (const string &name, bool defaultValue) @@ -221,8 +220,8 @@ void FGPropertyManager::SetArchivable (const string &name, bool state ) SGPropertyNode * node = getNode(name.c_str()); if (node == 0) cout << - "Attempt to set archive flag for non-existant property " - << name << endl; + "Attempt to set archive flag for non-existant property " + << name << endl; else node->setAttribute(SGPropertyNode::ARCHIVE, state); } @@ -234,8 +233,8 @@ void FGPropertyManager::SetReadable (const string &name, bool state ) SGPropertyNode * node = getNode(name.c_str()); if (node == 0) cout << - "Attempt to set read flag for non-existant property " - << name << endl; + "Attempt to set read flag for non-existant property " + << name << endl; else node->setAttribute(SGPropertyNode::READ, state); } @@ -247,8 +246,8 @@ void FGPropertyManager::SetWritable (const string &name, bool state ) SGPropertyNode * node = getNode(name.c_str()); if (node == 0) cout << - "Attempt to set write flag for non-existant property " - << name << endl; + "Attempt to set write flag for non-existant property " + << name << endl; else node->setAttribute(SGPropertyNode::WRITE, state); } @@ -266,53 +265,53 @@ void FGPropertyManager::Untie (const string &name) void FGPropertyManager::Tie (const string &name, bool *pointer, bool useDefault) { if (!tie(name.c_str(), SGRawValuePointer(pointer), - useDefault)) + useDefault)) cout << - "Failed to tie property " << name << " to a pointer" << endl; + "Failed to tie property " << name << " to a pointer" << endl; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropertyManager::Tie (const string &name, int *pointer, +void FGPropertyManager::Tie (const string &name, int *pointer, bool useDefault ) { if (!tie(name.c_str(), SGRawValuePointer(pointer), - useDefault)) + useDefault)) cout << - "Failed to tie property " << name << " to a pointer" << endl; + "Failed to tie property " << name << " to a pointer" << endl; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropertyManager::Tie (const string &name, long *pointer, +void FGPropertyManager::Tie (const string &name, long *pointer, bool useDefault ) { if (!tie(name.c_str(), SGRawValuePointer(pointer), - useDefault)) + useDefault)) cout << - "Failed to tie property " << name << " to a pointer" << endl; + "Failed to tie property " << name << " to a pointer" << endl; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropertyManager::Tie (const string &name, float *pointer, +void FGPropertyManager::Tie (const string &name, float *pointer, bool useDefault ) { if (!tie(name.c_str(), SGRawValuePointer(pointer), - useDefault)) + useDefault)) cout << - "Failed to tie property " << name << " to a pointer" << endl; + "Failed to tie property " << name << " to a pointer" << endl; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropertyManager::Tie (const string &name, double *pointer, +void FGPropertyManager::Tie (const string &name, double *pointer, bool useDefault) { if (!tie(name.c_str(), SGRawValuePointer(pointer), - useDefault)) + useDefault)) cout << - "Failed to tie property " << name << " to a pointer" << endl; + "Failed to tie property " << name << " to a pointer" << endl; } } // namespace JSBSim diff --git a/src/FDM/JSBSim/FGPropertyManager.h b/src/FDM/JSBSim/FGPropertyManager.h index 5e2ee4945..e257e9bc4 100644 --- a/src/FDM/JSBSim/FGPropertyManager.h +++ b/src/FDM/JSBSim/FGPropertyManager.h @@ -1,29 +1,29 @@ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - + Header: FGPropertyManager.h Author: Tony Peden Based on work originally by David Megginson Date: 2/2002 - + ------------- Copyright (C) 2002 ------------- - + 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. - + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SENTRY %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -36,7 +36,11 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include +#ifdef FGFS #include +#else +#include "simgear/props/props.hxx" +#endif /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS @@ -59,7 +63,7 @@ CLASS DOCUMENTATION /** Class wrapper for property handling. @author David Megginson, Tony Peden */ - + /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS DECLARATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -69,17 +73,17 @@ class FGPropertyManager : public SGPropertyNode { /// Constructor FGPropertyManager(void) {} /// Destructor - ~FGPropertyManager(void) {} - + ~FGPropertyManager(void) {} + /** Property-ify a name * replaces spaces with '-' and, optionally, makes name all lower case * @param name string to change * @param lowercase true to change all upper case chars to lower * NOTE: this function changes its argument and thus relies * on pass by value - */ + */ string mkPropertyName(string name, bool lowercase); - + /** * Get a property node. * @@ -87,10 +91,10 @@ class FGPropertyManager : public SGPropertyNode { * @param create true to create the node if it doesn't exist. * @return The node, or 0 if none exists and none was created. */ - FGPropertyManager* + FGPropertyManager* GetNode (const string &path, bool create = false); - - FGPropertyManager* + + FGPropertyManager* GetNode (const string &relpath, int index, bool create = false); /** @@ -105,7 +109,7 @@ class FGPropertyManager : public SGPropertyNode { * Get the name of a node */ string GetName( void ); - + /** * Get the fully qualified name of a node * This function is very slow, so is probably useful for debugging only. @@ -455,25 +459,25 @@ class FGPropertyManager : public SGPropertyNode { Tie (const string &name, double *pointer, bool useDefault = true); //============================================================================ -// -// All of the following functions *must* be inlined, otherwise linker +// +// All of the following functions *must* be inlined, otherwise linker // errors will result // //============================================================================ - + /* template void Tie (const string &name, V (*getter)(), void (*setter)(V) = 0, bool useDefault = true); - + template void Tie (const string &name, int index, V (*getter)(int), void (*setter)(int, V) = 0, bool useDefault = true); - + template void Tie (const string &name, T * obj, V (T::*getter)() const, void (T::*setter)(V) = 0, bool useDefault = true); - template void + template void Tie (const string &name, T * obj, int index, V (T::*getter)(int) const, void (T::*setter)(int, V) = 0, bool useDefault = true); */ @@ -490,12 +494,12 @@ class FGPropertyManager : public SGPropertyNode { * @param name The property name to tie (full path). * @param getter The getter function, or 0 if the value is unreadable. * @param setter The setter function, or 0 if the value is unmodifiable. - * @param useDefault true if the setter should be invoked with any existing + * @param useDefault true if the setter should be invoked with any existing * property value should be; false if the old value should be * discarded; defaults to true. */ - - template inline void + + template inline void Tie (const string &name, V (*getter)(), void (*setter)(V) = 0, bool useDefault = true) { @@ -522,11 +526,11 @@ class FGPropertyManager : public SGPropertyNode { * setter functions. * @param getter The getter function, or 0 if the value is unreadable. * @param setter The setter function, or 0 if the value is unmodifiable. - * @param useDefault true if the setter should be invoked with any existing + * @param useDefault true if the setter should be invoked with any existing * property value should be; false if the old value should be * discarded; defaults to true. */ - template inline void Tie (const string &name, + template inline void Tie (const string &name, int index, V (*getter)(int), void (*setter)(int, V) = 0, bool useDefault = true) { @@ -554,7 +558,7 @@ class FGPropertyManager : public SGPropertyNode { * unreadable. * @param setter The object's setter method, or 0 if the value is * unmodifiable. - * @param useDefault true if the setter should be invoked with any existing + * @param useDefault true if the setter should be invoked with any existing * property value should be; false if the old value should be * discarded; defaults to true. */ @@ -569,7 +573,7 @@ class FGPropertyManager : public SGPropertyNode { "Failed to tie property " << name << " to object methods" << endl; } } - + /** * Tie a property to a pair of indexed object methods. * @@ -585,11 +589,11 @@ class FGPropertyManager : public SGPropertyNode { * setter methods. * @param getter The getter method, or 0 if the value is unreadable. * @param setter The setter method, or 0 if the value is unmodifiable. - * @param useDefault true if the setter should be invoked with any existing + * @param useDefault true if the setter should be invoked with any existing * property value should be; false if the old value should be * discarded; defaults to true. */ - template inline void + template inline void Tie (const string &name, T * obj, int index, V (T::*getter)(int) const, void (T::*setter)(int, V) = 0, bool useDefault = true) @@ -601,7 +605,7 @@ class FGPropertyManager : public SGPropertyNode { "Failed to tie property " << name << " to indexed object methods" << endl; } } -}; +}; } #endif // FGPROPERTYMANAGER_H diff --git a/src/FDM/JSBSim/FGPropulsion.cpp b/src/FDM/JSBSim/FGPropulsion.cpp index 9b2c7f305..bd6f404ac 100644 --- a/src/FDM/JSBSim/FGPropulsion.cpp +++ b/src/FDM/JSBSim/FGPropulsion.cpp @@ -53,6 +53,12 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGPropulsion.h" +#include "FGRocket.h" +#include "FGSimTurbine.h" +#include "FGTurbine.h" +#include "FGPropeller.h" +#include "FGNozzle.h" +#include "FGPiston.h" #include "FGPropertyManager.h" #if defined (__APPLE__) @@ -85,6 +91,7 @@ FGPropulsion::FGPropulsion(FGFDMExec* exec) : FGModel(exec) numOxiTanks = numFuelTanks = 0; dt = 0.0; ActiveEngine = -1; // -1: ALL, 0: Engine 1, 1: Engine 2 ... + tankJ.InitMatrix(); bind(); @@ -119,6 +126,7 @@ bool FGPropulsion::Run(void) vForces += Thrusters[i]->GetBodyForces(); // sum body frame forces vMoments += Thrusters[i]->GetMoments(); // sum body frame moments } + return false; } else { return true; @@ -317,7 +325,7 @@ bool FGPropulsion::Load(FGConfigFile* AC_cfg) Thrusters.push_back(new FGNozzle(FDMExec, &Thruster_cfg )); } else if (thrType == "FG_DIRECT") { Thrusters.push_back(new FGThruster( FDMExec, &Thruster_cfg) ); - } + } AC_cfg->GetNextConfigLine(); while ((token = AC_cfg->GetValue()) != string("/AC_THRUSTER")) { @@ -355,6 +363,7 @@ bool FGPropulsion::Load(FGConfigFile* AC_cfg) AC_cfg->GetNextConfigLine(); } + CalculateTankInertias(); if (!ThrottleAdded) FCS->AddThrottle(); // need to have at least one throttle return true; @@ -484,14 +493,14 @@ string FGPropulsion::GetPropulsionValues(void) FGColumnVector3& FGPropulsion::GetTanksMoment(void) { iTank = Tanks.begin(); - vXYZtank.InitMatrix(); + vXYZtank_arm.InitMatrix(); while (iTank < Tanks.end()) { - vXYZtank(eX) += (*iTank)->GetX()*(*iTank)->GetContents(); - vXYZtank(eY) += (*iTank)->GetY()*(*iTank)->GetContents(); - vXYZtank(eZ) += (*iTank)->GetZ()*(*iTank)->GetContents(); + vXYZtank_arm(eX) += (*iTank)->GetXYZ(eX)*(*iTank)->GetContents(); + vXYZtank_arm(eY) += (*iTank)->GetXYZ(eY)*(*iTank)->GetContents(); + vXYZtank_arm(eZ) += (*iTank)->GetXYZ(eZ)*(*iTank)->GetContents(); iTank++; } - return vXYZtank; + return vXYZtank_arm; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -510,67 +519,20 @@ double FGPropulsion::GetTanksWeight(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGPropulsion::GetTanksIxx(const FGColumnVector3& vXYZcg) +FGMatrix33& FGPropulsion::CalculateTankInertias(void) { - double I = 0.0; - iTank = Tanks.begin(); - while (iTank < Tanks.end()) { - I += ((*iTank)->GetX() - vXYZcg(eX))*((*iTank)->GetX() - vXYZcg(eX)) * (*iTank)->GetContents()/(144.0*Inertial->gravity()); - iTank++; - } - return I; -} + unsigned int size; -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + size = Tanks.size(); + if (size == 0) return tankJ; -double FGPropulsion::GetTanksIyy(const FGColumnVector3& vXYZcg) -{ - double I = 0.0; - iTank = Tanks.begin(); - while (iTank < Tanks.end()) { - I += ((*iTank)->GetY() - vXYZcg(eY))*((*iTank)->GetY() - vXYZcg(eY)) * (*iTank)->GetContents()/(144.0*Inertial->gravity()); - iTank++; - } - return I; -} + tankJ = FGMatrix33(); -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + for (unsigned int i=0; iGetPointmassInertia( lbtoslug * Tanks[i]->GetContents(), + Tanks[i]->GetXYZ() ); -double FGPropulsion::GetTanksIzz(const FGColumnVector3& vXYZcg) -{ - double I = 0.0; - iTank = Tanks.begin(); - while (iTank < Tanks.end()) { - I += ((*iTank)->GetZ() - vXYZcg(eZ))*((*iTank)->GetZ() - vXYZcg(eZ)) * (*iTank)->GetContents()/(144.0*Inertial->gravity()); - iTank++; - } - return I; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -double FGPropulsion::GetTanksIxz(const FGColumnVector3& vXYZcg) -{ - double I = 0.0; - iTank = Tanks.begin(); - while (iTank < Tanks.end()) { - I += ((*iTank)->GetX() - vXYZcg(eX))*((*iTank)->GetZ() - vXYZcg(eZ)) * (*iTank)->GetContents()/(144.0*Inertial->gravity()); - iTank++; - } - return I; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -double FGPropulsion::GetTanksIxy(const FGColumnVector3& vXYZcg) -{ - double I = 0.0; - iTank = Tanks.begin(); - while (iTank != Tanks.end()) { - I += ((*iTank)->GetX() - vXYZcg(eX))*((*iTank)->GetY() - vXYZcg(eY)) * (*iTank)->GetContents()/(144.0*Inertial->gravity()); - iTank++; - } - return I; + return tankJ; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/FGPropulsion.h b/src/FDM/JSBSim/FGPropulsion.h index e392fc3d8..49f8ac8a6 100644 --- a/src/FDM/JSBSim/FGPropulsion.h +++ b/src/FDM/JSBSim/FGPropulsion.h @@ -3,30 +3,30 @@ Header: FGPropulsion.h Author: Jon S. Berndt Date started: 08/20/00 - + ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- - + 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 -------------------------------------------------------------------------------- 08/20/00 JSB Created - + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SENTRY %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -53,14 +53,10 @@ INCLUDES #endif #include "FGModel.h" - -#include "FGRocket.h" -#include "FGPiston.h" -#include "FGTurbine.h" -#include "FGSimTurbine.h" +#include "FGEngine.h" #include "FGTank.h" -#include "FGPropeller.h" -#include "FGNozzle.h" +#include "FGThruster.h" +#include "FGMatrix33.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS @@ -177,11 +173,11 @@ public: /** Loops the engines/thrusters until thrust output steady (used for trimming) */ bool GetSteadyState(void); - + /** starts the engines in IC mode (dt=0). All engine-specific setup must be done before calling this (i.e. magnetos, starter engage, etc.) */ bool ICEngineStart(void); - + string GetPropulsionStrings(void); string GetPropulsionValues(void); @@ -189,16 +185,10 @@ public: inline double GetForces(int n) const { return vForces(n);} inline FGColumnVector3& GetMoments(void) {return vMoments;} inline double GetMoments(int n) const {return vMoments(n);} - + FGColumnVector3& GetTanksMoment(void); double GetTanksWeight(void); - double GetTanksIxx(const FGColumnVector3& vXYZcg); - double GetTanksIyy(const FGColumnVector3& vXYZcg); - double GetTanksIzz(const FGColumnVector3& vXYZcg); - double GetTanksIxz(const FGColumnVector3& vXYZcg); - double GetTanksIxy(const FGColumnVector3& vXYZcg); - inline int GetActiveEngine(void) const { return ActiveEngine; @@ -210,10 +200,11 @@ public: void SetStarter(int setting); void SetCutoff(int setting=0); void SetActiveEngine(int engine); - + FGMatrix33& CalculateTankInertias(void); + void bind(); void unbind(); - + private: vector Engines; vector Tanks; @@ -230,7 +221,9 @@ private: double dt; FGColumnVector3 vForces; FGColumnVector3 vMoments; - FGColumnVector3 vXYZtank; + FGColumnVector3 vTankXYZ; + FGColumnVector3 vXYZtank_arm; + FGMatrix33 tankJ; void Debug(int from); }; } diff --git a/src/FDM/JSBSim/FGRotation.cpp b/src/FDM/JSBSim/FGRotation.cpp index 1b98e75cb..6ecd770b2 100644 --- a/src/FDM/JSBSim/FGRotation.cpp +++ b/src/FDM/JSBSim/FGRotation.cpp @@ -59,13 +59,8 @@ INCLUDES #include "FGAtmosphere.h" #include "FGState.h" #include "FGFDMExec.h" -#include "FGFCS.h" #include "FGAircraft.h" #include "FGMassBalance.h" -#include "FGTranslation.h" -#include "FGPosition.h" -#include "FGAuxiliary.h" -#include "FGOutput.h" #include "FGPropertyManager.h" @@ -92,7 +87,7 @@ FGRotation::FGRotation(FGFDMExec* fdmex) : FGModel(fdmex) vPQRdot_prev[3].InitMatrix(); bind(); - + Debug(0); } @@ -108,22 +103,14 @@ FGRotation::~FGRotation() bool FGRotation::Run(void) { - double L2, N1; double tTheta; if (!FGModel::Run()) { GetState(); - L2 = vMoments(eL) + Ixz*vPQR(eP)*vPQR(eQ) - (Izz-Iyy)*vPQR(eR)*vPQR(eQ); - N1 = vMoments(eN) - (Iyy-Ixx)*vPQR(eP)*vPQR(eQ) - Ixz*vPQR(eR)*vPQR(eQ); - - vPQRdot(eP) = (L2*Izz - N1*Ixz) / (Ixx*Izz - Ixz*Ixz); - vPQRdot(eQ) = (vMoments(eM) - (Ixx-Izz)*vPQR(eP)*vPQR(eR) - - Ixz*(vPQR(eP)*vPQR(eP) - vPQR(eR)*vPQR(eR)))/Iyy; - vPQRdot(eR) = (N1*Ixx + L2*Ixz) / (Ixx*Izz - Ixz*Ixz); - + vPQRdot = MassBalance->GetJinv()*(vMoments - vPQR*(MassBalance->GetJ()*vPQR)); vPQR += State->Integrate(FGState::TRAPZ, dt*rate, vPQRdot, vPQRdot_prev); - + vAeroPQR = vPQR + Atmosphere->GetTurbPQR(); State->IntegrateQuat(vPQR, rate); @@ -155,11 +142,6 @@ void FGRotation::GetState(void) { dt = State->Getdt(); vMoments = Aircraft->GetMoments(); - - Ixx = MassBalance->GetIxx(); - Iyy = MassBalance->GetIyy(); - Izz = MassBalance->GetIzz(); - Ixz = MassBalance->GetIxz(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/FGRotation.h b/src/FDM/JSBSim/FGRotation.h index 832540c8b..85ee9b255 100644 --- a/src/FDM/JSBSim/FGRotation.h +++ b/src/FDM/JSBSim/FGRotation.h @@ -50,9 +50,7 @@ INCLUDES #endif #include "FGModel.h" -#include "FGMatrix33.h" #include "FGColumnVector3.h" -#include "FGColumnVector4.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS @@ -117,11 +115,11 @@ public: vAeroPQR(eQ)=q; vAeroPQR(eR)=r;} inline void SetEuler(FGColumnVector3 tt) {vEuler = tt;} - + inline double Getphi(void) const {return vEuler(1);} inline double Gettht(void) const {return vEuler(2);} inline double Getpsi(void) const {return vEuler(3);} - + inline double GetCosphi(void) const {return cPhi;} inline double GetCostht(void) const {return cTht;} inline double GetCospsi(void) const {return cPsi;} @@ -129,7 +127,7 @@ public: inline double GetSinphi(void) const {return sPhi;} inline double GetSintht(void) const {return sTht;} inline double GetSinpsi(void) const {return sPsi;} - + void bind(void); void unbind(void); @@ -141,12 +139,11 @@ private: FGColumnVector3 vMoments; FGColumnVector3 vEuler; FGColumnVector3 vEulerRates; - + double cTht,sTht; double cPhi,sPhi; double cPsi,sPsi; - - double Ixx, Iyy, Izz, Ixz; + double dt; void GetState(void); diff --git a/src/FDM/JSBSim/FGSimTurbine.cpp b/src/FDM/JSBSim/FGSimTurbine.cpp index 36ff0d4b3..cb653346f 100644 --- a/src/FDM/JSBSim/FGSimTurbine.cpp +++ b/src/FDM/JSBSim/FGSimTurbine.cpp @@ -303,6 +303,7 @@ double FGSimTurbine::Seek(double *var, double target, double accel, double decel void FGSimTurbine::SetDefaults(void) { Name = "Not defined"; + N1 = N2 = 0.0; Type = etSimTurbine; MilThrust = 10000.0; MaxThrust = 10000.0; diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp index 999af7da5..5f8a4ddd8 100644 --- a/src/FDM/JSBSim/FGState.cpp +++ b/src/FDM/JSBSim/FGState.cpp @@ -1,37 +1,37 @@ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - + Module: FGState.cpp Author: Jon Berndt Date started: 11/17/98 Called by: FGFDMExec and accessed by all models. - + ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- - + 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. - + FUNCTIONAL DESCRIPTION -------------------------------------------------------------------------------- See header file. - + HISTORY -------------------------------------------------------------------------------- 11/17/98 JSB Created - + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -70,7 +70,6 @@ FGState::FGState(FGFDMExec* fdex) { FDMExec = fdex; - a = 1000.0; sim_time = 0.0; dt = 1.0/120.0; @@ -89,7 +88,7 @@ FGState::FGState(FGFDMExec* fdex) for(int i=0;i<4;i++) vQdot_prev[i].InitMatrix(); bind(); - + Debug(0); } @@ -114,25 +113,26 @@ void FGState::Initialize(double U, double V, double W, double alpha, beta; double qbar, Vt; FGColumnVector3 vAeroUVW; + FGColumnVector3 vUVW; Position->SetLatitude(Latitude); Position->SetLongitude(Longitude); Position->Seth(H); Atmosphere->Run(); - + vLocalEuler << phi << tht << psi; Rotation->SetEuler(vLocalEuler); InitMatrices(phi, tht, psi); - + vUVW << U << V << W; Translation->SetUVW(vUVW); - + Atmosphere->SetWindNED(wnorth, weast, wdown); - + vAeroUVW = vUVW + mTl2b*Atmosphere->GetWindNED(); - + if (vAeroUVW(eW) != 0.0) alpha = vAeroUVW(eU)*vAeroUVW(eU) > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0; else @@ -164,7 +164,7 @@ void FGState::Initialize(FGInitialCondition *FGIC) double U, V, W, h; double latitude, longitude; double wnorth,weast, wdown; - + latitude = FGIC->GetLatitudeRadIC(); longitude = FGIC->GetLongitudeRadIC(); h = FGIC->GetAltitudeFtIC(); @@ -177,12 +177,12 @@ void FGState::Initialize(FGInitialCondition *FGIC) wnorth = FGIC->GetWindNFpsIC(); weast = FGIC->GetWindEFpsIC(); wdown = FGIC->GetWindDFpsIC(); - + Position->SetSeaLevelRadius( FGIC->GetSeaLevelRadiusFtIC() ); - Position->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() + + Position->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() + FGIC->GetTerrainAltitudeFtIC() ); - // need to fix the wind speed args, here. + // need to fix the wind speed args, here. Initialize(U, V, W, phi, tht, psi, latitude, longitude, h, wnorth, weast, wdown); } @@ -319,10 +319,10 @@ FGMatrix33& FGState::GetTb2s(void) { float alpha,beta; float ca, cb, sa, sb; - + alpha = Translation->Getalpha(); beta = Translation->Getbeta(); - + ca = cos(alpha); sa = sin(alpha); cb = cos(beta); @@ -347,14 +347,14 @@ void FGState::ReportState(void) { #if !defined(__BORLANDCPP__) char out[80], flap[10], gear[12]; - + cout << endl << " JSBSim State" << endl; snprintf(out,80," Weight: %7.0f lbs. CG: %5.1f, %5.1f, %5.1f inches\n", FDMExec->GetMassBalance()->GetWeight(), FDMExec->GetMassBalance()->GetXYZcg(1), FDMExec->GetMassBalance()->GetXYZcg(2), FDMExec->GetMassBalance()->GetXYZcg(3)); - cout << out; + cout << out; if ( FCS->GetDfPos() <= 0.01) snprintf(flap,10,"Up"); else @@ -384,7 +384,7 @@ void FGState::ReportState(void) snprintf(out,80, " Flight Path Angle: %6.2f deg Climb Rate: %5.0f ft/min\n", Position->GetGamma()*radtodeg, Position->Gethdot()*60 ); - cout << out; + cout << out; snprintf(out,80, " Normal Load Factor: %4.2f g's Pitch Rate: %5.2f deg/s\n", Aircraft->GetNlf(), Rotation->GetPQR(2)*radtodeg ); @@ -392,32 +392,32 @@ void FGState::ReportState(void) snprintf(out,80, " Heading: %3.0f deg true Sideslip: %5.2f deg Yaw Rate: %5.2f deg/s\n", Rotation->Getpsi()*radtodeg, Translation->Getbeta()*radtodeg, - Rotation->GetPQR(3)*radtodeg ); + Rotation->GetPQR(3)*radtodeg ); cout << out; snprintf(out,80, " Bank Angle: %5.2f deg Roll Rate: %5.2f deg/s\n", - Rotation->Getphi()*radtodeg, + Rotation->Getphi()*radtodeg, Rotation->GetPQR(1)*radtodeg ); cout << out; snprintf(out,80, " Elevator: %5.2f deg Left Aileron: %5.2f deg Rudder: %5.2f deg\n", FCS->GetDePos(ofRad)*radtodeg, FCS->GetDaLPos(ofRad)*radtodeg, FCS->GetDrPos(ofRad)*radtodeg ); - cout << out; + cout << out; snprintf(out,80, " Throttle: %5.2f%c\n", FCS->GetThrottlePos(0)*100,'%' ); cout << out; - + snprintf(out,80, " Wind Components: %5.2f kts head wind, %5.2f kts cross wind\n", FDMExec->GetAuxiliary()->GetHeadWind()*fpstokts, FDMExec->GetAuxiliary()->GetCrossWind()*fpstokts ); - cout << out; - + cout << out; + snprintf(out,80, " Ground Speed: %4.0f knots , Ground Track: %3.0f deg true\n", Position->GetVground()*fpstokts, Position->GetGroundTrack()*radtodeg ); - cout << out; + cout << out; #endif -} +} //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -428,7 +428,7 @@ void FGState::bind(void) } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - + void FGState::unbind(void) { PropertyManager->Untie("sim-time-sec"); diff --git a/src/FDM/JSBSim/FGState.h b/src/FDM/JSBSim/FGState.h index bb8875c3c..5b43ea502 100644 --- a/src/FDM/JSBSim/FGState.h +++ b/src/FDM/JSBSim/FGState.h @@ -143,9 +143,6 @@ public: */ void Initialize(FGInitialCondition *FGIC); - /// returns the speed of sound in feet per second. - inline double Geta(void) { return a; } - /// Returns the simulation time in seconds. inline double Getsim_time(void) const { return sim_time; } /// Returns the simulation delta T. @@ -156,11 +153,6 @@ public: /// Resumes the simulation by resetting delta T to the correct value. inline void Resume(void) {dt = saved_dt;} - /** Sets the speed of sound. - @param speed the speed of sound in feet per second. - */ - inline void Seta(double speed) { a = speed; } - /** Sets the current sim time. @param cur_time the current time @return the current time. @@ -335,7 +327,6 @@ public: void unbind(); private: - double a; // speed of sound double sim_time, dt; double saved_dt; @@ -347,7 +338,6 @@ private: FGColumnVector4 vQtrn; FGColumnVector4 vQdot_prev[4]; FGColumnVector4 vQdot; - FGColumnVector3 vUVW; FGColumnVector3 vLocalVelNED; FGColumnVector3 vLocalEuler; diff --git a/src/FDM/JSBSim/FGTank.cpp b/src/FDM/JSBSim/FGTank.cpp index ea56e13f4..93092c159 100644 --- a/src/FDM/JSBSim/FGTank.cpp +++ b/src/FDM/JSBSim/FGTank.cpp @@ -56,13 +56,14 @@ CLASS IMPLEMENTATION FGTank::FGTank(FGConfigFile* AC_cfg) { string token; - + double X, Y, Z; + type = AC_cfg->GetValue("TYPE"); if (type == "FUEL") Type = ttFUEL; else if (type == "OXIDIZER") Type = ttOXIDIZER; else Type = ttUNKNOWN; - + AC_cfg->GetNextConfigLine(); while ((token = AC_cfg->GetValue()) != string("/AC_TANK")) { if (token == "XLOC") *AC_cfg >> X; @@ -73,7 +74,9 @@ FGTank::FGTank(FGConfigFile* AC_cfg) else if (token == "CONTENTS") *AC_cfg >> Contents; else cerr << "Unknown identifier: " << token << " in tank definition." << endl; } - + + vXYZ << X << Y << Z; + Selected = true; if (Capacity != 0) { @@ -81,7 +84,7 @@ FGTank::FGTank(FGConfigFile* AC_cfg) } else { Contents = 0; PctFull = 0; - } + } Debug(0); } @@ -137,7 +140,7 @@ void FGTank::Debug(int from) if (from == 0) { // Constructor cout << " " << type << " tank holds " << Capacity << " lbs. " << type << endl; cout << " currently at " << PctFull << "% of maximum capacity" << endl; - cout << " Tank location (X, Y, Z): " << X << ", " << Y << ", " << Z << endl; + cout << " Tank location (X, Y, Z): " << vXYZ(eX) << ", " << vXYZ(eY) << ", " << vXYZ(eZ) << endl; cout << " Effective radius: " << Radius << " inches" << endl; } } diff --git a/src/FDM/JSBSim/FGTank.h b/src/FDM/JSBSim/FGTank.h index c04f7128b..9cd81c58e 100644 --- a/src/FDM/JSBSim/FGTank.h +++ b/src/FDM/JSBSim/FGTank.h @@ -44,8 +44,9 @@ SENTRY INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#include "FGConfigFile.h" #include "FGJSBBase.h" +#include "FGConfigFile.h" +#include "FGColumnVector3.h" #ifdef FGFS # include @@ -98,9 +99,8 @@ public: bool GetSelected(void) {return Selected;} double GetPctFull(void) {return PctFull;} double GetContents(void) {return Contents;} - double inline GetX(void) {return X;} - double inline GetY(void) {return Y;} - double inline GetZ(void) {return Z;} + const FGColumnVector3& GetXYZ(void) {return vXYZ;} + double GetXYZ(int idx) {return vXYZ(idx);} void SetContents(double contents) { Contents = contents; } @@ -109,7 +109,7 @@ public: private: TankType Type; string type; - double X, Y, Z; + FGColumnVector3 vXYZ; double Capacity; double Radius; double PctFull; diff --git a/src/FDM/JSBSim/FGTranslation.cpp b/src/FDM/JSBSim/FGTranslation.cpp index 4042a02d0..f2c84f20f 100644 --- a/src/FDM/JSBSim/FGTranslation.cpp +++ b/src/FDM/JSBSim/FGTranslation.cpp @@ -115,17 +115,7 @@ bool FGTranslation::Run(void) { if (!FGModel::Run()) { - mVel(1,1) = 0.0; - mVel(1,2) = -vUVW(eW); - mVel(1,3) = vUVW(eV); - mVel(2,1) = vUVW(eW); - mVel(2,2) = 0.0; - mVel(2,3) = -vUVW(eU); - mVel(3,1) = -vUVW(eV); - mVel(3,2) = vUVW(eU); - mVel(3,3) = 0.0; - - vUVWdot = mVel*Rotation->GetPQR() + Aircraft->GetBodyAccel(); + vUVWdot = vUVW*Rotation->GetPQR() + Aircraft->GetBodyAccel(); vUVW += State->Integrate(FGState::TRAPZ, State->Getdt()*rate, vUVWdot, vUVWdot_prev); @@ -139,7 +129,6 @@ bool FGTranslation::Run(void) beta = vAeroUVW(eU)*vAeroUVW(eU)+vAeroUVW(eW)*vAeroUVW(eW) > 0.0 ? atan2(vAeroUVW(eV), sqrt(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW))) : 0.0; - // stolen, quite shamelessly, from LaRCsim double mUW = (vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW)); double signU=1; if (vAeroUVW(eU) != 0.0) @@ -160,10 +149,10 @@ bool FGTranslation::Run(void) qbar = 0.5*Atmosphere->GetDensity()*Vt*Vt; qbarUW = 0.5*Atmosphere->GetDensity()*(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW)); qbarUV = 0.5*Atmosphere->GetDensity()*(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eV)*vAeroUVW(eV)); - Mach = Vt / State->Geta(); - vMachUVW(eU) = vAeroUVW(eU) / State->Geta(); - vMachUVW(eV) = vAeroUVW(eV) / State->Geta(); - vMachUVW(eW) = vAeroUVW(eW) / State->Geta(); + Mach = Vt / Atmosphere->GetSoundSpeed(); + vMachUVW(eU) = vAeroUVW(eU) / Atmosphere->GetSoundSpeed(); + vMachUVW(eV) = vAeroUVW(eV) / Atmosphere->GetSoundSpeed(); + vMachUVW(eW) = vAeroUVW(eW) / Atmosphere->GetSoundSpeed(); if (debug_lvl > 1) Debug(1); diff --git a/src/FDM/JSBSim/FGTranslation.h b/src/FDM/JSBSim/FGTranslation.h index 6442b30f0..70766f473 100644 --- a/src/FDM/JSBSim/FGTranslation.h +++ b/src/FDM/JSBSim/FGTranslation.h @@ -54,9 +54,7 @@ INCLUDES #endif #include "FGModel.h" -#include "FGMatrix33.h" #include "FGColumnVector3.h" -#include "FGColumnVector4.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS @@ -142,13 +140,11 @@ private: FGColumnVector3 vUVW; FGColumnVector3 vUVWdot; FGColumnVector3 vUVWdot_prev[4]; - FGMatrix33 mVel; FGColumnVector3 vAeroUVW; FGColumnVector3 vMachUVW; double Vt, Mach; double qbar, qbarUW, qbarUV; - double dt; double alpha, beta; double adot,bdot; void Debug(int from); diff --git a/src/FDM/JSBSim/FGTrim.cpp b/src/FDM/JSBSim/FGTrim.cpp index 5c7e37e0a..83ff7351e 100644 --- a/src/FDM/JSBSim/FGTrim.cpp +++ b/src/FDM/JSBSim/FGTrim.cpp @@ -52,8 +52,11 @@ INCLUDES #include "FGTrim.h" #include "FGAircraft.h" #include "FGMassBalance.h" +#include "FGGroundReactions.h" +#include "FGInertial.h" #include "FGAerodynamics.h" #include "FGColumnVector3.h" + #if _MSC_VER #pragma warning (disable : 4786 4788) #endif @@ -548,7 +551,6 @@ bool FGTrim::checkLimits(void) { void FGTrim::setupPullup() { float g,q,cgamma; - FGColumnVector3 vPQR; g=fdmex->GetInertial()->gravity(); cgamma=cos(fgic->GetFlightPathAngleRadIC()); cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", " @@ -594,7 +596,6 @@ void FGTrim::updateRates(void){ fdmex->GetRotation()->SetPQR(p,q,r); } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) { float g,q,cgamma; - FGColumnVector3 vPQR; g=fdmex->GetInertial()->gravity(); cgamma=cos(fgic->GetFlightPathAngleRadIC()); q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC(); diff --git a/src/FDM/JSBSim/JSBSim.cxx b/src/FDM/JSBSim/JSBSim.cxx index 97ecdd285..61b8fdec4 100644 --- a/src/FDM/JSBSim/JSBSim.cxx +++ b/src/FDM/JSBSim/JSBSim.cxx @@ -62,6 +62,11 @@ #include #include #include +#include +#include +#include +#include +#include #include #include "JSBSim.hxx" @@ -74,11 +79,11 @@ FMAX (double a, double b) /******************************************************************************/ -FGJSBsim::FGJSBsim( double dt ) +FGJSBsim::FGJSBsim( double dt ) : FGInterface(dt) { bool result; - + // Set up the debugging level // FIXME: this will not respond to // runtime changes @@ -105,7 +110,7 @@ FGJSBsim::FGJSBsim( double dt ) } fdmex = new FGFDMExec( (FGPropertyManager*)globals->get_props() ); - + State = fdmex->GetState(); Atmosphere = fdmex->GetAtmosphere(); FCS = fdmex->GetFCS(); @@ -117,11 +122,11 @@ FGJSBsim::FGJSBsim( double dt ) Position = fdmex->GetPosition(); Auxiliary = fdmex->GetAuxiliary(); Aerodynamics = fdmex->GetAerodynamics(); - GroundReactions = fdmex->GetGroundReactions(); - + GroundReactions = fdmex->GetGroundReactions(); + fgic=fdmex->GetIC(); needTrim=true; - + SGPath aircraft_path( globals->get_fg_root() ); aircraft_path.append( "Aircraft" ); @@ -132,7 +137,7 @@ FGJSBsim::FGJSBsim( double dt ) result = fdmex->LoadModel( aircraft_path.str(), engine_path.str(), fgGetString("/sim/aero") ); - + if (result) { SG_LOG( SG_FLIGHT, SG_INFO, " loaded aero."); } else { @@ -147,7 +152,7 @@ FGJSBsim::FGJSBsim( double dt ) int Neng = Propulsion->GetNumEngines(); SG_LOG( SG_FLIGHT, SG_INFO, "num engines = " << Neng ); - + if ( GroundReactions->GetNumGearUnits() <= 0 ) { SG_LOG( SG_FLIGHT, SG_ALERT, "num gear units = " << GroundReactions->GetNumGearUnits() ); @@ -169,7 +174,7 @@ FGJSBsim::FGJSBsim( double dt ) node->setDoubleValue("level-gal_us", Propulsion->GetTank(i)->GetContents() / 6.6); } } - + fgSetDouble("/fdm/trim/pitch-trim", FCS->GetPitchTrimCmd()); fgSetDouble("/fdm/trim/throttle", FCS->GetThrottleCmd(0)); fgSetDouble("/fdm/trim/aileron", FCS->GetDaCmd()); @@ -184,10 +189,10 @@ FGJSBsim::FGJSBsim( double dt ) throttle_trim = fgGetNode("/fdm/trim/throttle", true ); aileron_trim = fgGetNode("/fdm/trim/aileron", true ); rudder_trim = fgGetNode("/fdm/trim/rudder", true ); - + stall_warning = fgGetNode("/sim/alarms/stall-warning",true); stall_warning->setDoubleValue(0); - + flap_pos_pct=fgGetNode("/surface-positions/flap-pos-norm",true); elevator_pos_pct=fgGetNode("/surface-positions/elevator-pos-norm",true); @@ -199,7 +204,7 @@ FGJSBsim::FGJSBsim( double dt ) speedbrake_pos_pct =fgGetNode("/surface-positions/speedbrake-pos-norm",true); spoilers_pos_pct=fgGetNode("/surface-positions/spoilers-pos-norm",true); - + elevator_pos_pct->setDoubleValue(0); left_aileron_pos_pct->setDoubleValue(0); right_aileron_pos_pct->setDoubleValue(0); @@ -213,7 +218,7 @@ FGJSBsim::FGJSBsim( double dt ) density = fgGetNode("/environment/density-slugft3",true); turbulence_gain = fgGetNode("/environment/turbulence/magnitude-norm",true); turbulence_rate = fgGetNode("/environment/turbulence/rate-hz",true); - + wind_from_north= fgGetNode("/environment/wind-from-north-fps",true); wind_from_east = fgGetNode("/environment/wind-from-east-fps" ,true); wind_from_down = fgGetNode("/environment/wind-from-down-fps" ,true); @@ -239,7 +244,7 @@ FGJSBsim::~FGJSBsim(void) void FGJSBsim::init() { double tmp; - + SG_LOG( SG_FLIGHT, SG_INFO, "Starting and initializing JSBsim" ); // Explicitly call the superclass's @@ -265,7 +270,7 @@ void FGJSBsim::init() Atmosphere->UseInternal(); } #endif - + fgic->SetVnorthFpsIC( wind_from_north->getDoubleValue() ); fgic->SetVeastFpsIC( wind_from_east->getDoubleValue() ); fgic->SetVdownFpsIC( wind_from_down->getDoubleValue() ); @@ -274,7 +279,7 @@ void FGJSBsim::init() //Atmosphere->SetExPressure(get_Static_pressure()); //Atmosphere->SetExDensity(get_Density()); SG_LOG(SG_FLIGHT,SG_INFO,"T,p,rho: " << fdmex->GetAtmosphere()->GetTemperature() - << ", " << fdmex->GetAtmosphere()->GetPressure() + << ", " << fdmex->GetAtmosphere()->GetPressure() << ", " << fdmex->GetAtmosphere()->GetDensity() ); common_init(); @@ -308,9 +313,9 @@ void FGJSBsim::init() << Auxiliary->GetVcalibratedKTS() << " knots" ); break; } - + stall_warning->setDoubleValue(0); - + SG_LOG( SG_FLIGHT, SG_INFO, " Bank Angle: " << Rotation->Getphi()*RADTODEG << " deg" ); SG_LOG( SG_FLIGHT, SG_INFO, " Pitch Angle: " @@ -328,8 +333,8 @@ void FGJSBsim::init() SG_LOG( SG_FLIGHT, SG_INFO, " set dt" ); SG_LOG( SG_FLIGHT, SG_INFO, "Finished initializing JSBSim" ); - - SG_LOG( SG_FLIGHT, SG_INFO, "FGControls::get_gear_down()= " << + + SG_LOG( SG_FLIGHT, SG_INFO, "FGControls::get_gear_down()= " << globals->get_controls()->get_gear_down() ); } @@ -351,20 +356,20 @@ void FGJSBsim::update( double dt ) copy_to_JSBsim(); trimmed->setBoolValue(false); - + if ( needTrim ) { if ( startup_trim->getBoolValue() ) { SG_LOG(SG_FLIGHT, SG_INFO, - "Ready to trim, terrain altitude is: " + "Ready to trim, terrain altitude is: " << cur_fdm_state->get_Runway_altitude() * SG_METER_TO_FEET ); fgic->SetTerrainAltitudeFtIC( cur_fdm_state->get_ground_elev_ft() ); do_trim(); } else { fdmex->RunIC(); //apply any changes made through the set_ functions } - needTrim = false; - } - + needTrim = false; + } + for ( i=0; i < multiloop; i++ ) { fdmex->Run(); } @@ -414,8 +419,8 @@ bool FGJSBsim::copy_to_JSBsim() FCS->SetDrCmd( -globals->get_controls()->get_rudder() ); FCS->SetYawTrimCmd( -globals->get_controls()->get_rudder_trim() ); FCS->SetDfCmd( globals->get_controls()->get_flaps() ); - FCS->SetDsbCmd( globals->get_controls()->get_speedbrake() ); - FCS->SetDspCmd( globals->get_controls()->get_spoilers() ); + FCS->SetDsbCmd( globals->get_controls()->get_speedbrake() ); + FCS->SetDspCmd( globals->get_controls()->get_spoilers() ); // Parking brake sets minimum braking // level for mains. @@ -467,7 +472,7 @@ bool FGJSBsim::copy_to_JSBsim() _set_Runway_altitude( cur_fdm_state->get_Runway_altitude() ); Position->SetSeaLevelRadius( get_Sea_level_radius() ); - Position->SetRunwayRadius( get_Runway_altitude() + Position->SetRunwayRadius( get_Runway_altitude() + get_Sea_level_radius() ); Atmosphere->SetExTemperature( @@ -506,13 +511,13 @@ bool FGJSBsim::copy_to_JSBsim() bool FGJSBsim::copy_from_JSBsim() { unsigned int i, j; - +/* _set_Inertias( MassBalance->GetMass(), MassBalance->GetIxx(), MassBalance->GetIyy(), MassBalance->GetIzz(), MassBalance->GetIxz() ); - +*/ _set_CG_Position( MassBalance->GetXYZcg(1), MassBalance->GetXYZcg(2), MassBalance->GetXYZcg(3) ); @@ -524,7 +529,7 @@ bool FGJSBsim::copy_from_JSBsim() _set_Accels_CG_Body_N ( Aircraft->GetNcg(1), Aircraft->GetNcg(2), Aircraft->GetNcg(3) ); - + _set_Accels_Pilot_Body( Auxiliary->GetPilotAccel(1), Auxiliary->GetPilotAccel(2), Auxiliary->GetPilotAccel(3) ); @@ -541,6 +546,11 @@ bool FGJSBsim::copy_from_JSBsim() Translation->GetUVW(2), Translation->GetUVW(3) ); + // Make the HUD work ... + _set_Velocities_Ground( Position->GetVn(), + Position->GetVe(), + -Position->GetVd() ); + _set_V_rel_wind( Translation->GetVt() ); _set_V_equiv_kts( Auxiliary->GetVequivalentKTS() ); @@ -633,6 +643,7 @@ bool FGJSBsim::copy_from_JSBsim() node->setBoolValue("ignition", eng->GetIgnition()); node->setDoubleValue("nozzle-pos-norm", eng->GetNozzle()); node->setDoubleValue("inlet-pos-norm", eng->GetInlet()); + node->setDoubleValue("oil-pressure-psi", eng->getOilPressure_psi()); node->setBoolValue("reversed", eng->GetReversed()); node->setBoolValue("cutoff", eng->GetCutoff()); globals->get_controls()->set_reverser(i, eng->GetReversed() ); @@ -665,7 +676,7 @@ bool FGJSBsim::copy_from_JSBsim() FGPropeller* prop = (FGPropeller*)thruster; tnode->setDoubleValue("rpm", thruster->GetRPM()); tnode->setDoubleValue("pitch", prop->GetPitch()); - tnode->setDoubleValue("torque", prop->GetTorque()); + tnode->setDoubleValue("torque", prop->GetTorque()); } // end FGPropeller code block break; case FGThruster::ttRotor: @@ -691,22 +702,22 @@ bool FGJSBsim::copy_from_JSBsim() double contents = Propulsion->GetTank(i)->GetContents(); node->setDoubleValue("level-gal_us", contents/6.6); node->setDoubleValue("level-lb", contents); - // node->setDoubleValue("temperature_degC", + // node->setDoubleValue("temperature_degC", } } update_gear(); - + stall_warning->setDoubleValue( Aerodynamics->GetStallWarn() ); - + elevator_pos_pct->setDoubleValue( FCS->GetDePos(ofNorm) ); left_aileron_pos_pct->setDoubleValue( FCS->GetDaLPos(ofNorm) ); - right_aileron_pos_pct->setDoubleValue( -1*FCS->GetDaLPos(ofNorm) ); + right_aileron_pos_pct->setDoubleValue( FCS->GetDaRPos(ofNorm) ); rudder_pos_pct->setDoubleValue( -1*FCS->GetDrPos(ofNorm) ); flap_pos_pct->setDoubleValue( FCS->GetDfPos(ofNorm) ); speedbrake_pos_pct->setDoubleValue( FCS->GetDsbPos(ofNorm) ); spoilers_pos_pct->setDoubleValue( FCS->GetDspPos(ofNorm) ); - + return true; } @@ -738,21 +749,21 @@ void FGJSBsim::set_Latitude(double lat) // In case we're not trimming FGInterface::set_Latitude(lat); - + if ( altitude->getDoubleValue() > -9990 ) { alt = altitude->getDoubleValue(); } else { alt = 0.0; } - + update_ic(); SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Latitude: " << lat ); SG_LOG(SG_FLIGHT,SG_INFO," cur alt (ft) = " << alt ); - sgGeodToGeoc( lat, alt * SG_FEET_TO_METER, + sgGeodToGeoc( lat, alt * SG_FEET_TO_METER, &sea_level_radius_meters, &lat_geoc ); _set_Sea_level_radius( sea_level_radius_meters * SG_METER_TO_FEET ); - fgic->SetSeaLevelRadiusFtIC( sea_level_radius_meters * SG_METER_TO_FEET ); + fgic->SetSeaLevelRadiusFtIC( sea_level_radius_meters * SG_METER_TO_FEET ); _set_Runway_altitude( cur_fdm_state->get_Runway_altitude() ); fgic->SetTerrainAltitudeFtIC( cur_fdm_state->get_ground_elev_ft() ); fgic->SetLatitudeRadIC( lat_geoc ); @@ -782,7 +793,7 @@ void FGJSBsim::set_Altitude(double alt) SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Altitude: " << alt ); SG_LOG(SG_FLIGHT,SG_INFO, " lat (deg) = " << latitude->getDoubleValue() ); - + // In case we're not trimming FGInterface::set_Altitude(alt); @@ -815,7 +826,7 @@ void FGJSBsim::set_V_calibrated_kts(double vc) void FGJSBsim::set_Mach_number(double mach) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Mach_number: " << mach ); - + // In case we're not trimming FGInterface::set_Mach_number(mach); @@ -828,7 +839,7 @@ void FGJSBsim::set_Velocities_Local( double north, double east, double down ) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local: " << north << ", " << east << ", " << down ); - + // In case we're not trimming FGInterface::set_Velocities_Local(north, east, down); @@ -843,7 +854,7 @@ void FGJSBsim::set_Velocities_Wind_Body( double u, double v, double w) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Wind_Body: " << u << ", " << v << ", " << w ); - + // In case we're not trimming FGInterface::set_Velocities_Wind_Body(u, v, w); @@ -859,7 +870,7 @@ void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi ) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Euler_Angles: " << phi << ", " << theta << ", " << psi ); - + // In case we're not trimming FGInterface::set_Euler_Angles(phi, theta, psi); @@ -874,7 +885,7 @@ void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi ) void FGJSBsim::set_Climb_Rate( double roc) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Climb_Rate: " << roc ); - + // In case we're not trimming FGInterface::set_Climb_Rate(roc); @@ -884,18 +895,18 @@ void FGJSBsim::set_Climb_Rate( double roc) //out the other. if( !(fabs(roc) > 1 && fabs(fgic->GetFlightPathAngleRadIC()) < 0.01) ) { fgic->SetClimbRateFpsIC(roc); - } + } needTrim=true; } void FGJSBsim::set_Gamma_vert_rad( double gamma) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Gamma_vert_rad: " << gamma ); - + update_ic(); if( !(fabs(gamma) < 0.01 && fabs(fgic->GetClimbRateFpsIC()) > 1) ) { fgic->SetFlightPathAngleRadIC(gamma); - } + } needTrim=true; } @@ -915,7 +926,7 @@ void FGJSBsim::init_gear(void ) node->setBoolValue("has-brake", gr->GetGearUnit(i)->GetBrakeGroup() > 0); node->setDoubleValue("position-norm", FCS->GetGearPos()); node->setDoubleValue("tire-pressure-norm", gr->GetGearUnit(i)->GetTirePressure()); - } + } } void FGJSBsim::update_gear(void) @@ -927,7 +938,7 @@ void FGJSBsim::update_gear(void) node->getChild("wow", 0, true)->setBoolValue(gr->GetGearUnit(i)->GetWOW()); node->getChild("position-norm", 0, true)->setDoubleValue(FCS->GetGearPos()); gr->GetGearUnit(i)->SetTirePressure(node->getDoubleValue("tire-pressure-norm")); - } + } } void FGJSBsim::do_trim(void) @@ -967,19 +978,19 @@ void FGJSBsim::do_trim(void) globals->get_controls()->set_rudder( FCS->GetDrCmd()); SG_LOG( SG_FLIGHT, SG_INFO, " Trim complete" ); -} +} void FGJSBsim::update_ic(void) { if ( !needTrim ) { - fgic->SetLatitudeRadIC(get_Lat_geocentric() ); - fgic->SetLongitudeRadIC( get_Longitude() ); - fgic->SetAltitudeFtIC( get_Altitude() ); - fgic->SetVcalibratedKtsIC( get_V_calibrated_kts() ); - fgic->SetPitchAngleRadIC( get_Theta() ); - fgic->SetRollAngleRadIC( get_Phi() ); - fgic->SetTrueHeadingRadIC( get_Psi() ); + fgic->SetLatitudeRadIC(get_Lat_geocentric() ); + fgic->SetLongitudeRadIC( get_Longitude() ); + fgic->SetAltitudeFtIC( get_Altitude() ); + fgic->SetVcalibratedKtsIC( get_V_calibrated_kts() ); + fgic->SetPitchAngleRadIC( get_Theta() ); + fgic->SetRollAngleRadIC( get_Phi() ); + fgic->SetTrueHeadingRadIC( get_Psi() ); fgic->SetClimbRateFpsIC( get_Climb_Rate() ); - } + } } diff --git a/src/FDM/JSBSim/filtersjb/FGCondition.cpp b/src/FDM/JSBSim/filtersjb/FGCondition.cpp index 0cf9f00cf..bcb3b1bd7 100644 --- a/src/FDM/JSBSim/filtersjb/FGCondition.cpp +++ b/src/FDM/JSBSim/filtersjb/FGCondition.cpp @@ -1,258 +1,257 @@ -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - Module: FGCondition.cpp - Author: Jon S. Berndt - Date started: 1/2/2003 - - -------------- Copyright (C) 2003 Jon S. Berndt (jsb@hal-pc.org) -------------- - - 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 --------------------------------------------------------------------------------- - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -COMMENTS, REFERENCES, and NOTES -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -INCLUDES -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -#include "FGCondition.h" - -namespace JSBSim { - -static const char *IdSrc = "$Id$"; -static const char *IdHdr = ID_CONDITION; - -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -CLASS IMPLEMENTATION -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -string FGCondition::indent = " "; - - -FGCondition::FGCondition(FGConfigFile* AC_cfg, FGPropertyManager* PropertyManager) : - PropertyManager(PropertyManager) -{ - string property1, property2; - - mComparison["EQ"] = eEQ; - mComparison["NE"] = eNE; - mComparison["GT"] = eGT; - mComparison["GE"] = eGE; - mComparison["LT"] = eLT; - mComparison["LE"] = eLE; - mComparison["=="] = eEQ; - mComparison["!="] = eNE; - mComparison[">"] = eGT; - mComparison[">="] = eGE; - mComparison["<"] = eLT; - mComparison["<="] = eLE; - - TestParam1 = TestParam2 = 0L; - TestValue = 0.0; - Comparison = ecUndef; - Logic = elUndef; - conditions.clear(); - - if (AC_cfg->GetValue("CONDITION_GROUP").empty()) { // define a condition - - *AC_cfg >> property1 >> conditional >> property2; - TestParam1 = PropertyManager->GetNode(property1, true); - Comparison = mComparison[conditional]; - - if (property2.find_first_not_of("-.0123456789eE") == string::npos) { - TestValue = atof(property2.c_str()); - } else { - TestParam2 = PropertyManager->GetNode(property2, true); - } - - isGroup = false; - - } else { // define a condition group - - if (AC_cfg->GetValue("LOGIC") == "OR") Logic = eOR; - else if (AC_cfg->GetValue("LOGIC") == "AND") Logic = eAND; - - AC_cfg->GetNextConfigLine(); - while (AC_cfg->GetValue() != string("/CONDITION_GROUP")) { - conditions.push_back(*(new FGCondition(AC_cfg, PropertyManager))); - } - isGroup = true; - AC_cfg->GetNextConfigLine(); - } - - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGCondition::~FGCondition(void) -{ - Debug(1); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -bool FGCondition::Evaluate(void ) -{ - vector ::iterator iConditions; - bool pass = false; - double compareValue; - - if (Logic == eAND) { - - iConditions = conditions.begin(); - pass = true; - while (iConditions < conditions.end()) { - if (!iConditions->Evaluate()) pass = false; - *iConditions++; - } - - } else if (Logic == eOR) { - - pass = false; - while (iConditions < conditions.end()) { - if (iConditions->Evaluate()) pass = true; - *iConditions++; - } - - } else { - - if (TestParam2 != 0L) compareValue = TestParam2->getDoubleValue(); - else compareValue = TestValue; - - switch (Comparison) { - case ecUndef: - cerr << "Undefined comparison operator." << endl; - break; - case eEQ: - pass = TestParam1->getDoubleValue() == compareValue; - break; - case eNE: - pass = TestParam1->getDoubleValue() != compareValue; - break; - case eGT: - pass = TestParam1->getDoubleValue() > compareValue; - break; - case eGE: - pass = TestParam1->getDoubleValue() >= compareValue; - break; - case eLT: - pass = TestParam1->getDoubleValue() < compareValue; - break; - case eLE: - pass = TestParam1->getDoubleValue() <= compareValue; - break; - default: - cerr << "Unknown comparison operator." << endl; - } - } - - return pass; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGCondition::PrintCondition(void ) -{ - vector ::iterator iConditions; - string scratch; - bool first = false; - - if (isGroup) { - switch(Logic) { - case (elUndef): - scratch = " UNSET"; - cerr << "unset logic for test condition" << endl; - break; - case (eAND): - scratch = " if all of the following are true"; - break; - case (eOR): - scratch = " if any of the following are true:"; - break; - default: - scratch = " UNKNOWN"; - cerr << "Unknown logic for test condition" << endl; - } - - iConditions = conditions.begin(); - cout << scratch << endl; - while (iConditions < conditions.end()) { - iConditions->PrintCondition(); - *iConditions++; - } - } else { - if (TestParam2 != 0L) - cout << TestParam1->GetName() << " " << conditional << " " << TestParam2->GetName(); - else - cout << TestParam1->GetName() << " " << conditional << " " << TestValue; - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// The bitmasked value choices are as follows: -// unset: In this case (the default) JSBSim would only print -// out the normally expected messages, essentially echoing -// the config files as they are read. If the environment -// variable is not set, debug_lvl is set to 1 internally -// 0: This requests JSBSim not to output any messages -// whatsoever. -// 1: This value explicity requests the normal JSBSim -// startup messages -// 2: This value asks for a message to be printed out when -// a class is instantiated -// 4: When this value is set, a message is displayed when a -// FGModel object executes its Run() method -// 8: When this value is set, various runtime state variables -// are printed out periodically -// 16: When set various parameters are sanity checked and -// a message is printed out when they go out of bounds - -void FGCondition::Debug(int from) -{ - if (debug_lvl <= 0) return; - - if (debug_lvl & 1) { // Standard console startup message output - if (from == 0) { // Constructor - - } - } - if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGCondition" << endl; - if (from == 1) cout << "Destroyed: FGCondition" << endl; - } - if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects - } - if (debug_lvl & 8 ) { // Runtime state variables - } - if (debug_lvl & 16) { // Sanity checking - } - if (debug_lvl & 64) { - if (from == 0) { // Constructor - cout << IdSrc << endl; - cout << IdHdr << endl; - } - } -} - -} //namespace JSBSim - +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGCondition.cpp + Author: Jon S. Berndt + Date started: 1/2/2003 + + -------------- Copyright (C) 2003 Jon S. Berndt (jsb@hal-pc.org) -------------- + + 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 +-------------------------------------------------------------------------------- + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +COMMENTS, REFERENCES, and NOTES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGCondition.h" + +namespace JSBSim { + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_CONDITION; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +string FGCondition::indent = " "; + + +FGCondition::FGCondition(FGConfigFile* AC_cfg, FGPropertyManager* PropertyManager) : + PropertyManager(PropertyManager) +{ + string property1, property2; + + mComparison["EQ"] = eEQ; + mComparison["NE"] = eNE; + mComparison["GT"] = eGT; + mComparison["GE"] = eGE; + mComparison["LT"] = eLT; + mComparison["LE"] = eLE; + mComparison["=="] = eEQ; + mComparison["!="] = eNE; + mComparison[">"] = eGT; + mComparison[">="] = eGE; + mComparison["<"] = eLT; + mComparison["<="] = eLE; + + TestParam1 = TestParam2 = 0L; + TestValue = 0.0; + Comparison = ecUndef; + Logic = elUndef; + conditions.clear(); + + if (AC_cfg->GetValue("CONDITION_GROUP").empty()) { // define a condition + + *AC_cfg >> property1 >> conditional >> property2; + TestParam1 = PropertyManager->GetNode(property1, true); + Comparison = mComparison[conditional]; + + if (property2.find_first_not_of("-.0123456789eE") == string::npos) { + TestValue = atof(property2.c_str()); + } else { + TestParam2 = PropertyManager->GetNode(property2, true); + } + + isGroup = false; + + } else { // define a condition group + + if (AC_cfg->GetValue("LOGIC") == "OR") Logic = eOR; + else if (AC_cfg->GetValue("LOGIC") == "AND") Logic = eAND; + + AC_cfg->GetNextConfigLine(); + while (AC_cfg->GetValue() != string("/CONDITION_GROUP")) { + conditions.push_back(FGCondition(AC_cfg, PropertyManager)); + } + isGroup = true; + AC_cfg->GetNextConfigLine(); + } + + Debug(0); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGCondition::~FGCondition(void) +{ + Debug(1); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGCondition::Evaluate(void ) +{ + vector ::iterator iConditions; + bool pass = false; + double compareValue; + + if (Logic == eAND) { + + iConditions = conditions.begin(); + pass = true; + while (iConditions < conditions.end()) { + if (!iConditions->Evaluate()) pass = false; + *iConditions++; + } + + } else if (Logic == eOR) { + + pass = false; + while (iConditions < conditions.end()) { + if (iConditions->Evaluate()) pass = true; + *iConditions++; + } + + } else { + + if (TestParam2 != 0L) compareValue = TestParam2->getDoubleValue(); + else compareValue = TestValue; + + switch (Comparison) { + case ecUndef: + cerr << "Undefined comparison operator." << endl; + break; + case eEQ: + pass = TestParam1->getDoubleValue() == compareValue; + break; + case eNE: + pass = TestParam1->getDoubleValue() != compareValue; + break; + case eGT: + pass = TestParam1->getDoubleValue() > compareValue; + break; + case eGE: + pass = TestParam1->getDoubleValue() >= compareValue; + break; + case eLT: + pass = TestParam1->getDoubleValue() < compareValue; + break; + case eLE: + pass = TestParam1->getDoubleValue() <= compareValue; + break; + default: + cerr << "Unknown comparison operator." << endl; + } + } + + return pass; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGCondition::PrintCondition(void ) +{ + vector ::iterator iConditions; + string scratch; + + if (isGroup) { + switch(Logic) { + case (elUndef): + scratch = " UNSET"; + cerr << "unset logic for test condition" << endl; + break; + case (eAND): + scratch = " if all of the following are true"; + break; + case (eOR): + scratch = " if any of the following are true:"; + break; + default: + scratch = " UNKNOWN"; + cerr << "Unknown logic for test condition" << endl; + } + + iConditions = conditions.begin(); + cout << scratch << endl; + while (iConditions < conditions.end()) { + iConditions->PrintCondition(); + *iConditions++; + } + } else { + if (TestParam2 != 0L) + cout << TestParam1->GetName() << " " << conditional << " " << TestParam2->GetName(); + else + cout << TestParam1->GetName() << " " << conditional << " " << TestValue; + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// The bitmasked value choices are as follows: +// unset: In this case (the default) JSBSim would only print +// out the normally expected messages, essentially echoing +// the config files as they are read. If the environment +// variable is not set, debug_lvl is set to 1 internally +// 0: This requests JSBSim not to output any messages +// whatsoever. +// 1: This value explicity requests the normal JSBSim +// startup messages +// 2: This value asks for a message to be printed out when +// a class is instantiated +// 4: When this value is set, a message is displayed when a +// FGModel object executes its Run() method +// 8: When this value is set, various runtime state variables +// are printed out periodically +// 16: When set various parameters are sanity checked and +// a message is printed out when they go out of bounds + +void FGCondition::Debug(int from) +{ + if (debug_lvl <= 0) return; + + if (debug_lvl & 1) { // Standard console startup message output + if (from == 0) { // Constructor + + } + } + if (debug_lvl & 2 ) { // Instantiation/Destruction notification + if (from == 0) cout << "Instantiated: FGCondition" << endl; + if (from == 1) cout << "Destroyed: FGCondition" << endl; + } + if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects + } + if (debug_lvl & 8 ) { // Runtime state variables + } + if (debug_lvl & 16) { // Sanity checking + } + if (debug_lvl & 64) { + if (from == 0) { // Constructor + cout << IdSrc << endl; + cout << IdHdr << endl; + } + } +} + +} //namespace JSBSim + diff --git a/src/FDM/JSBSim/filtersjb/FGKinemat.cpp b/src/FDM/JSBSim/filtersjb/FGKinemat.cpp index 8c29a6577..a12a47113 100644 --- a/src/FDM/JSBSim/filtersjb/FGKinemat.cpp +++ b/src/FDM/JSBSim/filtersjb/FGKinemat.cpp @@ -1,42 +1,45 @@ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - + Module: FGKinemat.cpp Author: Tony Peden, for flight control system authored by Jon S. Berndt Date started: 12/02/01 - + ------------- Copyright (C) 2000 Anthony K. Peden ------------- - + 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. - + FUNCTIONAL DESCRIPTION -------------------------------------------------------------------------------- - + HISTORY -------------------------------------------------------------------------------- - + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% COMMENTS, REFERENCES, and NOTES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ +#include +#include + #include "FGKinemat.h" namespace JSBSim { @@ -57,9 +60,9 @@ FGKinemat::FGKinemat(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs), Detents.clear(); TransitionTimes.clear(); - + OutputPct=0; - InTransit=0; + DoScale = true; Type = AC_cfg->GetValue("TYPE"); Name = AC_cfg->GetValue("NAME"); @@ -74,10 +77,13 @@ FGKinemat::FGKinemat(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs), } else { *AC_cfg >> token; InputNodes.push_back( resolveSymbol(token) ); - } + } } else if ( token == "DETENTS" ) { *AC_cfg >> NumDetents; + if (NumDetents < 2) { + cerr << "Kinemat must have at least 2 DETENTS" << endl; + } for (int i=0;i> tmpDetent; *AC_cfg >> tmpTime; @@ -88,7 +94,10 @@ FGKinemat::FGKinemat(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs), IsOutput = true; *AC_cfg >> sOutputIdx; - OutputNode = PropertyManager->GetNode(sOutputIdx); + OutputNode = PropertyManager->GetNode(sOutputIdx, true); + } else if (token == "NOSCALE") { + + DoScale = false; } } FGFCSComponent::bind(); @@ -106,71 +115,58 @@ FGKinemat::~FGKinemat() //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -bool FGKinemat::Run(void ) { - double dt=fcs->GetState()->Getdt(); - double output_transit_rate=0; +bool FGKinemat::Run(void ) +{ + double dt = fcs->GetState()->Getdt(); Input = InputNodes[0]->getDoubleValue(); - InputCmd = Input*Detents[NumDetents-1]; - OutputPos = OutputNode->getDoubleValue(); - - if (InputCmd < Detents[0]) { - fi=0; - InputCmd=Detents[0]; - lastInputCmd=InputCmd; - OutputPos=Detents[0]; - Output=OutputPos; - } else if (InputCmd > Detents[NumDetents-1]) { - fi=NumDetents-1; - InputCmd=Detents[fi]; - lastInputCmd=InputCmd; - OutputPos=Detents[fi]; - Output=OutputPos; - } else { - //cout << "FGKinemat::Run Handle: " << InputCmd << " Position: " << OutputPos << endl; - if (dt <= 0) - OutputPos=InputCmd; - else { - if (InputCmd != lastInputCmd) { - InTransit=1; - } - //cout << "FGKinemat::Run, InTransit: " << InTransit << endl; - if (InTransit) { - //fprintf(stderr,"InputCmd: %g, OutputPos: %g\n",InputCmd,OutputPos); - fi=0; - while (Detents[fi] < InputCmd) { - fi++; - } - if (OutputPos < InputCmd) { - if (TransitionTimes[fi] > 0) - output_transit_rate=(Detents[fi] - Detents[fi-1])/TransitionTimes[fi]; - else - output_transit_rate=(Detents[fi] - Detents[fi-1])/5; - //cout << "FGKinemat::Run, output_transit_rate: " << output_transit_rate << endl; - } else { - if (TransitionTimes[fi+1] > 0) - output_transit_rate=(Detents[fi] - Detents[fi+1])/TransitionTimes[fi+1]; - else - output_transit_rate=(Detents[fi] - Detents[fi+1])/5; - } - if (fabs(OutputPos - InputCmd) > fabs(dt*output_transit_rate) ) { - OutputPos+=output_transit_rate*dt; - //cout << "FGKinemat::Run, OutputPos: " << OutputPos - // << " dt: " << dt << endl; - } else { - InTransit=0; - OutputPos=InputCmd; - } - } + + if (DoScale) Input *= Detents[NumDetents-1]; + + Output = OutputNode->getDoubleValue(); + + if (Input < Detents[0]) + Input = Detents[0]; + else if (Detents[NumDetents-1] < Input) + Input = Detents[NumDetents-1]; + + // Process all detent intervals the movement traverses until either the + // final value is reached or the time interval has finished. + while (0.0 < dt && Input != Output) { + + // Find the area where Output is in + int ind; + for (ind = 1; (Input < Output) ? Detents[ind] < Output : Detents[ind] <= Output ; ++ind) + if (NumDetents <= ind) + break; + + // A transition time of 0.0 means an infinite rate. + // The output is reached in one step + if (TransitionTimes[ind] <= 0.0) { + Output = Input; + break; + } else { + // Compute the rate in this area + double Rate = (Detents[ind] - Detents[ind-1])/TransitionTimes[ind]; + // Compute the maximum input value inside this area + double ThisInput = Input; + if (ThisInput < Detents[ind-1]) ThisInput = Detents[ind-1]; + if (Detents[ind] < ThisInput) ThisInput = Detents[ind]; + // Compute the time to reach the value in ThisInput + double ThisDt = fabs((ThisInput-Output)/Rate); + // and clip to the timestep size + if (dt < ThisDt) ThisDt = dt; + dt -= ThisDt; + // Do the output calculation + if (Output < Input) + Output += ThisDt*Rate; + else + Output -= ThisDt*Rate; } - lastInputCmd = InputCmd; - Output = OutputPos; - } - - if ( Detents[NumDetents-1] > 0 ) { - OutputPct = Output / Detents[NumDetents-1]; } - + + OutputPct = (Output-Detents[0])/(Detents[NumDetents-1]-Detents[0]); + if (IsOutput) SetOutput(); return true; @@ -207,6 +203,7 @@ void FGKinemat::Debug(int from) cout << " " << Detents[i] << " " << TransitionTimes[i] << endl; } if (IsOutput) cout << " OUTPUT: " << OutputNode->getName() << endl; + if (!DoScale) cout << " NOSCALE" << endl; } } if (debug_lvl & 2 ) { // Instantiation/Destruction notification diff --git a/src/FDM/JSBSim/filtersjb/FGKinemat.h b/src/FDM/JSBSim/filtersjb/FGKinemat.h index 9f16e8dea..f00ca9a9a 100644 --- a/src/FDM/JSBSim/filtersjb/FGKinemat.h +++ b/src/FDM/JSBSim/filtersjb/FGKinemat.h @@ -82,18 +82,15 @@ public: double GetOutputPct() const { return OutputPct; } - bool Run (void ); + bool Run (void); private: FGConfigFile* AC_cfg; vector Detents; vector TransitionTimes; - int NumDetents,fi; - double lastInputCmd; - double InputCmd; - double OutputPos; + int NumDetents; double OutputPct; - bool InTransit; + bool DoScale; void Debug(int from); }; diff --git a/src/FDM/JSBSim/filtersjb/FGSwitch.cpp b/src/FDM/JSBSim/filtersjb/FGSwitch.cpp index e38b2064b..8557d099c 100644 --- a/src/FDM/JSBSim/filtersjb/FGSwitch.cpp +++ b/src/FDM/JSBSim/filtersjb/FGSwitch.cpp @@ -1,280 +1,280 @@ -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - Module: FGSwitch.cpp - Author: Jon S. Berndt - Date started: 4/2000 - - ------------- Copyright (C) 2000 ------------- - - 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. - -FUNCTIONAL DESCRIPTION --------------------------------------------------------------------------------- - -HISTORY --------------------------------------------------------------------------------- - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -COMMENTS, REFERENCES, and NOTES -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -The SWITCH component is defined as follows (see the API documentation for more -information): - - - - {property} {conditional} {property|value} - - {property} {conditional} {property|value} - ... - - ... - - - {property} {conditional} {property|value} - ... - - ... - - -Also, see the header file (FGSwitch.h) for further details. - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -INCLUDES -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - -#include "FGSwitch.h" - -namespace JSBSim { - -static const char *IdSrc = "$Id$"; -static const char *IdHdr = ID_SWITCH; - -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -CLASS IMPLEMENTATION -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ - - -FGSwitch::FGSwitch(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs), - AC_cfg(AC_cfg) -{ - string token, value; - struct test *current_test; - struct FGCondition *current_condition; - - Type = AC_cfg->GetValue("TYPE"); - Name = AC_cfg->GetValue("NAME"); - - AC_cfg->GetNextConfigLine(); - while ((token = AC_cfg->GetValue()) != string("/COMPONENT")) { - - // See the above documentation, or the API docs, for information on what - // the SWITCH component is supposed to look like in the configuration file. - // Below, the switch component is read in. - - if (token == "TEST") { - tests.push_back(*(new test)); - current_test = &tests.back(); - - if (AC_cfg->GetValue("LOGIC") == "OR") { - current_test->Logic = eOR; - } else if (AC_cfg->GetValue("LOGIC") == "AND") { - current_test->Logic = eAND; - } else if (AC_cfg->GetValue("LOGIC") == "DEFAULT") { - current_test->Logic = eDefault; - } else { // error - cerr << "Unrecognized LOGIC token in switch component: " << Name << endl; - } - - value = AC_cfg->GetValue("VALUE"); - if (value.empty()) { - cerr << "No VALUE supplied for switch component: " << Name << endl; - } else { - if (value.find_first_not_of("-.0123456789eE") == string::npos) { - // if true (and execution falls into this block), "value" is a number. - current_test->OutputVal = atof(value.c_str()); - } else { - // "value" must be a property if execution passes to here. - if (value[0] == '-') { - current_test->sign = -1.0; - value.erase(0,1); - } else { - current_test->sign = 1.0; - } - current_test->OutputProp = PropertyManager->GetNode(value); - } - } - - AC_cfg->GetNextConfigLine(); - while (AC_cfg->GetValue() != string("/TEST")) { - current_test->conditions.push_back(*(new FGCondition(AC_cfg, PropertyManager))); - } - } - AC_cfg->GetNextConfigLine(); - } - - FGFCSComponent::bind(); - - Debug(0); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGSwitch::~FGSwitch() -{ - Debug(1); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -bool FGSwitch::Run(void ) -{ - vector ::iterator iTests = tests.begin(); - vector ::iterator iConditions; - bool pass = false; - - FGFCSComponent::Run(); // call the base class for initialization of Input - - while (iTests < tests.end()) { - iConditions = iTests->conditions.begin(); - - if (iTests->Logic == eDefault) { - Output = iTests->GetValue(); - } else if (iTests->Logic == eAND) { - pass = true; - while (iConditions < iTests->conditions.end()) { - if (!iConditions->Evaluate()) pass = false; - *iConditions++; - } - } else if (iTests->Logic == eOR) { - pass = false; - while (iConditions < iTests->conditions.end()) { - if (iConditions->Evaluate()) pass = true; - *iConditions++; - } - } else { - cerr << "Invalid logic test" << endl; - } - - if (pass) { - Output = iTests->GetValue(); - break; - } - *iTests++; - } - - return true; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// The bitmasked value choices are as follows: -// unset: In this case (the default) JSBSim would only print -// out the normally expected messages, essentially echoing -// the config files as they are read. If the environment -// variable is not set, debug_lvl is set to 1 internally -// 0: This requests JSBSim not to output any messages -// whatsoever. -// 1: This value explicity requests the normal JSBSim -// startup messages -// 2: This value asks for a message to be printed out when -// a class is instantiated -// 4: When this value is set, a message is displayed when a -// FGModel object executes its Run() method -// 8: When this value is set, various runtime state variables -// are printed out periodically -// 16: When set various parameters are sanity checked and -// a message is printed out when they go out of bounds - -void FGSwitch::Debug(int from) -{ - vector ::iterator iTests = tests.begin(); - vector ::iterator iConditions; - string comp, scratch; - string indent = " "; - bool first = false; - - if (debug_lvl <= 0) return; - - if (debug_lvl & 1) { // Standard console startup message output - if (from == 0) { // Constructor - while (iTests < tests.end()) { - - scratch = " if "; - - switch(iTests->Logic) { - case (elUndef): - comp = " UNSET "; - cerr << "Unset logic for test condition" << endl; - break; - case (eAND): - comp = " AND "; - break; - case (eOR): - comp=" OR "; - break; - case (eDefault): - scratch = " by default."; - break; - default: - comp = " UNKNOWN "; - cerr << "Unknown logic for test condition" << endl; - } - - if (iTests->OutputProp != 0L) - if (iTests->sign < 0) - cout << indent << "Switch VALUE is - " << iTests->OutputProp->GetName() << scratch << endl; - else - cout << indent << "Switch VALUE is " << iTests->OutputProp->GetName() << scratch << endl; - else - cout << indent << "Switch VALUE is " << iTests->OutputVal << scratch << endl; - - iConditions = iTests->conditions.begin(); - first = true; - while (iConditions < iTests->conditions.end()) { - if (!first) cout << indent << comp << " "; - else cout << indent << " "; - first = false; - iConditions->PrintCondition(); - cout << endl; - *iConditions++; - } - cout << endl; - *iTests++; - } - } - } - if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGSwitch" << endl; - if (from == 1) cout << "Destroyed: FGSwitch" << endl; - } - if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects - } - if (debug_lvl & 8 ) { // Runtime state variables - } - if (debug_lvl & 16) { // Sanity checking - } - if (debug_lvl & 64) { - if (from == 0) { // Constructor - cout << IdSrc << endl; - cout << IdHdr << endl; - } - } -} - -} //namespace JSBSim - +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGSwitch.cpp + Author: Jon S. Berndt + Date started: 4/2000 + + ------------- Copyright (C) 2000 ------------- + + 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. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- + +HISTORY +-------------------------------------------------------------------------------- + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +COMMENTS, REFERENCES, and NOTES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +The SWITCH component is defined as follows (see the API documentation for more +information): + + + + {property} {conditional} {property|value} + + {property} {conditional} {property|value} + ... + + ... + + + {property} {conditional} {property|value} + ... + + ... + + +Also, see the header file (FGSwitch.h) for further details. + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGSwitch.h" + +namespace JSBSim { + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_SWITCH; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + + +FGSwitch::FGSwitch(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs), + AC_cfg(AC_cfg) +{ + string token, value; + struct test *current_test; + struct FGCondition *current_condition; + + Type = AC_cfg->GetValue("TYPE"); + Name = AC_cfg->GetValue("NAME"); + + AC_cfg->GetNextConfigLine(); + while ((token = AC_cfg->GetValue()) != string("/COMPONENT")) { + + // See the above documentation, or the API docs, for information on what + // the SWITCH component is supposed to look like in the configuration file. + // Below, the switch component is read in. + + if (token == "TEST") { + tests.push_back(test()); + current_test = &tests.back(); + + if (AC_cfg->GetValue("LOGIC") == "OR") { + current_test->Logic = eOR; + } else if (AC_cfg->GetValue("LOGIC") == "AND") { + current_test->Logic = eAND; + } else if (AC_cfg->GetValue("LOGIC") == "DEFAULT") { + current_test->Logic = eDefault; + } else { // error + cerr << "Unrecognized LOGIC token in switch component: " << Name << endl; + } + + value = AC_cfg->GetValue("VALUE"); + if (value.empty()) { + cerr << "No VALUE supplied for switch component: " << Name << endl; + } else { + if (value.find_first_not_of("-.0123456789eE") == string::npos) { + // if true (and execution falls into this block), "value" is a number. + current_test->OutputVal = atof(value.c_str()); + } else { + // "value" must be a property if execution passes to here. + if (value[0] == '-') { + current_test->sign = -1.0; + value.erase(0,1); + } else { + current_test->sign = 1.0; + } + current_test->OutputProp = PropertyManager->GetNode(value); + } + } + + AC_cfg->GetNextConfigLine(); + while (AC_cfg->GetValue() != string("/TEST")) { + current_test->conditions.push_back(FGCondition(AC_cfg, PropertyManager)); + } + } + AC_cfg->GetNextConfigLine(); + } + + FGFCSComponent::bind(); + + Debug(0); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGSwitch::~FGSwitch() +{ + Debug(1); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGSwitch::Run(void ) +{ + vector ::iterator iTests = tests.begin(); + vector ::iterator iConditions; + bool pass = false; + + FGFCSComponent::Run(); // call the base class for initialization of Input + + while (iTests < tests.end()) { + iConditions = iTests->conditions.begin(); + + if (iTests->Logic == eDefault) { + Output = iTests->GetValue(); + } else if (iTests->Logic == eAND) { + pass = true; + while (iConditions < iTests->conditions.end()) { + if (!iConditions->Evaluate()) pass = false; + *iConditions++; + } + } else if (iTests->Logic == eOR) { + pass = false; + while (iConditions < iTests->conditions.end()) { + if (iConditions->Evaluate()) pass = true; + *iConditions++; + } + } else { + cerr << "Invalid logic test" << endl; + } + + if (pass) { + Output = iTests->GetValue(); + break; + } + *iTests++; + } + + return true; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// The bitmasked value choices are as follows: +// unset: In this case (the default) JSBSim would only print +// out the normally expected messages, essentially echoing +// the config files as they are read. If the environment +// variable is not set, debug_lvl is set to 1 internally +// 0: This requests JSBSim not to output any messages +// whatsoever. +// 1: This value explicity requests the normal JSBSim +// startup messages +// 2: This value asks for a message to be printed out when +// a class is instantiated +// 4: When this value is set, a message is displayed when a +// FGModel object executes its Run() method +// 8: When this value is set, various runtime state variables +// are printed out periodically +// 16: When set various parameters are sanity checked and +// a message is printed out when they go out of bounds + +void FGSwitch::Debug(int from) +{ + vector ::iterator iTests = tests.begin(); + vector ::iterator iConditions; + string comp, scratch; + string indent = " "; + bool first = false; + + if (debug_lvl <= 0) return; + + if (debug_lvl & 1) { // Standard console startup message output + if (from == 0) { // Constructor + while (iTests < tests.end()) { + + scratch = " if "; + + switch(iTests->Logic) { + case (elUndef): + comp = " UNSET "; + cerr << "Unset logic for test condition" << endl; + break; + case (eAND): + comp = " AND "; + break; + case (eOR): + comp=" OR "; + break; + case (eDefault): + scratch = " by default."; + break; + default: + comp = " UNKNOWN "; + cerr << "Unknown logic for test condition" << endl; + } + + if (iTests->OutputProp != 0L) + if (iTests->sign < 0) + cout << indent << "Switch VALUE is - " << iTests->OutputProp->GetName() << scratch << endl; + else + cout << indent << "Switch VALUE is " << iTests->OutputProp->GetName() << scratch << endl; + else + cout << indent << "Switch VALUE is " << iTests->OutputVal << scratch << endl; + + iConditions = iTests->conditions.begin(); + first = true; + while (iConditions < iTests->conditions.end()) { + if (!first) cout << indent << comp << " "; + else cout << indent << " "; + first = false; + iConditions->PrintCondition(); + cout << endl; + *iConditions++; + } + cout << endl; + *iTests++; + } + } + } + if (debug_lvl & 2 ) { // Instantiation/Destruction notification + if (from == 0) cout << "Instantiated: FGSwitch" << endl; + if (from == 1) cout << "Destroyed: FGSwitch" << endl; + } + if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects + } + if (debug_lvl & 8 ) { // Runtime state variables + } + if (debug_lvl & 16) { // Sanity checking + } + if (debug_lvl & 64) { + if (from == 0) { // Constructor + cout << IdSrc << endl; + cout << IdHdr << endl; + } + } +} + +} //namespace JSBSim + diff --git a/src/FDM/JSBSim/filtersjb/FGSwitch.h b/src/FDM/JSBSim/filtersjb/FGSwitch.h index 451bbb223..fef43f2c0 100644 --- a/src/FDM/JSBSim/filtersjb/FGSwitch.h +++ b/src/FDM/JSBSim/filtersjb/FGSwitch.h @@ -120,7 +120,7 @@ whatever value fcs/roll-ap-error-summer is. @author Jon S. Berndt @version $Id$ */ - + /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS DECLARATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -133,20 +133,20 @@ public: bool Run(void); + enum eLogic {elUndef=0, eAND, eOR, eDefault}; + enum eComparison {ecUndef=0, eEQ, eNE, eGT, eGE, eLT, eLE}; + private: FGFCS* fcs; FGConfigFile* AC_cfg; - enum eLogic {elUndef=0, eAND, eOR, eDefault}; - enum eComparison {ecUndef=0, eEQ, eNE, eGT, eGE, eLT, eLE}; - struct test { vector conditions; eLogic Logic; double OutputVal; FGPropertyManager *OutputProp; float sign; - + double GetValue(void) { if (OutputProp == 0L) return OutputVal; else return OutputProp->getDoubleValue()*sign; @@ -162,7 +162,7 @@ private: }; vector tests; - + void Debug(int from); }; } -- 2.39.5