From c37874e140f0c0dc7e804db156766e93d37c3130 Mon Sep 17 00:00:00 2001 From: ehofman Date: Mon, 15 Mar 2004 09:24:57 +0000 Subject: [PATCH] Fix an issue where math.h doesn't define sqrt in namespace std (at least on MSVC) and remove DOS line-endings. --- src/FDM/JSBSim/FGColumnVector3.cpp | 290 ++--- src/FDM/JSBSim/FGColumnVector3.h | 2 +- src/FDM/JSBSim/FGColumnVector4.h | 2 +- src/FDM/JSBSim/FGForce.cpp | 360 +++--- src/FDM/JSBSim/FGJSBBase.cpp | 398 +++---- src/FDM/JSBSim/FGLGear.cpp | 1172 ++++++++++---------- src/FDM/JSBSim/FGMatrix33.cpp | 782 ++++++------- src/FDM/JSBSim/FGOutput.cpp | 1288 +++++++++++----------- src/FDM/JSBSim/FGPosition.cpp | 734 ++++++------ src/FDM/JSBSim/filtersjb/FGCondition.cpp | 514 ++++----- src/FDM/JSBSim/filtersjb/FGSwitch.cpp | 560 +++++----- 11 files changed, 3051 insertions(+), 3051 deletions(-) diff --git a/src/FDM/JSBSim/FGColumnVector3.cpp b/src/FDM/JSBSim/FGColumnVector3.cpp index 0e9c1060e..96a69e64b 100644 --- a/src/FDM/JSBSim/FGColumnVector3.cpp +++ b/src/FDM/JSBSim/FGColumnVector3.cpp @@ -1,145 +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) -{ - 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 +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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 f5044709a..b0a471dc2 100644 --- a/src/FDM/JSBSim/FGColumnVector3.h +++ b/src/FDM/JSBSim/FGColumnVector3.h @@ -33,7 +33,7 @@ INCLUDES SG_USING_STD(cerr); SG_USING_STD(cout); SG_USING_STD(endl); - SG_USING_STD(sqrt); +// SG_USING_STD(sqrt); #else # include # if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740) diff --git a/src/FDM/JSBSim/FGColumnVector4.h b/src/FDM/JSBSim/FGColumnVector4.h index 154945379..8b87c4841 100644 --- a/src/FDM/JSBSim/FGColumnVector4.h +++ b/src/FDM/JSBSim/FGColumnVector4.h @@ -31,7 +31,7 @@ INCLUDES SG_USING_STD(cerr); SG_USING_STD(cout); SG_USING_STD(endl); - SG_USING_STD(sqrt); +// SG_USING_STD(sqrt); #else # include # if defined (sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740) diff --git a/src/FDM/JSBSim/FGForce.cpp b/src/FDM/JSBSim/FGForce.cpp index d134b325f..72256d198 100644 --- a/src/FDM/JSBSim/FGForce.cpp +++ b/src/FDM/JSBSim/FGForce.cpp @@ -1,180 +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 "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; - } - } -} -} +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + 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/FGJSBBase.cpp b/src/FDM/JSBSim/FGJSBBase.cpp index f3188b54e..4ed156854 100644 --- a/src/FDM/JSBSim/FGJSBBase.cpp +++ b/src/FDM/JSBSim/FGJSBBase.cpp @@ -1,199 +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; - -// 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 - +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + 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/FGLGear.cpp b/src/FDM/JSBSim/FGLGear.cpp index c9c418350..276ec70d8 100644 --- a/src/FDM/JSBSim/FGLGear.cpp +++ b/src/FDM/JSBSim/FGLGear.cpp @@ -1,586 +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" - -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 - +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + 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/FGMatrix33.cpp b/src/FDM/JSBSim/FGMatrix33.cpp index ddd442b5d..0d42129f1 100644 --- a/src/FDM/JSBSim/FGMatrix33.cpp +++ b/src/FDM/JSBSim/FGMatrix33.cpp @@ -1,391 +1,391 @@ -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -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; - } - } -} -} +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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/FGOutput.cpp b/src/FDM/JSBSim/FGOutput.cpp index 54231a5ec..15b76d8da 100644 --- a/src/FDM/JSBSim/FGOutput.cpp +++ b/src/FDM/JSBSim/FGOutput.cpp @@ -1,644 +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" -#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; - } - } -} -} +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + 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 e33dba821..28b83b8f4 100644 --- a/src/FDM/JSBSim/FGPosition.cpp +++ b/src/FDM/JSBSim/FGPosition.cpp @@ -1,367 +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 "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; - } - } -} -} +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + 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/filtersjb/FGCondition.cpp b/src/FDM/JSBSim/filtersjb/FGCondition.cpp index bcb3b1bd7..b9fcb62af 100644 --- a/src/FDM/JSBSim/filtersjb/FGCondition.cpp +++ b/src/FDM/JSBSim/filtersjb/FGCondition.cpp @@ -1,257 +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(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 - +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + 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/FGSwitch.cpp b/src/FDM/JSBSim/filtersjb/FGSwitch.cpp index 8557d099c..cf323fcb6 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(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 - +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + 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 + -- 2.39.5