fly/flightgear
1
0
Fork 0
Code Activity

Sync. with JSBSim CVS.

Browse source
This commit is contained in:
ehofman 2004-03-14 14:57:07 +00:00
parent a97b69056e
commit f63d336ebc
54 changed files with 4272 additions and 4103 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
src/FDM/JSBSim/FGAerodynamics.cpp
View file

@ -37,8 +37,11 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGAerodynamics.h"
#include "FGTranslation.h"
#include "FGAircraft.h"
#include "FGState.h"
#include "FGMassBalance.h"
#include "FGFactorGroup.h"
#include "FGCoefficient.h"
#include "FGPropertyManager.h"
namespace JSBSim {

5
src/FDM/JSBSim/FGAerodynamics.h
View file

@ -54,11 +54,8 @@ INCLUDES
#include "FGModel.h"
#include "FGConfigFile.h"
#include "FGState.h"
#include "FGMassBalance.h"
#include "FGTranslation.h"
#include "FGCoefficient.h"
#include "FGFactorGroup.h"
#include "FGColumnVector3.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS

96
src/FDM/JSBSim/FGAircraft.cpp
View file

@ -1,49 +1,49 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGAircraft.cpp
Author: Jon S. Berndt
Date started: 12/12/98
Date started: 12/12/98
Purpose: Encapsulates an aircraft
Called by: FGFDMExec
------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
Models the aircraft reactions and forces. This class is instantiated by the
FGFDMExec class and scheduled as an FDM entry.
FGFDMExec class and scheduled as an FDM entry.
HISTORY
--------------------------------------------------------------------------------
12/12/98 JSB Created
04/03/99 JSB Changed Aero() method to correct body axis force calculation
from wind vector. Fix provided by Tony Peden.
05/03/99 JSB Changed (for the better?) the way configurations are read in.
9/17/99 TP Combined force and moment functions. Added aero reference
point to config file. Added calculations for moments due to
9/17/99 TP Combined force and moment functions. Added aero reference
point to config file. Added calculations for moments due to
difference in cg and aero reference point
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
COMMENTS, REFERENCES, and NOTES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
@ -73,15 +73,9 @@ INCLUDES
#include "FGInertial.h"
#include "FGGroundReactions.h"
#include "FGAerodynamics.h"
#include "FGTranslation.h"
#include "FGRotation.h"
#include "FGAtmosphere.h"
#include "FGState.h"
#include "FGFDMExec.h"
#include "FGFCS.h"
#include "FGPosition.h"
#include "FGAuxiliary.h"
#include "FGOutput.h"
#include "FGPropertyManager.h"
namespace JSBSim {
@ -139,14 +133,14 @@ bool FGAircraft::Run(void)
vMoments += Aerodynamics->GetMoments();
vMoments += Propulsion->GetMoments();
vMoments += GroundReactions->GetMoments();
vBodyAccel = vForces/MassBalance->GetMass();
vNcg = vBodyAccel/Inertial->gravity();
vNwcg = State->GetTb2s() * vNcg;
vNwcg(3) = -1*vNwcg(3) + 1;
return false;
} else { // skip Run() execution this time
return true;
@ -166,10 +160,12 @@ bool FGAircraft::Load(FGConfigFile* AC_cfg)
{
string token = "";
string parameter;
double EW, bixx, biyy, bizz, bixy, bixz;
double EW, bixx, biyy, bizz, bixy, bixz, biyz;
double pmWt, pmX, pmY, pmZ;
FGColumnVector3 vbaseXYZcg;
bixx = biyy = bizz = bixy = bixz = biyz = 0.0;
AC_cfg->GetNextConfigLine();
while ((token = AC_cfg->GetValue()) != string("/METRICS")) {
@ -182,7 +178,7 @@ bool FGAircraft::Load(FGConfigFile* AC_cfg)
if (debug_lvl > 0) cout << " WingSpan: " << WingSpan << endl;
} else if (parameter == "AC_WINGINCIDENCE") {
*AC_cfg >> WingIncidence;
if (debug_lvl > 0) cout << " Chord: " << cbar << endl;
if (debug_lvl > 0) cout << " Incidence: " << WingIncidence << endl;
} else if (parameter == "AC_CHORD") {
*AC_cfg >> cbar;
if (debug_lvl > 0) cout << " Chord: " << cbar << endl;
@ -200,28 +196,26 @@ bool FGAircraft::Load(FGConfigFile* AC_cfg)
if (debug_lvl > 0) cout << " V. Tail Arm: " << VTailArm << endl;
} else if (parameter == "AC_IXX") {
*AC_cfg >> bixx;
if (debug_lvl > 0) cout << " baseIxx: " << bixx << endl;
MassBalance->SetBaseIxx(bixx);
if (debug_lvl > 0) cout << " baseIxx: " << bixx << " slug-ft2" << endl;
} else if (parameter == "AC_IYY") {
*AC_cfg >> biyy;
if (debug_lvl > 0) cout << " baseIyy: " << biyy << endl;
MassBalance->SetBaseIyy(biyy);
if (debug_lvl > 0) cout << " baseIyy: " << biyy << " slug-ft2" << endl;
} else if (parameter == "AC_IZZ") {
*AC_cfg >> bizz;
if (debug_lvl > 0) cout << " baseIzz: " << bizz << endl;
MassBalance->SetBaseIzz(bizz);
if (debug_lvl > 0) cout << " baseIzz: " << bizz << " slug-ft2" << endl;
} else if (parameter == "AC_IXY") {
*AC_cfg >> bixy;
if (debug_lvl > 0) cout << " baseIxy: " << bixy << endl;
MassBalance->SetBaseIxy(bixy);
if (debug_lvl > 0) cout << " baseIxy: " << bixy << " slug-ft2" << endl;
} else if (parameter == "AC_IXZ") {
*AC_cfg >> bixz;
if (debug_lvl > 0) cout << " baseIxz: " << bixz << endl;
MassBalance->SetBaseIxz(bixz);
if (debug_lvl > 0) cout << " baseIxz: " << bixz << " slug-ft2" << endl;
} else if (parameter == "AC_IYZ") {
*AC_cfg >> biyz;
if (debug_lvl > 0) cout << " baseIyz: " << biyz << " slug-ft2" << endl;
} else if (parameter == "AC_EMPTYWT") {
*AC_cfg >> EW;
MassBalance->SetEmptyWeight(EW);
if (debug_lvl > 0) cout << " EmptyWeight: " << EW << endl;
if (debug_lvl > 0) cout << " EmptyWeight: " << EW << " lbm" << endl;
} else if (parameter == "AC_CGLOC") {
*AC_cfg >> vbaseXYZcg(eX) >> vbaseXYZcg(eY) >> vbaseXYZcg(eZ);
MassBalance->SetBaseCG(vbaseXYZcg);
@ -244,7 +238,11 @@ bool FGAircraft::Load(FGConfigFile* AC_cfg)
<< endl;
}
}
MassBalance->SetAircraftBaseInertias(FGMatrix33( bixx, -bixy, -bixz,
-bixy, biyy, -biyz,
-bixz, -biyz, bizz ));
// calculate some derived parameters
if (cbar != 0.0) {
lbarh = HTailArm/cbar;
@ -253,7 +251,7 @@ bool FGAircraft::Load(FGConfigFile* AC_cfg)
vbarh = HTailArm*HTailArea / (cbar*WingArea);
vbarv = VTailArm*VTailArea / (cbar*WingArea);
}
}
}
return true;
}
@ -298,17 +296,17 @@ void FGAircraft::bind(void)
(PMF)&FGAircraft::GetForces);
PropertyManager->Tie("forces/fbz-total-lbs", this,3,
(PMF)&FGAircraft::GetForces);
PropertyManager->Tie("metrics/aero-rp-x-ft", this,1,
PropertyManager->Tie("metrics/aero-rp-x-in", this,1,
(PMF)&FGAircraft::GetXYZrp);
PropertyManager->Tie("metrics/aero-rp-y-ft", this,2,
PropertyManager->Tie("metrics/aero-rp-y-in", this,2,
(PMF)&FGAircraft::GetXYZrp);
PropertyManager->Tie("metrics/aero-rp-z-ft", this,3,
PropertyManager->Tie("metrics/aero-rp-z-in", this,3,
(PMF)&FGAircraft::GetXYZrp);
PropertyManager->Tie("metrics/eyepoint-x-ft", this,1,
PropertyManager->Tie("metrics/eyepoint-x-in", this,1,
(PMF)&FGAircraft::GetXYZep);
PropertyManager->Tie("metrics/eyepoint-y-ft", this,2,
PropertyManager->Tie("metrics/eyepoint-y-in", this,2,
(PMF)&FGAircraft::GetXYZep);
PropertyManager->Tie("metrics/eyepoint-z-ft", this,3,
PropertyManager->Tie("metrics/eyepoint-z-in", this,3,
(PMF)&FGAircraft::GetXYZep);
PropertyManager->Tie("metrics/visualrefpoint-x-in", this,1,
(PMF)&FGAircraft::GetXYZvrp);
@ -340,12 +338,12 @@ void FGAircraft::unbind(void)
PropertyManager->Untie("forces/fbx-total-lbs");
PropertyManager->Untie("forces/fby-total-lbs");
PropertyManager->Untie("forces/fbz-total-lbs");
PropertyManager->Untie("metrics/aero-rp-x-ft");
PropertyManager->Untie("metrics/aero-rp-y-ft");
PropertyManager->Untie("metrics/aero-rp-z-ft");
PropertyManager->Untie("metrics/eyepoint-x-ft");
PropertyManager->Untie("metrics/eyepoint-y-ft");
PropertyManager->Untie("metrics/eyepoint-z-ft");
PropertyManager->Untie("metrics/aero-rp-x-in");
PropertyManager->Untie("metrics/aero-rp-y-in");
PropertyManager->Untie("metrics/aero-rp-z-in");
PropertyManager->Untie("metrics/eyepoint-x-in");
PropertyManager->Untie("metrics/eyepoint-y-in");
PropertyManager->Untie("metrics/eyepoint-z-in");
PropertyManager->Untie("metrics/visualrefpoint-x-in");
PropertyManager->Untie("metrics/visualrefpoint-y-in");
PropertyManager->Untie("metrics/visualrefpoint-z-in");

5
src/FDM/JSBSim/FGAircraft.h
View file

@ -53,12 +53,8 @@ INCLUDES
#endif
#include "FGModel.h"
#include "FGPropulsion.h"
#include "FGConfigFile.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
#include "FGLGear.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
@ -169,7 +165,6 @@ private:
FGColumnVector3 vXYZrp;
FGColumnVector3 vXYZvrp;
FGColumnVector3 vXYZep;
FGColumnVector3 vEuler;
FGColumnVector3 vDXYZcg;
FGColumnVector3 vBodyAccel;
FGColumnVector3 vNcg;

24
src/FDM/JSBSim/FGAtmosphere.cpp
View file

@ -50,16 +50,9 @@ INCLUDES
#include "FGAtmosphere.h"
#include "FGState.h"
#include "FGFDMExec.h"
#include "FGFCS.h"
#include "FGAircraft.h"
#include "FGTranslation.h"
#include "FGRotation.h"
#include "FGPosition.h"
#include "FGAuxiliary.h"
#include "FGOutput.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
#include "FGInertial.h"
#include "FGPropertyManager.h"
namespace JSBSim {
@ -152,8 +145,6 @@ bool FGAtmosphere::Run(void)
soundspeed = sqrt(SHRatio*Reng*(*temperature));
State->Seta(soundspeed);
Debug(2);
return false;
@ -308,13 +299,18 @@ void FGAtmosphere::Turbulence(void)
vTurbulenceGrad = TurbGain*MagnitudeAccel * vDirection;
vBodyTurbGrad = State->GetTl2b()*vTurbulenceGrad;
vTurbPQR(eP) = vBodyTurbGrad(eY)/Aircraft->GetWingSpan();
if (Aircraft->GetWingSpan() > 0) {
vTurbPQR(eP) = vBodyTurbGrad(eY)/Aircraft->GetWingSpan();
} else {
vTurbPQR(eP) = vBodyTurbGrad(eY)/30.0;
}
// if (Aircraft->GetHTailArm() != 0.0)
// vTurbPQR(eQ) = vBodyTurbGrad(eZ)/Aircraft->GetHTailArm();
// else
// vTurbPQR(eQ) = vBodyTurbGrad(eZ)/10.0;
if (Aircraft->GetVTailArm())
if (Aircraft->GetVTailArm() > 0)
vTurbPQR(eR) = vBodyTurbGrad(eX)/Aircraft->GetVTailArm();
else
vTurbPQR(eR) = vBodyTurbGrad(eX)/10.0;
@ -356,12 +352,12 @@ void FGAtmosphere::Turbulence(void)
vBodyTurbGrad = State->GetTl2b()*vTurbulenceGrad;
vTurbPQR(eP) = vBodyTurbGrad(eY)/Aircraft->GetWingSpan();
if (Aircraft->GetHTailArm() != 0.0)
if (Aircraft->GetHTailArm() > 0)
vTurbPQR(eQ) = vBodyTurbGrad(eZ)/Aircraft->GetHTailArm();
else
vTurbPQR(eQ) = vBodyTurbGrad(eZ)/10.0;
if (Aircraft->GetVTailArm())
if (Aircraft->GetVTailArm() > 0)
vTurbPQR(eR) = vBodyTurbGrad(eX)/Aircraft->GetVTailArm();
else
vTurbPQR(eR) = vBodyTurbGrad(eX)/10.0;

2
src/FDM/JSBSim/FGAtmosphere.h
View file

@ -43,9 +43,7 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGModel.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS

6
src/FDM/JSBSim/FGAuxiliary.cpp
View file

@ -47,14 +47,8 @@ INCLUDES
#include "FGAtmosphere.h"
#include "FGState.h"
#include "FGFDMExec.h"
#include "FGFCS.h"
#include "FGAircraft.h"
#include "FGPosition.h"
#include "FGOutput.h"
#include "FGInertial.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
#include "FGPropertyManager.h"
namespace JSBSim {

2
src/FDM/JSBSim/FGAuxiliary.h
View file

@ -40,9 +40,7 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGModel.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS

474
src/FDM/JSBSim/FGColumnVector3.cpp
View file

@ -1,329 +1,145 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGColumnVector3.cpp
Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
Date started: 1998
Purpose: FGColumnVector3 class
Called by: Various
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
HISTORY
--------------------------------------------------------------------------------
??/??/?? TP Created
03/16/2000 JSB Added exception throwing
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGColumnVector3.h"
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_COLUMNVECTOR3;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
FGColumnVector3::FGColumnVector3(void)
{
rowCtr = 1;
data[0]=0; data[1]=0; data[2]=0; data[3]=0;
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3::FGColumnVector3(double X, double Y, double Z)
{
rowCtr = 1;
data[0] = 0; data[eX] = X; data[eY] = Y; data[eZ] = Z;
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3::~FGColumnVector3(void)
{
Debug(1);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3::FGColumnVector3(const FGColumnVector3& b)
{
data[1] = b.data[1];
data[2] = b.data[2];
data[3] = b.data[3];
rowCtr = 1;
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGColumnVector3::operator=(const FGColumnVector3& b)
{
data[1] = b.data[1];
data[2] = b.data[2];
data[3] = b.data[3];
rowCtr = 1;
return *this;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGColumnVector3::operator+(const FGColumnVector3& C)
{
FGColumnVector3 Sum;
Sum(1) = C(1) + data[1];
Sum(2) = C(2) + data[2];
Sum(3) = C(3) + data[3];
return Sum;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGColumnVector3::operator+=(const FGColumnVector3& C)
{
data[1] += C(1);
data[2] += C(2);
data[3] += C(3);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGColumnVector3::operator*(const double scalar)
{
FGColumnVector3 Product;
Product(1) = scalar * data[1];
Product(2) = scalar * data[2];
Product(3) = scalar * data[3];
return Product;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGColumnVector3::operator*=(const double scalar)
{
data[1] *= scalar;
data[2] *= scalar;
data[3] *= scalar;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGColumnVector3::operator-(const FGColumnVector3& V)
{
FGColumnVector3 Diff;
Diff(1) = data[1] - V(1);
Diff(2) = data[2] - V(2);
Diff(3) = data[3] - V(3);
return Diff;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGColumnVector3::operator-=(const FGColumnVector3& V)
{
data[1] -= V(1);
data[2] -= V(2);
data[3] -= V(3);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGColumnVector3::operator/(const double scalar)
{
FGColumnVector3 Quotient;
if (scalar != 0) {
double tmp = 1.0/scalar;
Quotient(1) = data[1] * tmp;
Quotient(2) = data[2] * tmp;
Quotient(3) = data[3] * tmp;
} else {
cerr << "Attempt to divide by zero in method FGColumnVector3::operator/(const double scalar), object " << this << endl;
}
return Quotient;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGColumnVector3::operator/=(const double scalar)
{
FGColumnVector3 Quotient;
if (scalar != 0) {
double tmp = 1.0/scalar;
data[1] *= tmp;
data[2] *= tmp;
data[3] *= tmp;
} else {
cerr << "Attempt to divide by zero in method FGColumnVector3::operator/=(const double scalar), object " << this << endl;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 operator*(const double scalar, const FGColumnVector3& C)
{
FGColumnVector3 Product;
Product(1) = scalar * C(1);
Product(2) = scalar * C(2);
Product(3) = scalar * C(3);
return Product;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGColumnVector3::Magnitude(void)
{
double num;
if ((data[1] == 0.00) &&
(data[2] == 0.00) &&
(data[3] == 0.00))
{
return 0.00;
} else {
num = data[1]*data[1];
num += data[2]*data[2];
num += data[3]*data[3];
return sqrt(num);
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGColumnVector3::Normalize(void)
{
double Mag = Magnitude();
if (Mag != 0) {
Mag = 1.0/Mag;
data[1] *= Mag;
data[2] *= Mag;
data[3] *= Mag;
}
return *this;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGColumnVector3::operator*(const FGColumnVector3& V)
{
FGColumnVector3 Product;
Product(1) = data[2] * V(3) - data[3] * V(2);
Product(2) = data[3] * V(1) - data[1] * V(3);
Product(3) = data[1] * V(2) - data[2] * V(1);
return Product;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGColumnVector3::operator*=(const FGColumnVector3& V)
{
double a,b,c;
a = data[1]; b=data[2]; c=data[3];
data[1] = b * V(3) - c * V(2);
data[2] = c * V(1) - a * V(3);
data[3] = a * V(2) - b * V(1);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGColumnVector3::multElementWise(const FGColumnVector3& V)
{
FGColumnVector3 Product;
Product(1) = data[1] * V(1);
Product(2) = data[2] * V(2);
Product(3) = data[3] * V(3);
return Product;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ostream& operator<<(ostream& os, const FGColumnVector3& col)
{
os << col(1) << " , " << col(2) << " , " << col(3);
return os;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3& FGColumnVector3::operator<<(const double ff)
{
data[rowCtr] = ff;
if (++rowCtr > 3 )
rowCtr = 1;
return *this;
}
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
// The bitmasked value choices are as follows:
// unset: In this case (the default) JSBSim would only print
// out the normally expected messages, essentially echoing
// the config files as they are read. If the environment
// variable is not set, debug_lvl is set to 1 internally
// 0: This requests JSBSim not to output any messages
// whatsoever.
// 1: This value explicity requests the normal JSBSim
// startup messages
// 2: This value asks for a message to be printed out when
// a class is instantiated
// 4: When this value is set, a message is displayed when a
// FGModel object executes its Run() method
// 8: When this value is set, various runtime state variables
// are printed out periodically
// 16: When set various parameters are sanity checked and
// a message is printed out when they go out of bounds
void FGColumnVector3::Debug(int from)
{
if (debug_lvl <= 0) return;
if (debug_lvl & 1) { // Standard console startup message output
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
if (from == 0) cout << "Instantiated: FGColumnVector3" << endl;
if (from == 1) cout << "Destroyed: FGColumnVector3" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 ) { // Runtime state variables
}
if (debug_lvl & 16) { // Sanity checking
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
cout << IdSrc << endl;
cout << IdHdr << endl;
}
}
}
} // namespace JSBSim
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGColumnVector3.cpp
Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
Date started: 1998
Purpose: FGColumnVector3 class
Called by: Various
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
HISTORY
--------------------------------------------------------------------------------
??/??/?? TP Created
03/16/2000 JSB Added exception throwing
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGColumnVector3.h"
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_COLUMNVECTOR3;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
FGColumnVector3::FGColumnVector3(void)
{
data[0] = data[1] = data[2] = 0.0;
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGColumnVector3::operator/(const double scalar) const
{
if (scalar != 0.0)
return operator*( 1.0/scalar );
cerr << "Attempt to divide by zero in method "
"FGColumnVector3::operator/(const double scalar), "
"object " << this << endl;
return FGColumnVector3();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3& FGColumnVector3::operator/=(const double scalar)
{
if (scalar != 0.0)
operator*=( 1.0/scalar );
else
cerr << "Attempt to divide by zero in method "
"FGColumnVector3::operator/=(const double scalar), "
"object " << this << endl;
return *this;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGColumnVector3::Magnitude(void) const
{
if (data[1] == 0.0 && data[2] == 0.0 && data[3] == 0.0)
return 0.0;
else
return sqrt( Entry(1)*Entry(1) + Entry(2)*Entry(2) + Entry(3)*Entry(3) );
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3& FGColumnVector3::Normalize(void)
{
double Mag = Magnitude();
if (Mag != 0.0)
operator*=( 1.0/Mag );
return *this;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGColumnVector3::multElementWise(const FGColumnVector3& V) const
{
return FGColumnVector3(Entry(1) * V(1), Entry(2) * V(2), Entry(3) * V(3));
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ostream& operator<<(ostream& os, const FGColumnVector3& col)
{
os << col(1) << " , " << col(2) << " , " << col(3);
return os;
}
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
// The bitmasked value choices are as follows:
// unset: In this case (the default) JSBSim would only print
// out the normally expected messages, essentially echoing
// the config files as they are read. If the environment
// variable is not set, debug_lvl is set to 1 internally
// 0: This requests JSBSim not to output any messages
// whatsoever.
// 1: This value explicity requests the normal JSBSim
// startup messages
// 2: This value asks for a message to be printed out when
// a class is instantiated
// 4: When this value is set, a message is displayed when a
// FGModel object executes its Run() method
// 8: When this value is set, various runtime state variables
// are printed out periodically
// 16: When set various parameters are sanity checked and
// a message is printed out when they go out of bounds
void FGColumnVector3::Debug(int from)
{
if (debug_lvl <= 0) return;
if (debug_lvl & 1) { // Standard console startup message output
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
if (from == 0) cout << "Instantiated: FGColumnVector3" << endl;
if (from == 1) cout << "Destroyed: FGColumnVector3" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 ) { // Runtime state variables
}
if (debug_lvl & 16) { // Sanity checking
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
cout << IdSrc << endl;
cout << IdHdr << endl;
}
}
}
} // namespace JSBSim

267
src/FDM/JSBSim/FGColumnVector3.h
View file

@ -6,6 +6,8 @@ Date started: Unknown
HISTORY
--------------------------------------------------------------------------------
??/??/???? ?? Initial version and more.
03/06/2004 MF Rework, document and do much inlineing.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
@ -31,6 +33,7 @@ INCLUDES
SG_USING_STD(cerr);
SG_USING_STD(cout);
SG_USING_STD(endl);
SG_USING_STD(sqrt);
#else
# include <string>
# if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
@ -50,6 +53,7 @@ INCLUDES
using std::cerr;
using std::cout;
using std::endl;
using std::sqrt;
# endif
using std::string;
#endif
@ -84,47 +88,252 @@ CLASS DECLARATION
class FGColumnVector3 : public FGJSBBase
{
public:
/** Default initializer.
Create a zero vector.
*/
FGColumnVector3(void);
FGColumnVector3(double X, double Y, double Z);
FGColumnVector3(const FGColumnVector3& b);
~FGColumnVector3(void);
FGColumnVector3 operator=(const FGColumnVector3& b);
FGColumnVector3 operator*(const double scalar);
FGColumnVector3 operator*(const FGColumnVector3& V); // Cross product operator
FGColumnVector3 operator/(const double scalar);
FGColumnVector3 operator+(const FGColumnVector3& B); // must not return reference
FGColumnVector3 operator-(const FGColumnVector3& B);
void operator-=(const FGColumnVector3 &B);
void operator+=(const FGColumnVector3 &B);
void operator*=(const FGColumnVector3 &B);
void operator*=(const double scalar);
void operator/=(const double scalar);
FGColumnVector3& operator<<(const double ff);
/** Initialization by given values.
@param X value of the x-conponent.
@param Y value of the y-conponent.
@param Z value of the z-conponent.
Create a vector from the doubles given in the arguments.
*/
FGColumnVector3(double X, double Y, double Z) {
data[0] = X;
data[1] = Y;
data[2] = Z;
Debug(0);
}
inline void InitMatrix(void) { data[1]=0; data[2]=0; data[3]=0; }
inline void InitMatrix(double ff) { data[1]=ff; data[2]=ff; data[3]=ff; }
/** Copy constructor.
@param v Vector which is used for initialization.
Create copy of the vector given in the argument.
*/
FGColumnVector3(const FGColumnVector3& v) {
data[0] = v.data[0];
data[1] = v.data[1];
data[2] = v.data[2];
Debug(0);
}
double Magnitude(void);
FGColumnVector3 Normalize(void);
/** Destructor.
*/
~FGColumnVector3(void) { Debug(1); }
friend FGColumnVector3 operator*(const double scalar, const FGColumnVector3& A);
friend ostream& operator<<(ostream& os, const FGColumnVector3& col);
/** Read access the entries of the vector.
@param idx the component index.
Return the value of the matrix entry at the given index.
Indices are counted starting with 1.
Note that the index given in the argument is unchecked.
*/
double operator()(unsigned int idx) const { return Entry(idx); }
inline double operator()(int m) const { return data[m]; }
inline double& operator()(int m) { return data[m]; }
/** Write access the entries of the vector.
@param idx the component index.
Return a reference to the vector entry at the given index.
Indices are counted starting with 1.
Note that the index given in the argument is unchecked.
*/
double& operator()(unsigned int idx) { return Entry(idx); }
FGColumnVector3 multElementWise(const FGColumnVector3& V);
/** Read access the entries of the vector.
@param idx the component index.
Return the value of the matrix entry at the given index.
Indices are counted starting with 1.
This function is just a shortcut for the @ref double
operator()(unsigned int idx) const function. It is
used internally to access the elements in a more convenient way.
Note that the index given in the argument is unchecked.
*/
double Entry(unsigned int idx) const { return data[idx-1]; }
/** Write access the entries of the vector.
@param idx the component index.
Return a reference to the vector entry at the given index.
Indices are counted starting with 1.
This function is just a shortcut for the @ref double&
operator()(unsigned int idx) function. It is
used internally to access the elements in a more convenient way.
Note that the index given in the argument is unchecked.
*/
double& Entry(unsigned int idx) { return data[idx-1]; }
/** Assignment operator.
@param b source vector.
Copy the content of the vector given in the argument into *this.
*/
FGColumnVector3& operator=(const FGColumnVector3& b) {
data[0] = b.data[0];
data[1] = b.data[1];
data[2] = b.data[2];
return *this;
}
/** Multiplication by a scalar.
@param scalar scalar value to multiply the vector with.
@return The resulting vector from the multiplication with that scalar.
Multiply the vector with the scalar given in the argument.
*/
FGColumnVector3 operator*(const double scalar) const {
return FGColumnVector3(scalar*Entry(1), scalar*Entry(2), scalar*Entry(3));
}
/** Multiply by 1/scalar.
@param scalar scalar value to devide the vector through.
@return The resulting vector from the division through that scalar.
Multiply the vector with the 1/scalar given in the argument.
*/
FGColumnVector3 operator/(const double scalar) const;
/** Cross product multiplication.
@param v vector to multiply with.
@return The resulting vector from the cross product multiplication.
Compute and return the cross product of the current vector with
the given argument.
*/
FGColumnVector3 operator*(const FGColumnVector3& V) const {
return FGColumnVector3( Entry(2) * V(3) - Entry(3) * V(2),
Entry(3) * V(1) - Entry(1) * V(3),
Entry(1) * V(2) - Entry(2) * V(1) );
}
/** Addition operator.
*/
FGColumnVector3 operator+(const FGColumnVector3& B) const {
return FGColumnVector3( Entry(1) + B(1), Entry(2) + B(2), Entry(3) + B(3) );
}
/** Subtraction operator.
*/
FGColumnVector3 operator-(const FGColumnVector3& B) const {
return FGColumnVector3( Entry(1) - B(1), Entry(2) - B(2), Entry(3) - B(3) );
}
/** Subtract an other vector.
*/
FGColumnVector3& operator-=(const FGColumnVector3 &B) {
Entry(1) -= B(1);
Entry(2) -= B(2);
Entry(3) -= B(3);
return *this;
}
/** Add an other vector.
*/
FGColumnVector3& operator+=(const FGColumnVector3 &B) {
Entry(1) += B(1);
Entry(2) += B(2);
Entry(3) += B(3);
return *this;
}
/** Scale by a scalar.
*/
FGColumnVector3& operator*=(const double scalar) {
Entry(1) *= scalar;
Entry(2) *= scalar;
Entry(3) *= scalar;
return *this;
}
/** Scale by a 1/scalar.
*/
FGColumnVector3& operator/=(const double scalar);
void InitMatrix(void) { data[0] = data[1] = data[2] = 0.0; }
void InitMatrix(double a) { data[0] = data[1] = data[2] = a; }
void InitMatrix(double a, double b, double c) {
data[0]=a; data[1]=b; data[2]=c;
}
/** Length of the vector.
Compute and return the euclidean norm of this vector.
*/
double Magnitude(void) const;
/** Normialze.
Normalize the vector to have the Magnitude() == 1.0. If the vector
is equal to zero it is left untouched.
*/
FGColumnVector3& Normalize(void);
// ??? Is this something sensible ??
FGColumnVector3 multElementWise(const FGColumnVector3& V) const;
// little trick here.
struct AssignRef {
AssignRef(FGColumnVector3& r, int i) : Ref(r), idx(i) {}
AssignRef operator<<(const double ff) {
Ref.Entry(idx) = ff;
return AssignRef(Ref, idx+1);
}
FGColumnVector3& Ref;
int idx;
};
AssignRef operator<<(const double ff) {
Entry(1) = ff;
return AssignRef(*this, 2);
}
private:
double data[4];
int rowCtr;
double data[3];
void Debug(int from);
};
/** Scalar multiplication.
@param scalar scalar value to multiply with.
@param A Vector to multiply.
Multiply the Vector with a scalar value.
*/
inline FGColumnVector3 operator*(double scalar, const FGColumnVector3& A) {
// use already defined operation.
return A*scalar;
}
/** Write vector to a stream.
@param os Stream to write to.
@param M Matrix to write.
Write the matrix to a stream.
*/
ostream& operator<<(ostream& os, const FGColumnVector3& col);
} // namespace JSBSim
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endif

5
src/FDM/JSBSim/FGColumnVector4.cpp
View file

@ -42,7 +42,10 @@ FGColumnVector4::FGColumnVector4(void)
FGColumnVector4::FGColumnVector4(double A, double B, double C, double D)
{
rowCtr = 1;
data[1]=0;data[2]=0;data[3]=0;data[4]=0;
data[1]=A;
data[2]=B;
data[3]=C;
data[4]=D;
Debug(0);
}

12
src/FDM/JSBSim/FGColumnVector4.h
View file

@ -31,6 +31,7 @@ INCLUDES
SG_USING_STD(cerr);
SG_USING_STD(cout);
SG_USING_STD(endl);
SG_USING_STD(sqrt);
#else
# include <string>
# if defined (sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
@ -50,6 +51,7 @@ INCLUDES
using std::cerr;
using std::cout;
using std::endl;
using std::sqrt;
# endif
using std::string;
#endif
@ -88,22 +90,22 @@ public:
FGColumnVector4(double A, double B, double C, double D);
FGColumnVector4(const FGColumnVector4& b);
~FGColumnVector4(void);
FGColumnVector4 operator=(const FGColumnVector4& b);
FGColumnVector4 operator*(const double scalar);
FGColumnVector4 operator/(const double scalar);
FGColumnVector4 operator+(const FGColumnVector4& B); // must not return reference
FGColumnVector4 operator-(const FGColumnVector4& B);
void operator-=(const FGColumnVector4 &B);
void operator+=(const FGColumnVector4 &B);
void operator*=(const double scalar);
void operator/=(const double scalar);
inline double operator()(int m) const { return data[m]; }
inline double& operator()(int m) { return data[m]; }
FGColumnVector4& operator<<(const double ff);
inline void InitMatrix(void) { data[1]=0; data[2]=0; data[3]=0; data[4]=0; }

118
src/FDM/JSBSim/FGFCS.cpp
View file

@ -1,52 +1,44 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGFCS.cpp
Author: Jon Berndt
Date started: 12/12/98
Purpose: Model the flight controls
Called by: FDMExec
------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
This class models the flight controls for a specific airplane
HISTORY
--------------------------------------------------------------------------------
12/12/98 JSB Created
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGFCS.h"
#include "FGState.h"
#include "FGFDMExec.h"
#include "FGAtmosphere.h"
#include "FGAircraft.h"
#include "FGTranslation.h"
#include "FGRotation.h"
#include "FGPosition.h"
#include "FGAuxiliary.h"
#include "FGOutput.h"
#include "FGPropertyManager.h"
#include "filtersjb/FGFilter.h"
@ -82,7 +74,7 @@ FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex)
LeftBrake = RightBrake = CenterBrake = 0.0;
APAttitudeSetPt = APAltitudeSetPt = APHeadingSetPt = APAirspeedSetPt = 0.0;
DoNormalize=true;
eMode = mNone;
bind();
@ -90,7 +82,7 @@ FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex)
DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
}
for (i=0;i<NNorm;i++) { ToNormalize[i]=-1;}
Debug(0);
}
@ -115,7 +107,7 @@ FGFCS::~FGFCS()
for (i=0;i<APComponents.size();i++) delete APComponents[i];
for (i=0;i<FCSComponents.size();i++) delete FCSComponents[i];
Debug(1);
}
@ -218,7 +210,7 @@ double FGFCS::GetThrottlePos(int engineNum) const
<< " engines exist, but attempted throttle position setting is for engine "
<< engineNum << endl;
}
return 0.0;
return 0.0;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -321,7 +313,7 @@ bool FGFCS::Load(FGConfigFile* AC_cfg)
return false;
} else {
AC_cfg = FCS_cfg; // set local config file object pointer to FCS config
// file object pointer
// file object pointer
}
}
} else {
@ -339,7 +331,7 @@ bool FGFCS::Load(FGConfigFile* AC_cfg)
} else {
cerr << endl << "Unknown FCS delimiter" << endl << endl;
}
if (debug_lvl > 0) cout << " Control System Name: " << Name << endl;
while ((token = AC_cfg->GetValue()) != string("/" + delimiter)) {
@ -382,14 +374,14 @@ bool FGFCS::Load(FGConfigFile* AC_cfg)
string nodeName;
for (i=0; i<Components->size(); i++) {
if ( (((*Components)[i])->GetType() == "AEROSURFACE_SCALE"
|| ((*Components)[i])->GetType() == "KINEMAT")
&& ((*Components)[i])->GetOutputNode() ) {
if ( (((*Components)[i])->GetType() == "AEROSURFACE_SCALE"
|| ((*Components)[i])->GetType() == "KINEMAT")
&& ((*Components)[i])->GetOutputNode() ) {
nodeName = ((*Components)[i])->GetOutputNode()->GetName();
if ( nodeName == "elevator-pos-rad" ) {
ToNormalize[iDe]=i;
} else if ( nodeName == "left-aileron-pos-rad"
} else if ( nodeName == "left-aileron-pos-rad"
|| nodeName == "aileron-pos-rad" ) {
ToNormalize[iDaL]=i;
} else if ( nodeName == "right-aileron-pos-rad" ) {
@ -404,8 +396,8 @@ bool FGFCS::Load(FGConfigFile* AC_cfg)
ToNormalize[iDf]=i;
}
}
}
}
if (delimiter == "FLIGHT_CONTROL") bindModel();
eMode = mNone;
@ -443,7 +435,7 @@ string FGFCS::GetComponentName(int idx)
break;
}
return string("");
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -526,19 +518,19 @@ void FGFCS::AddThrottle(void)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGFCS::Normalize(void) {
//not all of these are guaranteed to be defined for every model
//those that are have an index >=0 in the ToNormalize array
//ToNormalize is filled in Load()
if ( ToNormalize[iDe] > -1 ) {
DePos[ofNorm] = FCSComponents[ToNormalize[iDe]]->GetOutputPct();
}
if ( ToNormalize[iDaL] > -1 ) {
DaLPos[ofNorm] = FCSComponents[ToNormalize[iDaL]]->GetOutputPct();
}
if ( ToNormalize[iDaR] > -1 ) {
DaRPos[ofNorm] = FCSComponents[ToNormalize[iDaR]]->GetOutputPct();
}
@ -546,19 +538,19 @@ void FGFCS::Normalize(void) {
if ( ToNormalize[iDr] > -1 ) {
DrPos[ofNorm] = FCSComponents[ToNormalize[iDr]]->GetOutputPct();
}
if ( ToNormalize[iDsb] > -1 ) {
if ( ToNormalize[iDsb] > -1 ) {
DsbPos[ofNorm] = FCSComponents[ToNormalize[iDsb]]->GetOutputPct();
}
if ( ToNormalize[iDsp] > -1 ) {
DspPos[ofNorm] = FCSComponents[ToNormalize[iDsp]]->GetOutputPct();
}
if ( ToNormalize[iDf] > -1 ) {
DfPos[ofNorm] = FCSComponents[ToNormalize[iDf]]->GetOutputPct();
}
DePos[ofMag] = fabs(DePos[ofRad]);
DaLPos[ofMag] = fabs(DaLPos[ofRad]);
DaRPos[ofMag] = fabs(DaRPos[ofRad]);
@ -566,9 +558,9 @@ void FGFCS::Normalize(void) {
DsbPos[ofMag] = fabs(DsbPos[ofRad]);
DspPos[ofMag] = fabs(DspPos[ofRad]);
DfPos[ofMag] = fabs(DfPos[ofRad]);
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGFCS::bind(void)
@ -613,7 +605,7 @@ void FGFCS::bind(void)
&FGFCS::GetGearCmd,
&FGFCS::SetGearCmd,
true);
PropertyManager->Tie("fcs/left-aileron-pos-rad", this,ofRad,
&FGFCS::GetDaLPos,
&FGFCS::SetDaLPos,
@ -626,7 +618,7 @@ void FGFCS::bind(void)
&FGFCS::GetDaLPos,
&FGFCS::SetDaLPos,
true);
PropertyManager->Tie("fcs/right-aileron-pos-rad", this,ofRad,
&FGFCS::GetDaRPos,
&FGFCS::SetDaRPos,
@ -639,20 +631,20 @@ void FGFCS::bind(void)
&FGFCS::GetDaRPos,
&FGFCS::SetDaRPos,
true);
PropertyManager->Tie("fcs/elevator-pos-rad", this, ofRad,
&FGFCS::GetDePos,
&FGFCS::SetDePos,
true );
PropertyManager->Tie("fcs/elevator-pos-norm", this,ofNorm,
&FGFCS::GetDePos,
&FGFCS::GetDePos,
&FGFCS::SetDePos,
true );
PropertyManager->Tie("fcs/mag-elevator-pos-rad", this,ofMag,
&FGFCS::GetDePos,
&FGFCS::SetDePos,
true );
PropertyManager->Tie("fcs/rudder-pos-rad", this,ofRad,
&FGFCS::GetDrPos,
&FGFCS::SetDrPos,
@ -665,7 +657,7 @@ void FGFCS::bind(void)
&FGFCS::GetDrPos,
&FGFCS::SetDrPos,
true);
PropertyManager->Tie("fcs/flap-pos-deg", this,ofRad,
&FGFCS::GetDfPos,
&FGFCS::SetDfPos,
@ -674,7 +666,7 @@ void FGFCS::bind(void)
&FGFCS::GetDfPos,
&FGFCS::SetDfPos,
true);
PropertyManager->Tie("fcs/speedbrake-pos-rad", this,ofRad,
&FGFCS::GetDsbPos,
&FGFCS::SetDsbPos,
@ -687,7 +679,7 @@ void FGFCS::bind(void)
&FGFCS::GetDsbPos,
&FGFCS::SetDsbPos,
true);
PropertyManager->Tie("fcs/spoiler-pos-rad", this,ofRad,
&FGFCS::GetDspPos,
&FGFCS::SetDspPos,
@ -700,7 +692,7 @@ void FGFCS::bind(void)
&FGFCS::GetDspPos,
&FGFCS::SetDspPos,
true);
PropertyManager->Tie("gear/gear-pos-norm", this,
&FGFCS::GetGearPos,
&FGFCS::SetGearPos,
@ -798,46 +790,46 @@ void FGFCS::bindModel(void)
{
unsigned i;
char tmp[80];
for (i=0; i<ThrottleCmd.size(); i++) {
snprintf(tmp,80,"fcs/throttle-cmd-norm[%u]",i);
PropertyManager->Tie( tmp,this,i,
&FGFCS::GetThrottleCmd,
&FGFCS::SetThrottleCmd,
true );
snprintf(tmp,80,"fcs/throttle-pos-norm[%u]",i);
snprintf(tmp,80,"fcs/throttle-pos-norm[%u]",i);
PropertyManager->Tie( tmp,this,i,
&FGFCS::GetThrottlePos,
&FGFCS::SetThrottlePos,
true );
if ( MixtureCmd.size() > i ) {
snprintf(tmp,80,"fcs/mixture-cmd-norm[%u]",i);
snprintf(tmp,80,"fcs/mixture-cmd-norm[%u]",i);
PropertyManager->Tie( tmp,this,i,
&FGFCS::GetMixtureCmd,
&FGFCS::SetMixtureCmd,
true );
snprintf(tmp,80,"fcs/mixture-pos-norm[%u]",i);
snprintf(tmp,80,"fcs/mixture-pos-norm[%u]",i);
PropertyManager->Tie( tmp,this,i,
&FGFCS::GetMixturePos,
&FGFCS::SetMixturePos,
true );
}
if ( PropAdvanceCmd.size() > i ) {
snprintf(tmp,80,"fcs/advance-cmd-norm[%u]",i);
snprintf(tmp,80,"fcs/advance-cmd-norm[%u]",i);
PropertyManager->Tie( tmp,this,i,
&FGFCS::GetPropAdvanceCmd,
&FGFCS::SetPropAdvanceCmd,
true );
snprintf(tmp,80,"fcs/advance-pos-norm[%u]",i);
snprintf(tmp,80,"fcs/advance-pos-norm[%u]",i);
PropertyManager->Tie( tmp,this,i,
&FGFCS::GetPropAdvance,
&FGFCS::SetPropAdvance,
true );
}
}
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGFCS::unbind(FGPropertyManager *node)
@ -848,8 +840,8 @@ void FGFCS::unbind(FGPropertyManager *node)
unbind( (FGPropertyManager*)node->getChild(i) );
} else if( node->getChild(i)->isTied() ) {
node->getChild(i)->untie();
}
}
}
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

362
src/FDM/JSBSim/FGForce.cpp
View file

@ -1,182 +1,180 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Source: FGForce.cpp
Author: Tony Peden
Date started: 6/10/00
------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
HISTORY
--------------------------------------------------------------------------------
6/10/00 TP Created
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
The purpose of this class is to provide storage for computed forces and
encapsulate all the functionality associated with transforming those
forces from their native coord system to the body system. This includes
computing the moments due to the difference between the point of application
and the cg.
*/
#include "FGFDMExec.h"
#include "FGAircraft.h"
#include "FGTranslation.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
#include "FGForce.h"
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_FORCE;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGForce::FGForce(FGFDMExec *FDMExec) :
ttype(tNone),
fdmex(FDMExec)
{
mT(1,1) = 1; //identity matrix
mT(2,2) = 1;
mT(3,3) = 1;
vSense.InitMatrix(1);
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGForce::~FGForce()
{
Debug(1);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3& FGForce::GetBodyForces(void)
{
vFb = Transform()*(vFn.multElementWise(vSense));
// Find the distance from this vector's acting location to the cg; this
// needs to be done like this to convert from structural to body coords.
// CG and RP values are in inches
vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn);
vM = vMn + vDXYZ*vFb;
return vFb;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 FGForce::Transform(void)
{
switch(ttype) {
case tWindBody:
return fdmex->GetState()->GetTs2b();
case tLocalBody:
return fdmex->GetState()->GetTl2b();
case tCustom:
case tNone:
return mT;
default:
cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
exit(1);
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)
{
if (ttype == tCustom) {
double cp,sp,cr,sr,cy,sy;
cp=cos(bpitch); sp=sin(bpitch);
cr=cos(broll); sr=sin(broll);
cy=cos(byaw); sy=sin(byaw);
mT(1,1)=cp*cy;
mT(1,2)=cp*sy;
mT(1,3)=-1*sp;
mT(2,1)=sr*sp*cy-cr*sy;
mT(2,2)=sr*sp*sy+cr*cy;
mT(2,3)=sr*cp;
mT(3,1)=cr*sp*cy+sr*sy;
mT(3,2)=cr*sp*sy-sr*cy;
mT(3,3)=cr*cp;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// The bitmasked value choices are as follows:
// unset: In this case (the default) JSBSim would only print
// out the normally expected messages, essentially echoing
// the config files as they are read. If the environment
// variable is not set, debug_lvl is set to 1 internally
// 0: This requests JSBSim not to output any messages
// whatsoever.
// 1: This value explicity requests the normal JSBSim
// startup messages
// 2: This value asks for a message to be printed out when
// a class is instantiated
// 4: When this value is set, a message is displayed when a
// FGModel object executes its Run() method
// 8: When this value is set, various runtime state variables
// are printed out periodically
// 16: When set various parameters are sanity checked and
// a message is printed out when they go out of bounds
void FGForce::Debug(int from)
{
if (debug_lvl <= 0) return;
if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) { // Constructor
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
if (from == 0) cout << "Instantiated: FGForce" << endl;
if (from == 1) cout << "Destroyed: FGForce" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 ) { // Runtime state variables
}
if (debug_lvl & 16) { // Sanity checking
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
cout << IdSrc << endl;
cout << IdHdr << endl;
}
}
}
}
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Source: FGForce.cpp
Author: Tony Peden
Date started: 6/10/00
------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
HISTORY
--------------------------------------------------------------------------------
6/10/00 TP Created
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
The purpose of this class is to provide storage for computed forces and
encapsulate all the functionality associated with transforming those
forces from their native coord system to the body system. This includes
computing the moments due to the difference between the point of application
and the cg.
*/
#include "FGFDMExec.h"
#include "FGAircraft.h"
#include "FGTranslation.h"
#include "FGMassBalance.h"
#include "FGState.h"
#include "FGForce.h"
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_FORCE;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGForce::FGForce(FGFDMExec *FDMExec) :
ttype(tNone),
fdmex(FDMExec)
{
mT(1,1) = 1; //identity matrix
mT(2,2) = 1;
mT(3,3) = 1;
vSense.InitMatrix(1);
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGForce::~FGForce()
{
Debug(1);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3& FGForce::GetBodyForces(void)
{
vFb = Transform()*(vFn.multElementWise(vSense));
// Find the distance from this vector's acting location to the cg; this
// needs to be done like this to convert from structural to body coords.
// CG and RP values are in inches
vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn);
vM = vMn + vDXYZ*vFb;
return vFb;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 FGForce::Transform(void)
{
switch(ttype) {
case tWindBody:
return fdmex->GetState()->GetTs2b();
case tLocalBody:
return fdmex->GetState()->GetTl2b();
case tCustom:
case tNone:
return mT;
default:
cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
exit(1);
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)
{
if (ttype == tCustom) {
double cp,sp,cr,sr,cy,sy;
cp=cos(bpitch); sp=sin(bpitch);
cr=cos(broll); sr=sin(broll);
cy=cos(byaw); sy=sin(byaw);
mT(1,1)=cp*cy;
mT(1,2)=cp*sy;
mT(1,3)=-1*sp;
mT(2,1)=sr*sp*cy-cr*sy;
mT(2,2)=sr*sp*sy+cr*cy;
mT(2,3)=sr*cp;
mT(3,1)=cr*sp*cy+sr*sy;
mT(3,2)=cr*sp*sy-sr*cy;
mT(3,3)=cr*cp;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// The bitmasked value choices are as follows:
// unset: In this case (the default) JSBSim would only print
// out the normally expected messages, essentially echoing
// the config files as they are read. If the environment
// variable is not set, debug_lvl is set to 1 internally
// 0: This requests JSBSim not to output any messages
// whatsoever.
// 1: This value explicity requests the normal JSBSim
// startup messages
// 2: This value asks for a message to be printed out when
// a class is instantiated
// 4: When this value is set, a message is displayed when a
// FGModel object executes its Run() method
// 8: When this value is set, various runtime state variables
// are printed out periodically
// 16: When set various parameters are sanity checked and
// a message is printed out when they go out of bounds
void FGForce::Debug(int from)
{
if (debug_lvl <= 0) return;
if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) { // Constructor
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
if (from == 0) cout << "Instantiated: FGForce" << endl;
if (from == 1) cout << "Destroyed: FGForce" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 ) { // Runtime state variables
}
if (debug_lvl & 16) { // Sanity checking
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
cout << IdSrc << endl;
cout << IdHdr << endl;
}
}
}
}

1
src/FDM/JSBSim/FGForce.h
View file

@ -61,7 +61,6 @@ INCLUDES
#include "FGJSBBase.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS

3
src/FDM/JSBSim/FGGroundReactions.h
View file

@ -52,8 +52,7 @@ INCLUDES
#include "FGModel.h"
#include "FGConfigFile.h"
#include "FGLGear.h"
#include "FGInertial.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#define ID_GROUNDREACTIONS "$Id$"

8
src/FDM/JSBSim/FGInertial.cpp
View file

@ -37,6 +37,7 @@ INCLUDES
#include "FGInertial.h"
#include "FGPosition.h"
#include "FGState.h"
#include "FGMassBalance.h"
namespace JSBSim {
@ -63,7 +64,7 @@ FGInertial::FGInertial(FGFDMExec* fgex) : FGModel(fgex)
vCoriolis.InitMatrix();
vCentrifugal.InitMatrix();
vGravity.InitMatrix();
bind();
Debug(0);
@ -110,8 +111,9 @@ bool FGInertial::Run(void)
vRadius(eDown) = Position->GetRadius();
vCentrifugal(eDown) = -vOmegaLocal.Magnitude() * vOmegaLocal.Magnitude() * vRadius(eDown);
vForces = State->GetTl2b() * MassBalance->GetMass() * (vCoriolis + vCentrifugal + vGravity);
// vForces = State->GetTl2b() * MassBalance->GetMass() * (vCoriolis + vCentrifugal + vGravity);
vForces = State->GetTl2b() * MassBalance->GetMass() * vGravity;
return false;
} else {
return true;

2
src/FDM/JSBSim/FGInertial.h
View file

@ -51,9 +51,7 @@ INCLUDES
#include "FGModel.h"
#include "FGConfigFile.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS

116
src/FDM/JSBSim/FGInitialCondition.cpp
View file

@ -1,59 +1,53 @@
/*******************************************************************************
Header: FGInitialCondition.cpp
Author: Tony Peden
Date started: 7/1/99
------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
HISTORY
--------------------------------------------------------------------------------
7/1/99 TP Created
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
The purpose of this class is to take a set of initial conditions and provide
a kinematically consistent set of body axis velocity components, euler
angles, and altitude. This class does not attempt to trim the model i.e.
the sim will most likely start in a very dynamic state (unless, of course,
you have chosen your IC's wisely) even after setting it up with this class.
********************************************************************************
INCLUDES
*******************************************************************************/
#include "FGInitialCondition.h"
#include "FGFDMExec.h"
#include "FGState.h"
#include "FGInertial.h"
#include "FGAtmosphere.h"
#include "FGAerodynamics.h"
#include "FGFCS.h"
#include "FGAircraft.h"
#include "FGTranslation.h"
#include "FGRotation.h"
#include "FGPosition.h"
#include "FGAuxiliary.h"
#include "FGOutput.h"
#include "FGConfigFile.h"
#include "FGPropertyManager.h"
@ -96,7 +90,7 @@ FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec)
} else {
cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
}
Debug(0);
}
@ -226,7 +220,7 @@ void FGInitialCondition::SetBetaRadIC(double tt) {
beta=tt;
sbeta=sin(beta); cbeta=cos(beta);
getTheta();
}
//******************************************************************************
@ -285,7 +279,7 @@ double FGInitialCondition::GetVBodyFpsIC(void) {
return v;
else {
return vt*sbeta - vw;
}
}
}
//******************************************************************************
@ -293,7 +287,7 @@ double FGInitialCondition::GetVBodyFpsIC(void) {
double FGInitialCondition::GetWBodyFpsIC(void) {
if( lastSpeedSet == setvg )
return w;
else
else
return vt*salpha*cbeta -ww;
}
@ -310,41 +304,41 @@ void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD ) {
//******************************************************************************
// positive from left
void FGInitialCondition::SetHeadWindKtsIC(double head){
void FGInitialCondition::SetHeadWindKtsIC(double head){
whead=head*ktstofps;
lastWindSet=setwhc;
lastWindSet=setwhc;
calcWindUVW();
if(lastSpeedSet == setvg)
SetVgroundFpsIC(vg);
}
}
//******************************************************************************
void FGInitialCondition::SetCrossWindKtsIC(double cross){
wcross=cross*ktstofps;
lastWindSet=setwhc;
void FGInitialCondition::SetCrossWindKtsIC(double cross){
wcross=cross*ktstofps;
lastWindSet=setwhc;
calcWindUVW();
if(lastSpeedSet == setvg)
SetVgroundFpsIC(vg);
}
}
//******************************************************************************
void FGInitialCondition::SetWindDownKtsIC(double wD) {
wdown=wD;
void FGInitialCondition::SetWindDownKtsIC(double wD) {
wdown=wD;
calcWindUVW();
if(lastSpeedSet == setvg)
SetVgroundFpsIC(vg);
}
}
//******************************************************************************
void FGInitialCondition::SetWindMagKtsIC(double mag) {
wmag=mag*ktstofps;
lastWindSet=setwmd;
calcWindUVW();
calcWindUVW();
if(lastSpeedSet == setvg)
SetVgroundFpsIC(vg);
}
@ -354,7 +348,7 @@ void FGInitialCondition::SetWindMagKtsIC(double mag) {
void FGInitialCondition::SetWindDirDegIC(double dir) {
wdir=dir*degtorad;
lastWindSet=setwmd;
calcWindUVW();
calcWindUVW();
if(lastSpeedSet == setvg)
SetVgroundFpsIC(vg);
}
@ -363,7 +357,7 @@ void FGInitialCondition::SetWindDirDegIC(double dir) {
//******************************************************************************
void FGInitialCondition::calcWindUVW(void) {
switch(lastWindSet) {
case setwmd:
wnorth=wmag*cos(wdir);
@ -375,7 +369,7 @@ void FGInitialCondition::calcWindUVW(void) {
break;
case setwned:
break;
}
}
uw=wnorth*ctheta*cpsi +
weast*ctheta*spsi -
wdown*stheta;
@ -385,14 +379,14 @@ void FGInitialCondition::calcWindUVW(void) {
ww=wnorth*(cphi*stheta*cpsi + sphi*spsi) +
weast*(cphi*stheta*spsi - sphi*cpsi) +
wdown*cphi*ctheta;
/* cout << "FGInitialCondition::calcWindUVW: wnorth, weast, wdown "
<< wnorth << ", " << weast << ", " << wdown << endl;
cout << "FGInitialCondition::calcWindUVW: theta, phi, psi "
<< theta << ", " << phi << ", " << psi << endl;
cout << "FGInitialCondition::calcWindUVW: uw, vw, ww "
<< uw << ", " << vw << ", " << ww << endl; */
<< uw << ", " << vw << ", " << ww << endl; */
}
@ -508,9 +502,9 @@ bool FGInitialCondition::getMachFromVcas(double *Mach,double vcas) {
bool FGInitialCondition::getAlpha(void) {
bool result=false;
double guess=theta-gamma;
if(vt < 0.01) return 0;
xlo=xhi=0;
xmin=fdmex->GetAerodynamics()->GetAlphaCLMin();
xmax=fdmex->GetAerodynamics()->GetAlphaCLMax();
@ -531,9 +525,9 @@ bool FGInitialCondition::getAlpha(void) {
bool FGInitialCondition::getTheta(void) {
bool result=false;
double guess=alpha+gamma;
if(vt < 0.01) return 0;
xlo=xhi=0;
xmin=-89;xmax=89;
sfunc=&FGInitialCondition::GammaEqOfTheta;
@ -679,7 +673,7 @@ bool FGInitialCondition::solve(double *y,double x)
while ((fabs(d) > eps) && (i < 100)) {
d=(x3-x1)/d0;
x2 = x1-d*d0*f1/(f3-f1);
f2=(this->*sfunc)(x2)-x;
//cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
//cout << " " << f1 << "," << f2 << "," << f3 << endl;
@ -710,19 +704,19 @@ bool FGInitialCondition::solve(double *y,double x)
//******************************************************************************
double FGInitialCondition::GetWindDirDegIC(void) {
if(weast != 0.0)
if(weast != 0.0)
return atan2(weast,wnorth)*radtodeg;
else if(wnorth > 0)
else if(wnorth > 0)
return 0.0;
else
return 180.0;
}
}
//******************************************************************************
bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
{
string resetDef;
string resetDef, acpath;
string token="";
double temp;
@ -730,20 +724,20 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
string sep = "/";
# else
string sep = ";";
#endif
#endif
if( useStoredPath ) {
string acpath = fdmex->GetAircraftPath() + sep + fdmex->GetModelName();
acpath = fdmex->GetAircraftPath() + sep + fdmex->GetModelName();
resetDef = acpath + sep + rstfile + ".xml";
} else {
resetDef = rstfile;
}
}
FGConfigFile resetfile(resetDef);
if (!resetfile.IsOpen()) {
cerr << "Failed to open reset file: " << resetDef << endl;
return false;
}
}
resetfile.GetNextConfigLine();
token = resetfile.GetValue();
@ -752,13 +746,13 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
<< " does not appear to be a reset file" << endl;
return false;
}
resetfile.GetNextConfigLine();
resetfile >> token;
while (token != string("/initialize") && token != string("EOF")) {
if (token == "UBODY" ) { resetfile >> temp; SetUBodyFpsIC(temp); }
if (token == "VBODY" ) { resetfile >> temp; SetVBodyFpsIC(temp); }
if (token == "WBODY" ) { resetfile >> temp; SetWBodyFpsIC(temp); }
if (token == "UBODY" ) { resetfile >> temp; SetUBodyFpsIC(temp); }
if (token == "VBODY" ) { resetfile >> temp; SetVBodyFpsIC(temp); }
if (token == "WBODY" ) { resetfile >> temp; SetWBodyFpsIC(temp); }
if (token == "LATITUDE" ) { resetfile >> temp; SetLatitudeDegIC(temp); }
if (token == "LONGITUDE" ) { resetfile >> temp; SetLongitudeDegIC(temp); }
if (token == "PHI" ) { resetfile >> temp; SetRollAngleDegIC(temp); }
@ -780,7 +774,7 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
}
fdmex->RunIC();
return true;
}
@ -966,7 +960,7 @@ void FGInitialCondition::unbind(void){
/* PropertyManager->Untie("ic/vw-dir-deg"); */
PropertyManager->Untie("ic/roc-fps");
/* PropertyManager->Untie("ic/u-fps");
PropertyManager->Untie("ic/v-fps");
PropertyManager->Untie("ic/w-fps"); */

3
src/FDM/JSBSim/FGInitialCondition.h
View file

@ -49,10 +49,7 @@ INCLUDES
#include "FGFDMExec.h"
#include "FGJSBBase.h"
#include "FGAtmosphere.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS

388
src/FDM/JSBSim/FGJSBBase.cpp
View file

@ -1,189 +1,199 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGJSBBase.cpp
Author: Jon S. Berndt
Date started: 07/01/01
Purpose: Encapsulates the JSBBase object
------------- Copyright (C) 2001 Jon S. Berndt (jsb@hal-pc.org) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
HISTORY
--------------------------------------------------------------------------------
07/01/01 JSB Created
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGJSBBase.h"
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_JSBBASE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
char FGJSBBase::highint[5] = {27, '[', '1', 'm', '\0' };
char FGJSBBase::halfint[5] = {27, '[', '2', 'm', '\0' };
char FGJSBBase::normint[6] = {27, '[', '2', '2', 'm', '\0' };
char FGJSBBase::reset[5] = {27, '[', '0', 'm', '\0' };
char FGJSBBase::underon[5] = {27, '[', '4', 'm', '\0' };
char FGJSBBase::underoff[6] = {27, '[', '2', '4', 'm', '\0' };
char FGJSBBase::fgblue[6] = {27, '[', '3', '4', 'm', '\0' };
char FGJSBBase::fgcyan[6] = {27, '[', '3', '6', 'm', '\0' };
char FGJSBBase::fgred[6] = {27, '[', '3', '1', 'm', '\0' };
char FGJSBBase::fggreen[6] = {27, '[', '3', '2', 'm', '\0' };
char FGJSBBase::fgdef[6] = {27, '[', '3', '9', 'm', '\0' };
const double FGJSBBase::radtodeg = 57.29578;
const double FGJSBBase::degtorad = 1.745329E-2;
const double FGJSBBase::hptoftlbssec = 550.0;
const double FGJSBBase::psftoinhg = 0.014138;
const double FGJSBBase::fpstokts = 0.592484;
const double FGJSBBase::ktstofps = 1.68781;
const double FGJSBBase::inchtoft = 0.08333333;
const double FGJSBBase::in3tom3 = 1.638706E-5;
double FGJSBBase::Reng = 1716.0;
const double FGJSBBase::SHRatio = 1.40;
const string FGJSBBase::needed_cfg_version = "1.61";
const string FGJSBBase::JSBSim_version = "0.9.5";
std::queue <FGJSBBase::Message*> 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::Message*> 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

2
src/FDM/JSBSim/FGJSBBase.h
View file

@ -240,6 +240,8 @@ protected:
static const double in3tom3;
static double Reng; // Specific Gas Constant,ft^2/(sec^2*R)
static const double SHRatio;
static const double lbtoslug;
static const double slugtolb;
static const string needed_cfg_version;
static const string JSBSim_version;
};

1171
src/FDM/JSBSim/FGLGear.cpp
View file

File diff suppressed because it is too large Load diff

6
src/FDM/JSBSim/FGLGear.h
View file

@ -42,13 +42,11 @@ INCLUDES
# include <simgear/compiler.h>
#endif
#include "FGJSBBase.h"
#include "FGFDMExec.h"
#include <string>
#include "FGConfigFile.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
#include "FGFDMExec.h"
#include "FGJSBBase.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS

141
src/FDM/JSBSim/FGMassBalance.cpp
View file

@ -39,6 +39,7 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGMassBalance.h"
#include "FGPropulsion.h"
#include "FGPropertyManager.h"
namespace JSBSim {
@ -55,11 +56,13 @@ FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex)
{
Name = "FGMassBalance";
Weight = EmptyWeight = Mass = 0.0;
Ixx = Iyy = Izz = Ixy = Ixz = 0.0;
baseIxx = baseIyy = baseIzz = baseIxy = baseIxz = 0.0;
vbaseXYZcg(eX) = 0.0;
vbaseXYZcg(eY) = 0.0;
vbaseXYZcg(eZ) = 0.0;
vbaseXYZcg.InitMatrix(0.0);
baseJ.InitMatrix();
mJ.InitMatrix();
mJinv.InitMatrix();
pmJ.InitMatrix();
bind();
Debug(0);
@ -77,24 +80,54 @@ FGMassBalance::~FGMassBalance()
bool FGMassBalance::Run(void)
{
double denom, k1, k2, k3, k4, k5, k6;
double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
if (!FGModel::Run()) {
Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetPointMassWeight();
Mass = Weight / Inertial->gravity();
Mass = lbtoslug*Weight;
// Calculate new CG here.
// Calculate new CG
vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg
+ GetPointMassMoment() ) / Weight;
// Calculate new moments of inertia here
// Calculate new total moments of inertia
Ixx = baseIxx + Propulsion->GetTanksIxx(vXYZcg) + GetPMIxx();
Iyy = baseIyy + Propulsion->GetTanksIyy(vXYZcg) + GetPMIyy();
Izz = baseIzz + Propulsion->GetTanksIzz(vXYZcg) + GetPMIzz();
Ixy = baseIxy + Propulsion->GetTanksIxy(vXYZcg) + GetPMIxy();
Ixz = baseIxz + Propulsion->GetTanksIxz(vXYZcg) + GetPMIxz();
// At first it is the base configuration inertia matrix ...
mJ = baseJ;
// ... with the additional term originating from the parallel axis theorem.
mJ += GetPointmassInertia( lbtoslug * EmptyWeight, vbaseXYZcg );
// Then add the contributions from the additional pointmasses.
mJ += CalculatePMInertias();
mJ += Propulsion->CalculateTankInertias();
Ixx = mJ(1,1);
Iyy = mJ(2,2);
Izz = mJ(3,3);
Ixy = -mJ(1,2);
Ixz = -mJ(1,3);
Iyz = -mJ(2,3);
// Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
k1 = (Iyy*Izz - Iyz*Iyz);
k2 = (Iyz*Ixz + Ixy*Izz);
k3 = (Ixy*Iyz + Iyy*Ixz);
denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 );
k1 = k1*denom;
k2 = k2*denom;
k3 = k3*denom;
k4 = (Izz*Ixx - Ixz*Ixz)*denom;
k5 = (Ixy*Ixz + Iyz*Ixx)*denom;
k6 = (Ixx*Iyy - Ixy*Ixy)*denom;
mJinv.InitMatrix( k1, k2, k3,
k2, k4, k5,
k3, k5, k6 );
Debug(2);
@ -108,7 +141,7 @@ bool FGMassBalance::Run(void)
void FGMassBalance::AddPointMass(double weight, double X, double Y, double Z)
{
PointMassLoc.push_back(*(new FGColumnVector3(X, Y, Z)));
PointMassLoc.push_back(FGColumnVector3(X, Y, Z));
PointMassWeight.push_back(weight);
}
@ -138,62 +171,19 @@ FGColumnVector3& FGMassBalance::GetPointMassMoment(void)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGMassBalance::GetPMIxx(void)
FGMatrix33& FGMassBalance::CalculatePMInertias(void)
{
double I = 0.0;
for (unsigned int i=0; i<PointMassLoc.size(); i++) {
I += (PointMassLoc[i](eX)-vXYZcg(eX))*(PointMassLoc[i](eX)-vXYZcg(eX))*PointMassWeight[i];
}
I /= (144.0*Inertial->gravity());
return I;
}
unsigned int size;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
size = PointMassLoc.size();
if (size == 0) return pmJ;
double FGMassBalance::GetPMIyy(void)
{
double I = 0.0;
for (unsigned int i=0; i<PointMassLoc.size(); i++) {
I += (PointMassLoc[i](eY)-vXYZcg(eY))*(PointMassLoc[i](eY)-vXYZcg(eY))*PointMassWeight[i];
}
I /= (144.0*Inertial->gravity());
return I;
}
pmJ = FGMatrix33();
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for (unsigned int i=0; i<size; i++)
pmJ += GetPointmassInertia( lbtoslug * PointMassWeight[i], PointMassLoc[i] );
double FGMassBalance::GetPMIzz(void)
{
double I = 0.0;
for (unsigned int i=0; i<PointMassLoc.size(); i++) {
I += (PointMassLoc[i](eZ)-vXYZcg(eZ))*(PointMassLoc[i](eZ)-vXYZcg(eZ))*PointMassWeight[i];
}
I /= (144.0*Inertial->gravity());
return I;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGMassBalance::GetPMIxy(void)
{
double I = 0.0;
for (unsigned int i=0; i<PointMassLoc.size(); i++) {
I += (PointMassLoc[i](eX)-vXYZcg(eX))*(PointMassLoc[i](eY)-vXYZcg(eY))*PointMassWeight[i];
}
I /= (144.0*Inertial->gravity());
return I;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGMassBalance::GetPMIxz(void)
{
double I = 0.0;
for (unsigned int i=0; i<PointMassLoc.size(); i++) {
I += (PointMassLoc[i](eX)-vXYZcg(eX))*(PointMassLoc[i](eZ)-vXYZcg(eZ))*PointMassWeight[i];
}
I /= (144.0*Inertial->gravity());
return I;
return pmJ;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -232,22 +222,12 @@ FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMassBalance::bind(void)
{
{
typedef double (FGMassBalance::*PMF)(int) const;
PropertyManager->Tie("inertia/mass-slugs", this,
&FGMassBalance::GetMass);
PropertyManager->Tie("inertia/weight-lbs", this,
&FGMassBalance::GetWeight);
PropertyManager->Tie("inertia/ixx-lbsft2", this,
&FGMassBalance::GetIxx);
PropertyManager->Tie("inertia/iyy-lbsft2", this,
&FGMassBalance::GetIyy);
PropertyManager->Tie("inertia/izz-lbsft2", this,
&FGMassBalance::GetIzz);
PropertyManager->Tie("inertia/ixy-lbsft2", this,
&FGMassBalance::GetIxy);
PropertyManager->Tie("inertia/ixz-lbsft2", this,
&FGMassBalance::GetIxz);
PropertyManager->Tie("inertia/cg-x-ft", this,1,
(PMF)&FGMassBalance::GetXYZcg);
PropertyManager->Tie("inertia/cg-y-ft", this,2,
@ -262,11 +242,6 @@ void FGMassBalance::unbind(void)
{
PropertyManager->Untie("inertia/mass-slugs");
PropertyManager->Untie("inertia/weight-lbs");
PropertyManager->Untie("inertia/ixx-lbsft2");
PropertyManager->Untie("inertia/iyy-lbsft2");
PropertyManager->Untie("inertia/izz-lbsft2");
PropertyManager->Untie("inertia/ixy-lbsft2");
PropertyManager->Untie("inertia/ixz-lbsft2");
PropertyManager->Untie("inertia/cg-x-ft");
PropertyManager->Untie("inertia/cg-y-ft");
PropertyManager->Untie("inertia/cg-z-ft");
@ -301,8 +276,8 @@ void FGMassBalance::Debug(int from)
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
if (from == 0) cout << "Instantiated: FGPiston" << endl;
if (from == 1) cout << "Destroyed: FGPiston" << endl;
if (from == 0) cout << "Instantiated: FGMassBalance" << endl;
if (from == 1) cout << "Destroyed: FGMassBalance" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}

78
src/FDM/JSBSim/FGMassBalance.h
View file

@ -39,7 +39,8 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGModel.h"
#include "FGPropulsion.h"
#include "FGColumnVector3.h"
#include "FGMatrix33.h"
#include <vector>
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -76,39 +77,53 @@ public:
inline double GetMass(void) const {return Mass;}
inline double GetWeight(void) const {return Weight;}
inline double GetIxx(void) const {return Ixx;}
inline double GetIyy(void) const {return Iyy;}
inline double GetIzz(void) const {return Izz;}
inline double GetIxy(void) const {return Ixy;}
inline double GetIxz(void) const {return Ixz;}
inline FGColumnVector3& GetXYZcg(void) {return vXYZcg;}
inline double GetXYZcg(int axis) const {return vXYZcg(axis);}
/** Computes the inertia contribution of a pointmass.
Computes and returns the inertia matrix of a pointmass of mass
slugs at the given vector r in the structural frame. The units
should be for the mass in slug and the vector in the structural
frame as usual in inches.
@param slugs the mass of this single pointmass given in slugs
@param r the location of this single pointmass in the structural frame
*/
FGMatrix33 GetPointmassInertia(double slugs, const FGColumnVector3& r) const
{
FGColumnVector3 v = StructuralToBody( r );
FGColumnVector3 sv = slugs*v;
double xx = sv(1)*v(1);
double yy = sv(2)*v(2);
double zz = sv(3)*v(3);
double xy = -sv(1)*v(2);
double xz = -sv(1)*v(3);
double yz = -sv(2)*v(3);
return FGMatrix33( yy+zz, xy, xz,
xy, xx+zz, yz,
xz, yz, xx+yy );
}
/** Conversion from the structural frame to the body frame.
* Converts the argument \parm r given in the reference frame
* coordinate system to the body frame. The units of the structural
* frame are assumed to be in inches. The unit of the result is in
* ft.
Converts the location given in the structural frame
coordinate system to the body frame. The units of the structural
frame are assumed to be in inches. The unit of the result is in
ft.
@param r vector coordinate in the structural reference frame (X positive
aft, measurements in inches).
@return vector coordinate in the body frame, in feet.
*/
FGColumnVector3 StructuralToBody(const FGColumnVector3& r) const;
inline void SetEmptyWeight(double EW) { EmptyWeight = EW;}
inline void SetBaseIxx(double bixx) { baseIxx = bixx;}
inline void SetBaseIyy(double biyy) { baseIyy = biyy;}
inline void SetBaseIzz(double bizz) { baseIzz = bizz;}
inline void SetBaseIxy(double bixy) { baseIxy = bixy;}
inline void SetBaseIxz(double bixz) { baseIxz = bixz;}
inline void SetBaseCG(const FGColumnVector3& CG) {vbaseXYZcg = CG;}
inline void SetBaseCG(const FGColumnVector3& CG) {vbaseXYZcg = vXYZcg = CG;}
void AddPointMass(double weight, double X, double Y, double Z);
double GetPointMassWeight(void);
FGColumnVector3& GetPointMassMoment(void);
double GetPMIxx(void);
double GetPMIyy(void);
double GetPMIzz(void);
double GetPMIxy(void);
double GetPMIxz(void);
FGMatrix33& GetJ(void) {return mJ;}
FGMatrix33& GetJinv(void) {return mJinv;}
void SetAircraftBaseInertias(FGMatrix33 BaseJ) {baseJ = BaseJ;}
void bind(void);
void unbind(void);
@ -116,22 +131,19 @@ private:
double Weight;
double EmptyWeight;
double Mass;
double Ixx;
double Iyy;
double Izz;
double Ixy;
double Ixz;
double baseIxx;
double baseIyy;
double baseIzz;
double baseIxy;
double baseIxz;
FGMatrix33 mJ;
FGMatrix33 mJinv;
FGMatrix33 pmJ;
FGMatrix33 baseJ;
FGColumnVector3 vXYZcg;
FGColumnVector3 vXYZtank;
FGColumnVector3 vbaseXYZcg;
FGColumnVector3 vPMxyz;
vector <FGColumnVector3> PointMassLoc;
vector <double> PointMassWeight;
FGColumnVector3 PointMassCG;
FGMatrix33& CalculatePMInertias(void);
void Debug(int from);
};
}

836
src/FDM/JSBSim/FGMatrix33.cpp
View file

@ -1,445 +1,391 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGMatrix33.cpp
Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
Date started: 1998
Purpose: FGMatrix33 class
Called by: Various
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
HISTORY
--------------------------------------------------------------------------------
??/??/?? TP Created
03/16/2000 JSB Added exception throwing
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_MATRIX33;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33::FGMatrix33(void)
{
InitMatrix();
rowCtr = colCtr = 1;
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33::FGMatrix33(int r, int c)
{
InitMatrix();
rowCtr = colCtr = 1;
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33::FGMatrix33(const FGMatrix33& M)
{
rowCtr = colCtr = 1;
data[1][1] = M.data[1][1];
data[1][2] = M.data[1][2];
data[1][3] = M.data[1][3];
data[2][1] = M.data[2][1];
data[2][2] = M.data[2][2];
data[2][3] = M.data[2][3];
data[3][1] = M.data[3][1];
data[3][2] = M.data[3][2];
data[3][3] = M.data[3][3];
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33::~FGMatrix33(void)
{
rowCtr = colCtr = 1;
Debug(1);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ostream& operator<<(ostream& os, const FGMatrix33& M)
{
for (unsigned int i=1; i<=M.Rows(); i++) {
for (unsigned int j=1; j<=M.Cols(); j++) {
if (i == M.Rows() && j == M.Cols())
os << M.data[i][j];
else
os << M.data[i][j] << ", ";
}
}
return os;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33& FGMatrix33::operator<<(const double ff)
{
data[rowCtr][colCtr] = ff;
if (++colCtr > Cols()) {
colCtr = 1;
if (++rowCtr > Rows())
rowCtr = 1;
}
return *this;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
istream& operator>>(istream& is, FGMatrix33& M)
{
for (unsigned int i=1; i<=M.Rows(); i++) {
for (unsigned int j=1; j<=M.Cols(); j++) {
is >> M.data[i][j];
}
}
return is;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33& FGMatrix33::operator=(const FGMatrix33& M)
{
if (&M != this) {
data[1][1] = M.data[1][1];
data[1][2] = M.data[1][2];
data[1][3] = M.data[1][3];
data[2][1] = M.data[2][1];
data[2][2] = M.data[2][2];
data[2][3] = M.data[2][3];
data[3][1] = M.data[3][1];
data[3][2] = M.data[3][2];
data[3][3] = M.data[3][3];
}
return *this;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMatrix33::InitMatrix(double value)
{
if (data) {
data[1][1] = value;
data[1][2] = value;
data[1][3] = value;
data[2][1] = value;
data[2][2] = value;
data[2][3] = value;
data[3][1] = value;
data[3][2] = value;
data[3][3] = value;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMatrix33::InitMatrix(void)
{
this->InitMatrix(0);
}
// *****************************************************************************
// binary operators ************************************************************
// *****************************************************************************
FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M)
{
FGMatrix33 Diff;
Diff(1,1) = data[1][1] - M(1,1);
Diff(1,2) = data[1][2] - M(1,2);
Diff(1,3) = data[1][3] - M(1,3);
Diff(2,1) = data[2][1] - M(2,1);
Diff(2,2) = data[2][2] - M(2,2);
Diff(2,3) = data[2][3] - M(2,3);
Diff(3,1) = data[3][1] - M(3,1);
Diff(3,2) = data[3][2] - M(3,2);
Diff(3,3) = data[3][3] - M(3,3);
return Diff;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMatrix33::operator-=(const FGMatrix33 &M)
{
data[1][1] -= M(1,1);
data[1][2] -= M(1,2);
data[1][3] -= M(1,3);
data[2][1] -= M(2,1);
data[2][2] -= M(2,2);
data[2][3] -= M(2,3);
data[3][1] -= M(3,1);
data[3][2] -= M(3,2);
data[3][3] -= M(3,3);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M)
{
FGMatrix33 Sum;
Sum(1,1) = data[1][1] + M(1,1);
Sum(1,2) = data[1][2] + M(1,2);
Sum(1,3) = data[1][3] + M(1,3);
Sum(2,1) = data[2][1] + M(2,1);
Sum(2,2) = data[2][2] + M(2,2);
Sum(2,3) = data[2][3] + M(2,3);
Sum(3,1) = data[3][1] + M(3,1);
Sum(3,2) = data[3][2] + M(3,2);
Sum(3,3) = data[3][3] + M(3,3);
return Sum;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMatrix33::operator+=(const FGMatrix33 &M)
{
data[1][1] += M(1,1);
data[1][2] += M(1,2);
data[1][3] += M(1,3);
data[2][1] += M(2,1);
data[2][2] += M(2,2);
data[2][3] += M(2,3);
data[3][1] += M(3,1);
data[3][2] += M(3,2);
data[3][3] += M(3,3);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 FGMatrix33::operator*(const double scalar)
{
FGMatrix33 Product;
Product(1,1) = data[1][1] * scalar;
Product(1,2) = data[1][2] * scalar;
Product(1,3) = data[1][3] * scalar;
Product(2,1) = data[2][1] * scalar;
Product(2,2) = data[2][2] * scalar;
Product(2,3) = data[2][3] * scalar;
Product(3,1) = data[3][1] * scalar;
Product(3,2) = data[3][2] * scalar;
Product(3,3) = data[3][3] * scalar;
return Product;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 operator*(double scalar, FGMatrix33 &M)
{
FGMatrix33 Product;
Product(1,1) = M(1,1) * scalar;
Product(1,2) = M(1,2) * scalar;
Product(1,3) = M(1,3) * scalar;
Product(2,1) = M(2,1) * scalar;
Product(2,2) = M(2,2) * scalar;
Product(2,3) = M(2,3) * scalar;
Product(3,1) = M(3,1) * scalar;
Product(3,2) = M(3,2) * scalar;
Product(3,3) = M(3,3) * scalar;
return Product;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMatrix33::operator*=(const double scalar)
{
data[1][1] *= scalar;
data[1][2] *= scalar;
data[1][3] *= scalar;
data[2][1] *= scalar;
data[2][2] *= scalar;
data[2][3] *= scalar;
data[3][1] *= scalar;
data[3][2] *= scalar;
data[3][3] *= scalar;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M)
{
FGMatrix33 Product;
Product(1,1) = data[1][1]*M(1,1) + data[1][2]*M(2,1) + data[1][3]*M(3,1);
Product(1,2) = data[1][1]*M(1,2) + data[1][2]*M(2,2) + data[1][3]*M(3,2);
Product(1,3) = data[1][1]*M(1,3) + data[1][2]*M(2,3) + data[1][3]*M(3,3);
Product(2,1) = data[2][1]*M(1,1) + data[2][2]*M(2,1) + data[2][3]*M(3,1);
Product(2,2) = data[2][1]*M(1,2) + data[2][2]*M(2,2) + data[2][3]*M(3,2);
Product(2,3) = data[2][1]*M(1,3) + data[2][2]*M(2,3) + data[2][3]*M(3,3);
Product(3,1) = data[3][1]*M(1,1) + data[3][2]*M(2,1) + data[3][3]*M(3,1);
Product(3,2) = data[3][1]*M(1,2) + data[3][2]*M(2,2) + data[3][3]*M(3,2);
Product(3,3) = data[3][1]*M(1,3) + data[3][2]*M(2,3) + data[3][3]*M(3,3);
return Product;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMatrix33::operator*=(const FGMatrix33& M)
{
double a,b,c;
a = data[1][1]; b=data[1][2]; c=data[1][3];
data[1][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
data[1][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
data[1][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
a = data[2][1]; b=data[2][2]; c=data[2][3];
data[2][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
data[2][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
data[2][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
a = data[3][1]; b=data[3][2]; c=data[3][3];
data[3][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
data[3][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
data[3][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 FGMatrix33::operator/(const double scalar)
{
FGMatrix33 Quot;
if ( scalar != 0 ) {
double tmp = 1.0/scalar;
Quot(1,1) = data[1][1] * tmp;
Quot(1,2) = data[1][2] * tmp;
Quot(1,3) = data[1][3] * tmp;
Quot(2,1) = data[2][1] * tmp;
Quot(2,2) = data[2][2] * tmp;
Quot(2,3) = data[2][3] * tmp;
Quot(3,1) = data[3][1] * tmp;
Quot(3,2) = data[3][2] * tmp;
Quot(3,3) = data[3][3] * tmp;
} else {
MatrixException mE;
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
throw mE;
}
return Quot;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMatrix33::operator/=(const double scalar)
{
if ( scalar != 0 ) {
double tmp = 1.0/scalar;
data[1][1] *= tmp;
data[1][2] *= tmp;
data[1][3] *= tmp;
data[2][1] *= tmp;
data[2][2] *= tmp;
data[2][3] *= tmp;
data[3][1] *= tmp;
data[3][2] *= tmp;
data[3][3] *= tmp;
} else {
MatrixException mE;
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
throw mE;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMatrix33::T(void)
{
for (unsigned int i=1; i<=3; i++) {
for (unsigned int j=i+1; j<=3; j++) {
double tmp = data[i][j];
data[i][j] = data[j][i];
data[j][i] = tmp;
}
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& Col)
{
FGColumnVector3 Product;
Product(1) = data[1][1]*Col(1) + data[1][2]*Col(2) + data[1][3]*Col(3);
Product(2) = data[2][1]*Col(1) + data[2][2]*Col(2) + data[2][3]*Col(3);
Product(3) = data[3][1]*Col(1) + data[3][2]*Col(2) + data[3][3]*Col(3);
return Product;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// The bitmasked value choices are as follows:
// unset: In this case (the default) JSBSim would only print
// out the normally expected messages, essentially echoing
// the config files as they are read. If the environment
// variable is not set, debug_lvl is set to 1 internally
// 0: This requests JSBSim not to output any messages
// whatsoever.
// 1: This value explicity requests the normal JSBSim
// startup messages
// 2: This value asks for a message to be printed out when
// a class is instantiated
// 4: When this value is set, a message is displayed when a
// FGModel object executes its Run() method
// 8: When this value is set, various runtime state variables
// are printed out periodically
// 16: When set various parameters are sanity checked and
// a message is printed out when they go out of bounds
void FGMatrix33::Debug(int from)
{
if (debug_lvl <= 0) return;
if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) { // Constructor
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
if (from == 1) cout << "Destroyed: FGMatrix33" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 ) { // Runtime state variables
}
if (debug_lvl & 16) { // Sanity checking
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
cout << IdSrc << endl;
cout << IdHdr << endl;
}
}
}
}
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGMatrix33.cpp
Author: Tony Peden, Jon Berndt, Mathias Frolich
Date started: 1998
Purpose: FGMatrix33 class
Called by: Various
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
HISTORY
--------------------------------------------------------------------------------
??/??/?? TP Created
03/16/2000 JSB Added exception throwing
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_MATRIX33;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33::FGMatrix33(void)
{
data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
data[6] = data[7] = data[8] = 0.0;
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ostream& operator<<(ostream& os, const FGMatrix33& M)
{
for (unsigned int i=1; i<=M.Rows(); i++) {
for (unsigned int j=1; j<=M.Cols(); j++) {
if (i == M.Rows() && j == M.Cols())
os << M(i,j);
else
os << M(i,j) << ", ";
}
}
return os;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
istream& operator>>(istream& is, FGMatrix33& M)
{
for (unsigned int i=1; i<=M.Rows(); i++) {
for (unsigned int j=1; j<=M.Cols(); j++) {
is >> M(i,j);
}
}
return is;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGMatrix33::Determinant(void) const {
return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
+ Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
- Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 FGMatrix33::Inverse(void) const {
// Compute the inverse of a general matrix using Cramers rule.
// I guess googling for cramers rule gives tons of references
// for this. :)
double rdet = 1.0/Determinant();
double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
return FGMatrix33( i11, i12, i13,
i21, i22, i23,
i31, i32, i33 );
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMatrix33::InitMatrix(void)
{
data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
data[6] = data[7] = data[8] = 0.0;
}
// *****************************************************************************
// binary operators ************************************************************
// *****************************************************************************
FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
{
return FGMatrix33( Entry(1,1) - M(1,1),
Entry(1,2) - M(1,2),
Entry(1,3) - M(1,3),
Entry(2,1) - M(2,1),
Entry(2,2) - M(2,2),
Entry(2,3) - M(2,3),
Entry(3,1) - M(3,1),
Entry(3,2) - M(3,2),
Entry(3,3) - M(3,3) );
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
{
data[0] -= M.data[0];
data[1] -= M.data[1];
data[2] -= M.data[2];
data[3] -= M.data[3];
data[4] -= M.data[4];
data[5] -= M.data[5];
data[6] -= M.data[6];
data[7] -= M.data[7];
data[8] -= M.data[8];
return *this;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
{
return FGMatrix33( Entry(1,1) + M(1,1),
Entry(1,2) + M(1,2),
Entry(1,3) + M(1,3),
Entry(2,1) + M(2,1),
Entry(2,2) + M(2,2),
Entry(2,3) + M(2,3),
Entry(3,1) + M(3,1),
Entry(3,2) + M(3,2),
Entry(3,3) + M(3,3) );
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
{
Entry(1,1) += M(1,1);
Entry(1,2) += M(1,2);
Entry(1,3) += M(1,3);
Entry(2,1) += M(2,1);
Entry(2,2) += M(2,2);
Entry(2,3) += M(2,3);
Entry(3,1) += M(3,1);
Entry(3,2) += M(3,2);
Entry(3,3) += M(3,3);
return *this;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 FGMatrix33::operator*(const double scalar) const
{
return FGMatrix33( scalar * Entry(1,1),
scalar * Entry(1,2),
scalar * Entry(1,3),
scalar * Entry(2,1),
scalar * Entry(2,2),
scalar * Entry(2,3),
scalar * Entry(3,1),
scalar * Entry(3,2),
scalar * Entry(3,3) );
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 operator*(double scalar, FGMatrix33 &M)
{
return FGMatrix33( scalar * M(1,1),
scalar * M(1,2),
scalar * M(1,3),
scalar * M(2,1),
scalar * M(2,2),
scalar * M(2,3),
scalar * M(3,1),
scalar * M(3,2),
scalar * M(3,3) );
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33& FGMatrix33::operator*=(const double scalar)
{
Entry(1,1) *= scalar;
Entry(1,2) *= scalar;
Entry(1,3) *= scalar;
Entry(2,1) *= scalar;
Entry(2,2) *= scalar;
Entry(2,3) *= scalar;
Entry(3,1) *= scalar;
Entry(3,2) *= scalar;
Entry(3,3) *= scalar;
return *this;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
{
// FIXME: Make compiler friendlier
FGMatrix33 Product;
Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
return Product;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
{
// FIXME: Make compiler friendlier
double a,b,c;
a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
return *this;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33 FGMatrix33::operator/(const double scalar) const
{
FGMatrix33 Quot;
if ( scalar != 0 ) {
double tmp = 1.0/scalar;
Quot(1,1) = Entry(1,1) * tmp;
Quot(1,2) = Entry(1,2) * tmp;
Quot(1,3) = Entry(1,3) * tmp;
Quot(2,1) = Entry(2,1) * tmp;
Quot(2,2) = Entry(2,2) * tmp;
Quot(2,3) = Entry(2,3) * tmp;
Quot(3,1) = Entry(3,1) * tmp;
Quot(3,2) = Entry(3,2) * tmp;
Quot(3,3) = Entry(3,3) * tmp;
} else {
MatrixException mE;
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
throw mE;
}
return Quot;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33& FGMatrix33::operator/=(const double scalar)
{
if ( scalar != 0 ) {
double tmp = 1.0/scalar;
Entry(1,1) *= tmp;
Entry(1,2) *= tmp;
Entry(1,3) *= tmp;
Entry(2,1) *= tmp;
Entry(2,2) *= tmp;
Entry(2,3) *= tmp;
Entry(3,1) *= tmp;
Entry(3,2) *= tmp;
Entry(3,3) *= tmp;
} else {
MatrixException mE;
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
throw mE;
}
return *this;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMatrix33::T(void)
{
for (unsigned int i=1; i<=3; i++) {
for (unsigned int j=i+1; j<=3; j++) {
double tmp = Entry(i,j);
Entry(i,j) = Entry(j,i);
Entry(j,i) = tmp;
}
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
double tmp1 = v(1)*Entry(1,1);
double tmp2 = v(1)*Entry(2,1);
double tmp3 = v(1)*Entry(3,1);
tmp1 += v(2)*Entry(1,2);
tmp2 += v(2)*Entry(2,2);
tmp3 += v(2)*Entry(3,2);
tmp1 += v(3)*Entry(1,3);
tmp2 += v(3)*Entry(2,3);
tmp3 += v(3)*Entry(3,3);
return FGColumnVector3( tmp1, tmp2, tmp3 );
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// The bitmasked value choices are as follows:
// unset: In this case (the default) JSBSim would only print
// out the normally expected messages, essentially echoing
// the config files as they are read. If the environment
// variable is not set, debug_lvl is set to 1 internally
// 0: This requests JSBSim not to output any messages
// whatsoever.
// 1: This value explicity requests the normal JSBSim
// startup messages
// 2: This value asks for a message to be printed out when
// a class is instantiated
// 4: When this value is set, a message is displayed when a
// FGModel object executes its Run() method
// 8: When this value is set, various runtime state variables
// are printed out periodically
// 16: When set various parameters are sanity checked and
// a message is printed out when they go out of bounds
void FGMatrix33::Debug(int from)
{
if (debug_lvl <= 0) return;
if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) { // Constructor
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
if (from == 1) cout << "Destroyed: FGMatrix33" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 ) { // Runtime state variables
}
if (debug_lvl & 16) { // Sanity checking
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
cout << IdSrc << endl;
cout << IdHdr << endl;
}
}
}
}

380
src/FDM/JSBSim/FGMatrix33.h
View file

@ -1,13 +1,14 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Header: FGMatrix33.h
Author: Originally by Tony Peden [formatted and adapted here by Jon Berndt]
Author: Tony Peden, Jon Berndt, Mathias Frolich
Date started: Unknown
HISTORY
--------------------------------------------------------------------------------
??/??/?? TP Created
03/16/2000 JSB Added exception throwing
03/06/2004 MF Rework of the code to make it a bit compiler friendlier
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
@ -94,7 +95,8 @@ public:
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/** Handles matrix math operations.
/** Handles matrix math operations.
@author Tony Peden, Jon Berndt, Mathias Froelich
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -104,49 +106,365 @@ DECLARATION: FGMatrix33
class FGMatrix33 : public FGJSBBase
{
public:
enum {
eRows = 3,
eColumns = 3
};
/** Default initializer.
Create a zero matrix.
*/
FGMatrix33(void);
FGMatrix33(int r, int c);
FGMatrix33(const FGMatrix33& A);
~FGMatrix33(void);
FGMatrix33& operator=(const FGMatrix33& A);
inline double operator()(unsigned int row, unsigned int col) const {return data[row][col];}
inline double& operator()(unsigned int row, unsigned int col) {return data[row][col];}
/** Copy constructor.
FGColumnVector3 operator*(const FGColumnVector3& Col);
@param M Matrix which is used for initialization.
inline unsigned int Rows(void) const { return 3; }
inline unsigned int Cols(void) const { return 3; }
Create copy of the matrix given in the argument.
*/
FGMatrix33(const FGMatrix33& M) {
Entry(1,1) = M.Entry(1,1);
Entry(2,1) = M.Entry(2,1);
Entry(3,1) = M.Entry(3,1);
Entry(1,2) = M.Entry(1,2);
Entry(2,2) = M.Entry(2,2);
Entry(3,2) = M.Entry(3,2);
Entry(1,3) = M.Entry(1,3);
Entry(2,3) = M.Entry(2,3);
Entry(3,3) = M.Entry(3,3);
Debug(0);
}
/** Initialization by given values.
@param m11 value of the 1,1 Matrix element.
@param m12 value of the 1,2 Matrix element.
@param m13 value of the 1,3 Matrix element.
@param m21 value of the 2,1 Matrix element.
@param m22 value of the 2,2 Matrix element.
@param m23 value of the 2,3 Matrix element.
@param m31 value of the 3,1 Matrix element.
@param m32 value of the 3,2 Matrix element.
@param m33 value of the 3,3 Matrix element.
Create a matrix from the doubles given in the arguments.
*/
FGMatrix33(double m11, double m12, double m13,
double m21, double m22, double m23,
double m31, double m32, double m33) {
Entry(1,1) = m11;
Entry(2,1) = m21;
Entry(3,1) = m31;
Entry(1,2) = m12;
Entry(2,2) = m22;
Entry(3,2) = m32;
Entry(1,3) = m13;
Entry(2,3) = m23;
Entry(3,3) = m33;
Debug(0);
}
/** Destructor.
*/
~FGMatrix33(void) { Debug(1); }
/** Read access the entries of the matrix.
@param row Row index.
@param col Column index.
@return the value of the matrix entry at the given row and
column indices. Indices are counted starting with 1.
*/
double operator()(unsigned int row, unsigned int col) const {
return Entry(row, col);
}
/** Write access the entries of the matrix.
Note that the indices given in the arguments are unchecked.
@param row Row index.
@param col Column index.
@return a reference to the matrix entry at the given row and
column indices. Indices are counted starting with 1.
*/
double& operator()(unsigned int row, unsigned int col) {
return Entry(row, col);
}
/** Read access the entries of the matrix.
This function is just a shortcut for the @ref double&
operator()(unsigned int row, unsigned int col) function. It is
used internally to access the elements in a more convenient way.
Note that the indices given in the arguments are unchecked.
@param row Row index.
@param col Column index.
@return the value of the matrix entry at the given row and
column indices. Indices are counted starting with 1.
*/
double Entry(unsigned int row, unsigned int col) const {
return data[(col-1)*eRows+row-1];
}
/** Write access the entries of the matrix.
This function is just a shortcut for the @ref double&
operator()(unsigned int row, unsigned int col) function. It is
used internally to access the elements in a more convenient way.
Note that the indices given in the arguments are unchecked.
@param row Row index.
@param col Column index.
@return a reference to the matrix entry at the given row and
column indices. Indices are counted starting with 1.
*/
double& Entry(unsigned int row, unsigned int col) {
return data[(col-1)*eRows+row-1];
}
/** Number of rows in the matrix.
@return the number of rows in the matrix.
*/
unsigned int Rows(void) const { return eRows; }
/** Number of cloumns in the matrix.
@return the number of columns in the matrix.
*/
unsigned int Cols(void) const { return eColumns; }
/** Transposed matrix.
This function only returns the transpose of this matrix. This matrix itself
remains unchanged.
@return the transposed matrix.
*/
FGMatrix33 Transposed(void) const {
return FGMatrix33( Entry(1,1), Entry(2,1), Entry(3,1),
Entry(1,2), Entry(2,2), Entry(3,2),
Entry(1,3), Entry(2,3), Entry(3,3) );
}
/** Transposes this matrix.
This function only transposes this matrix. Nothing is returned.
*/
void T(void);
/** Initialize the matrix.
This function initializes a matrix to all 0.0.
*/
void InitMatrix(void);
void InitMatrix(double value);
//friend FGMatrix33 operator*(double scalar,FGMatrix33& A);
FGMatrix33 operator-(const FGMatrix33& B);
FGMatrix33 operator+(const FGMatrix33& B);
FGMatrix33 operator*(const FGMatrix33& B);
FGMatrix33 operator*(const double scalar);
FGMatrix33 operator/(const double scalar);
FGMatrix33& operator<<(const double ff);
/** Initialize the matrix.
This function initializes a matrix to user specified values.
*/
void InitMatrix(double m11, double m12, double m13,
double m21, double m22, double m23,
double m31, double m32, double m33) {
Entry(1,1) = m11;
Entry(2,1) = m21;
Entry(3,1) = m31;
Entry(1,2) = m12;
Entry(2,2) = m22;
Entry(3,2) = m32;
Entry(1,3) = m13;
Entry(2,3) = m23;
Entry(3,3) = m33;
}
friend ostream& operator<<(ostream& os, const FGMatrix33& M);
friend istream& operator>>(istream& is, FGMatrix33& M);
/** Determinant of the matrix.
@return the determinant of the matrix.
*/
double Determinant(void) const;
void operator-=(const FGMatrix33 &B);
void operator+=(const FGMatrix33 &B);
void operator*=(const FGMatrix33 &B);
void operator*=(const double scalar);
void operator/=(const double scalar);
/** Return if the matrix is invertible.
Checks and returns if the matrix is nonsingular and thus
invertible. This is done by simply computing the determinant and
check if it is zero. Note that this test does not cover any
instabilities caused by nearly singular matirces using finite
arithmetics. It only checks exact singularity.
*/
bool Invertible(void) const { return 0.0 != Determinant(); }
protected:
double data[4][4];
/** Return the inverse of the matrix.
Computes and returns if the inverse of the matrix. It is computed
by Cramers Rule. Also there are no checks performed if the matrix
is invertible. If you are not sure that it really is check this
with the @ref Invertible() call before.
*/
FGMatrix33 Inverse(void) const;
/** Assignment operator.
@param A source matrix.
Copy the content of the matrix given in the argument into *this.
*/
FGMatrix33& operator=(const FGMatrix33& A) {
data[0] = A.data[0];
data[1] = A.data[1];
data[2] = A.data[2];
data[3] = A.data[3];
data[4] = A.data[4];
data[5] = A.data[5];
data[6] = A.data[6];
data[7] = A.data[7];
data[8] = A.data[8];
return *this;
}
/** Matrix vector multiplication.
@param v vector to multiply with.
@return matric vector product.
Compute and return the product of the current matrix with the
vector given in the argument.
*/
FGColumnVector3 operator*(const FGColumnVector3& v) const;
/** Matrix subtraction.
@param B matrix to add to.
@return difference of the matrices.
Compute and return the sum of the current matrix and the matrix
B given in the argument.
*/
FGMatrix33 operator-(const FGMatrix33& B) const;
/** Matrix addition.
@param B matrix to add to.
@return sum of the matrices.
Compute and return the sum of the current matrix and the matrix
B given in the argument.
*/
FGMatrix33 operator+(const FGMatrix33& B) const;
/** Matrix product.
@param B matrix to add to.
@return product of the matrices.
Compute and return the product of the current matrix and the matrix
B given in the argument.
*/
FGMatrix33 operator*(const FGMatrix33& B) const;
/** Multiply the matrix with a scalar.
@param scalar scalar factor to multiply with.
@return scaled matrix.
Compute and return the product of the current matrix with the
scalar value scalar given in the argument.
*/
FGMatrix33 operator*(const double scalar) const;
/** Multiply the matrix with 1.0/scalar.
@param scalar scalar factor to divide through.
@return scaled matrix.
Compute and return the product of the current matrix with the
scalar value 1.0/scalar, where scalar is given in the argument.
*/
FGMatrix33 operator/(const double scalar) const;
/** In place matrix subtraction.
@param B matrix to subtract.
@return reference to the current matrix.
Compute the diffence from the current matrix and the matrix B
given in the argument.
*/
FGMatrix33& operator-=(const FGMatrix33 &B);
/** In place matrix addition.
@param B matrix to add.
@return reference to the current matrix.
Compute the sum of the current matrix and the matrix B
given in the argument.
*/
FGMatrix33& operator+=(const FGMatrix33 &B);
/** In place matrix multiplication.
@param B matrix to multiply with.
@return reference to the current matrix.
Compute the product of the current matrix and the matrix B
given in the argument.
*/
FGMatrix33& operator*=(const FGMatrix33 &B);
/** In place matrix scale.
@param scalar scalar value to multiply with.
@return reference to the current matrix.
Compute the product of the current matrix and the scalar value scalar
given in the argument.
*/
FGMatrix33& operator*=(const double scalar);
/** In place matrix scale.
@param scalar scalar value to divide through.
@return reference to the current matrix.
Compute the product of the current matrix and the scalar value
1.0/scalar, where scalar is given in the argument.
*/
FGMatrix33& operator/=(const double scalar);
private:
void TransposeSquare(void);
unsigned int rowCtr, colCtr;
double data[eRows*eColumns];
void Debug(int from);
};
/** Scalar multiplication.
@param scalar scalar value to multiply with.
@param A Matrix to multiply.
Multiply the Matrix with a scalar value.
*/
inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) {
// use already defined operation.
return A*scalar;
}
/** Write matrix to a stream.
@param os Stream to write to.
@param M Matrix to write.
Write the matrix to a stream.
*/
ostream& operator<<(ostream& os, const FGMatrix33& M);
/** Read matrix from a stream.
@param os Stream to read from.
@param M Matrix to initialize with the values from the stream.
Read matrix from a stream.
*/
istream& operator>>(istream& is, FGMatrix33& M);
} // namespace JSBSim
#endif

1
src/FDM/JSBSim/FGNozzle.cpp
View file

@ -43,6 +43,7 @@ INCLUDES
#endif
#include "FGNozzle.h"
#include "FGAtmosphere.h"
namespace JSBSim {

1284
src/FDM/JSBSim/FGOutput.cpp
View file

File diff suppressed because it is too large Load diff

740
src/FDM/JSBSim/FGPosition.cpp
View file

@ -1,373 +1,367 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGPosition.cpp
Author: Jon S. Berndt
Date started: 01/05/99
Purpose: Integrate the EOM to determine instantaneous position
Called by: FGFDMExec
------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
This class encapsulates the integration of rates and accelerations to get the
current position of the aircraft.
HISTORY
--------------------------------------------------------------------------------
01/05/99 JSB Created
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
COMMENTS, REFERENCES, and NOTES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate
School, January 1994
[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
JSC 12960, July 1977
[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
NASA-Ames", NASA CR-2497, January 1975
[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
Wiley & Sons, 1979 ISBN 0-471-03032-5
[5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
1982 ISBN 0-471-08936-2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#ifdef FGFS
# include <simgear/compiler.h>
# ifdef SG_HAVE_STD_INCLUDES
# include <cmath>
# include <iomanip>
# else
# include <math.h>
# include <iomanip.h>
# endif
#else
# if defined(sgi) && !defined(__GNUC__)
# include <math.h>
# if (_COMPILER_VERSION < 740)
# include <iomanip.h>
# else
# include <iomanip>
# endif
# else
# include <cmath>
# include <iomanip>
# endif
#endif
#include "FGPosition.h"
#include "FGAtmosphere.h"
#include "FGState.h"
#include "FGFDMExec.h"
#include "FGFCS.h"
#include "FGAircraft.h"
#include "FGMassBalance.h"
#include "FGTranslation.h"
#include "FGRotation.h"
#include "FGAuxiliary.h"
#include "FGOutput.h"
#include "FGPropertyManager.h"
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_POSITION;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
extern double globalTriNormal[3];
extern double globalSceneryAltitude;
extern double globalSeaLevelRadius;
FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex)
{
Name = "FGPosition";
LongitudeDot = LatitudeDot = RadiusDot = 0.0;
for (int i=0;i<4;i++) {
LatitudeDot_prev[i] = 0.0;
LongitudeDot_prev[i] = 0.0;
RadiusDot_prev[i] = 0.0;
}
vVRPoffset.InitMatrix();
Longitude = Latitude = 0.0;
LongitudeVRP = LatitudeVRP = 0.0;
gamma = Vt = Vground = 0.0;
hoverbmac = hoverbcg = 0.0;
psigt = 0.0;
bind();
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGPosition::~FGPosition(void)
{
unbind();
Debug(1);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGPosition::InitModel(void)
{
FGModel::InitModel();
h = 3.0; // Est. height of aircraft cg off runway
SeaLevelRadius = Inertial->RefRadius(); // For initialization ONLY
Radius = SeaLevelRadius + h;
RunwayRadius = SeaLevelRadius;
DistanceAGL = Radius - RunwayRadius; // Geocentric
vRunwayNormal(3) = -1.0; // Initialized for standalone mode
b = 1;
return true;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/*
Purpose: Called on a schedule to perform Positioning algorithms
Notes: [TP] Make sure that -Vt <= hdot <= Vt, which, of course, should always
be the case
[JB] Run in standalone mode, SeaLevelRadius will be reference radius.
In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
*/
bool FGPosition::Run(void)
{
double cosLat;
double hdot_Vt;
if (!FGModel::Run()) {
GetState();
Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) );
psigt = atan2(vVel(eEast), vVel(eNorth));
if (psigt < 0.0)
psigt += 2*M_PI;
Radius = h + SeaLevelRadius;
cosLat = cos(Latitude);
if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat);
LatitudeDot = vVel(eNorth) / Radius;
RadiusDot = -vVel(eDown);
Longitude += State->Integrate(FGState::TRAPZ, dt*rate, LongitudeDot, LongitudeDot_prev);
Latitude += State->Integrate(FGState::TRAPZ, dt*rate, LatitudeDot, LatitudeDot_prev);
Radius += State->Integrate(FGState::TRAPZ, dt*rate, RadiusDot, RadiusDot_prev);
h = Radius - SeaLevelRadius; // Geocentric
vVRPoffset = State->GetTb2l() * MassBalance->StructuralToBody(Aircraft->GetXYZvrp());
// vVRP - the vector to the Visual Reference Point - now contains the
// offset from the CG to the VRP, in units of feet, in the Local coordinate
// frame, where X points north, Y points East, and Z points down. This needs
// to be converted to Lat/Lon/Alt, now.
if (cosLat != 0)
LongitudeVRP = vVRPoffset(eEast) / (Radius * cosLat) + Longitude;
LatitudeVRP = vVRPoffset(eNorth) / Radius + Latitude;
hVRP = h - vVRPoffset(eDown);
/*
cout << "Lat/Lon/Alt : " << Latitude << " / " << Longitude << " / " << h << endl;
cout << "Lat/Lon/Alt VRP: " << LatitudeVRP << " / " << LongitudeVRP << " / " << hVRP << endl << endl;
*/
DistanceAGL = Radius - RunwayRadius; // Geocentric
hoverbcg = DistanceAGL/b;
vMac = State->GetTb2l()*MassBalance->StructuralToBody(Aircraft->GetXYZrp());
hoverbmac = (DistanceAGL + vMac(3)) / b;
if (Vt > 0) {
hdot_Vt = RadiusDot/Vt;
if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt);
} else {
gamma = 0.0;
}
return false;
} else {
return true;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPosition::GetState(void)
{
dt = State->Getdt();
Vt = Translation->GetVt();
vVel = State->GetTb2l() * Translation->GetUVW();
vVelDot = State->GetTb2l() * Translation->GetUVWdot();
b = Aircraft->GetWingSpan();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPosition::Seth(double tt)
{
h = tt;
Radius = h + SeaLevelRadius;
DistanceAGL = Radius - RunwayRadius; // Geocentric
hoverbcg = DistanceAGL/b;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPosition::SetDistanceAGL(double tt)
{
DistanceAGL=tt;
Radius = RunwayRadius + DistanceAGL;
h = Radius - SeaLevelRadius;
hoverbcg = DistanceAGL/b;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPosition::bind(void)
{
PropertyManager->Tie("velocities/v-north-fps", this,
&FGPosition::GetVn);
PropertyManager->Tie("velocities/v-east-fps", this,
&FGPosition::GetVe);
PropertyManager->Tie("velocities/v-down-fps", this,
&FGPosition::GetVd);
PropertyManager->Tie("velocities/vg-fps", this,
&FGPosition::GetVground);
PropertyManager->Tie("flight-path/psi-gt-rad", this,
&FGPosition::GetGroundTrack);
PropertyManager->Tie("position/h-sl-ft", this,
&FGPosition::Geth,
&FGPosition::Seth,
true);
PropertyManager->Tie("velocities/h-dot-fps", this,
&FGPosition::Gethdot);
PropertyManager->Tie("position/lat-gc-rad", this,
&FGPosition::GetLatitude,
&FGPosition::SetLatitude);
PropertyManager->Tie("position/lat-dot-gc-rad", this,
&FGPosition::GetLatitudeDot);
PropertyManager->Tie("position/long-gc-rad", this,
&FGPosition::GetLongitude,
&FGPosition::SetLongitude,
true);
PropertyManager->Tie("position/long-dot-gc-rad", this,
&FGPosition::GetLongitudeDot);
PropertyManager->Tie("metrics/runway-radius", this,
&FGPosition::GetRunwayRadius,
&FGPosition::SetRunwayRadius);
PropertyManager->Tie("position/h-agl-ft", this,
&FGPosition::GetDistanceAGL,
&FGPosition::SetDistanceAGL);
PropertyManager->Tie("position/radius-to-vehicle-ft", this,
&FGPosition::GetRadius);
PropertyManager->Tie("flight-path/gamma-rad", this,
&FGPosition::GetGamma,
&FGPosition::SetGamma);
PropertyManager->Tie("aero/h_b-cg-ft", this,
&FGPosition::GetHOverBCG);
PropertyManager->Tie("aero/h_b-mac-ft", this,
&FGPosition::GetHOverBMAC);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPosition::unbind(void)
{
PropertyManager->Untie("velocities/v-north-fps");
PropertyManager->Untie("velocities/v-east-fps");
PropertyManager->Untie("velocities/v-down-fps");
PropertyManager->Untie("velocities/vg-fps");
PropertyManager->Untie("flight-path/psi-gt-rad");
PropertyManager->Untie("position/h-sl-ft");
PropertyManager->Untie("velocities/h-dot-fps");
PropertyManager->Untie("position/lat-gc-rad");
PropertyManager->Untie("position/lat-dot-gc-rad");
PropertyManager->Untie("position/long-gc-rad");
PropertyManager->Untie("position/long-dot-gc-rad");
PropertyManager->Untie("metrics/runway-radius");
PropertyManager->Untie("position/h-agl-ft");
PropertyManager->Untie("position/radius-to-vehicle-ft");
PropertyManager->Untie("flight-path/gamma-rad");
PropertyManager->Untie("aero/h_b-cg-ft");
PropertyManager->Untie("aero/h_b-mac-ft");
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// The bitmasked value choices are as follows:
// unset: In this case (the default) JSBSim would only print
// out the normally expected messages, essentially echoing
// the config files as they are read. If the environment
// variable is not set, debug_lvl is set to 1 internally
// 0: This requests JSBSim not to output any messages
// whatsoever.
// 1: This value explicity requests the normal JSBSim
// startup messages
// 2: This value asks for a message to be printed out when
// a class is instantiated
// 4: When this value is set, a message is displayed when a
// FGModel object executes its Run() method
// 8: When this value is set, various runtime state variables
// are printed out periodically
// 16: When set various parameters are sanity checked and
// a message is printed out when they go out of bounds
void FGPosition::Debug(int from)
{
if (debug_lvl <= 0) return;
if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) { // Constructor
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
if (from == 0) cout << "Instantiated: FGPosition" << endl;
if (from == 1) cout << "Destroyed: FGPosition" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 ) { // Runtime state variables
}
if (debug_lvl & 16) { // Sanity checking
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
cout << IdSrc << endl;
cout << IdHdr << endl;
}
}
}
}
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGPosition.cpp
Author: Jon S. Berndt
Date started: 01/05/99
Purpose: Integrate the EOM to determine instantaneous position
Called by: FGFDMExec
------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
This class encapsulates the integration of rates and accelerations to get the
current position of the aircraft.
HISTORY
--------------------------------------------------------------------------------
01/05/99 JSB Created
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
COMMENTS, REFERENCES, and NOTES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate
School, January 1994
[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
JSC 12960, July 1977
[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
NASA-Ames", NASA CR-2497, January 1975
[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
Wiley & Sons, 1979 ISBN 0-471-03032-5
[5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
1982 ISBN 0-471-08936-2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#ifdef FGFS
# include <simgear/compiler.h>
# ifdef SG_HAVE_STD_INCLUDES
# include <cmath>
# include <iomanip>
# else
# include <math.h>
# include <iomanip.h>
# endif
#else
# if defined(sgi) && !defined(__GNUC__)
# include <math.h>
# if (_COMPILER_VERSION < 740)
# include <iomanip.h>
# else
# include <iomanip>
# endif
# else
# include <cmath>
# include <iomanip>
# 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;
}
}
}
}

2
src/FDM/JSBSim/FGPosition.h
View file

@ -39,9 +39,7 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGModel.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS

3
src/FDM/JSBSim/FGPropeller.cpp
View file

@ -36,7 +36,10 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGPropeller.h"
#include "FGTranslation.h"
#include "FGRotation.h"
#include "FGFCS.h"
#include "FGAtmosphere.h"
namespace JSBSim {

2
src/FDM/JSBSim/FGPropeller.h
View file

@ -40,8 +40,6 @@ INCLUDES
#include "FGThruster.h"
#include "FGTable.h"
#include "FGTranslation.h"
#include "FGRotation.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS

83
src/FDM/JSBSim/FGPropertyManager.cpp
View file

@ -1,22 +1,22 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Header: FGPropertyManager.cpp
Author: Tony Peden
Based on work originally by David Megginson
Date: 2/2002
------------- Copyright (C) 2002 -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
@ -28,7 +28,6 @@
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include <simgear/props/props.hxx>
#include "FGPropertyManager.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -52,19 +51,19 @@ namespace JSBSim {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
string FGPropertyManager::mkPropertyName(string name, bool lowercase) {
/* do this two pass to avoid problems with characters getting skipped
because the index changed */
unsigned i;
for(i=0;i<name.length();i++) {
if( lowercase && isupper(name[i]) )
name[i]=tolower(name[i]);
else if( isspace(name[i]) )
else if( isspace(name[i]) )
name[i]='-';
}
for(i=0;i<name.length();i++) {
if( name[i] == '/' )
name.erase(i,1);
name.erase(i,1);
}
return name;
@ -72,23 +71,23 @@ string FGPropertyManager::mkPropertyName(string name, bool lowercase) {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGPropertyManager*
FGPropertyManager*
FGPropertyManager::GetNode (const string &path, bool create)
{
SGPropertyNode* node=this->getNode(path.c_str(), create);
if(node == 0)
cout << "FGPropertyManager::GetNode() No node found for "
if(node == 0)
cout << "FGPropertyManager::GetNode() No node found for "
<< path << endl;
return (FGPropertyManager*)node;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGPropertyManager*
FGPropertyManager*
FGPropertyManager::GetNode (const string &relpath, int index, bool create)
{
return (FGPropertyManager*)getNode(relpath.c_str(),index,create);
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -102,7 +101,7 @@ bool FGPropertyManager::HasNode (const string &path)
string FGPropertyManager::GetName( void ) {
return string( getName() );
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -113,23 +112,23 @@ string FGPropertyManager::GetFullyQualifiedName(void) {
bool atroot=false;
while( !atroot ) {
stack.push_back( tmpn->getDisplayName(true) );
if( !tmpn->getParent() )
if( !tmpn->getParent() )
atroot=true;
else
else
tmpn=tmpn->getParent();
}
string fqname="";
for(unsigned i=stack.size()-1;i>0;i--) {
fqname+= stack[i];
fqname+= "/";
}
fqname+= stack[0];
return fqname;
return fqname;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGPropertyManager::GetBool (const string &name, bool defaultValue)
@ -221,8 +220,8 @@ void FGPropertyManager::SetArchivable (const string &name, bool state )
SGPropertyNode * node = getNode(name.c_str());
if (node == 0)
cout <<
"Attempt to set archive flag for non-existant property "
<< name << endl;
"Attempt to set archive flag for non-existant property "
<< name << endl;
else
node->setAttribute(SGPropertyNode::ARCHIVE, state);
}
@ -234,8 +233,8 @@ void FGPropertyManager::SetReadable (const string &name, bool state )
SGPropertyNode * node = getNode(name.c_str());
if (node == 0)
cout <<
"Attempt to set read flag for non-existant property "
<< name << endl;
"Attempt to set read flag for non-existant property "
<< name << endl;
else
node->setAttribute(SGPropertyNode::READ, state);
}
@ -247,8 +246,8 @@ void FGPropertyManager::SetWritable (const string &name, bool state )
SGPropertyNode * node = getNode(name.c_str());
if (node == 0)
cout <<
"Attempt to set write flag for non-existant property "
<< name << endl;
"Attempt to set write flag for non-existant property "
<< name << endl;
else
node->setAttribute(SGPropertyNode::WRITE, state);
}
@ -266,53 +265,53 @@ void FGPropertyManager::Untie (const string &name)
void FGPropertyManager::Tie (const string &name, bool *pointer, bool useDefault)
{
if (!tie(name.c_str(), SGRawValuePointer<bool>(pointer),
useDefault))
useDefault))
cout <<
"Failed to tie property " << name << " to a pointer" << endl;
"Failed to tie property " << name << " to a pointer" << endl;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropertyManager::Tie (const string &name, int *pointer,
void FGPropertyManager::Tie (const string &name, int *pointer,
bool useDefault )
{
if (!tie(name.c_str(), SGRawValuePointer<int>(pointer),
useDefault))
useDefault))
cout <<
"Failed to tie property " << name << " to a pointer" << endl;
"Failed to tie property " << name << " to a pointer" << endl;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropertyManager::Tie (const string &name, long *pointer,
void FGPropertyManager::Tie (const string &name, long *pointer,
bool useDefault )
{
if (!tie(name.c_str(), SGRawValuePointer<long>(pointer),
useDefault))
useDefault))
cout <<
"Failed to tie property " << name << " to a pointer" << endl;
"Failed to tie property " << name << " to a pointer" << endl;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropertyManager::Tie (const string &name, float *pointer,
void FGPropertyManager::Tie (const string &name, float *pointer,
bool useDefault )
{
if (!tie(name.c_str(), SGRawValuePointer<float>(pointer),
useDefault))
useDefault))
cout <<
"Failed to tie property " << name << " to a pointer" << endl;
"Failed to tie property " << name << " to a pointer" << endl;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropertyManager::Tie (const string &name, double *pointer,
void FGPropertyManager::Tie (const string &name, double *pointer,
bool useDefault)
{
if (!tie(name.c_str(), SGRawValuePointer<double>(pointer),
useDefault))
useDefault))
cout <<
"Failed to tie property " << name << " to a pointer" << endl;
"Failed to tie property " << name << " to a pointer" << endl;
}
} // namespace JSBSim

66
src/FDM/JSBSim/FGPropertyManager.h
View file

@ -1,29 +1,29 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Header: FGPropertyManager.h
Author: Tony Peden
Based on work originally by David Megginson
Date: 2/2002
------------- Copyright (C) 2002 -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
@ -36,7 +36,11 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include <string>
#ifdef FGFS
#include <simgear/props/props.hxx>
#else
#include "simgear/props/props.hxx"
#endif
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
@ -59,7 +63,7 @@ CLASS DOCUMENTATION
/** Class wrapper for property handling.
@author David Megginson, Tony Peden
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
@ -69,17 +73,17 @@ class FGPropertyManager : public SGPropertyNode {
/// Constructor
FGPropertyManager(void) {}
/// Destructor
~FGPropertyManager(void) {}
~FGPropertyManager(void) {}
/** Property-ify a name
* replaces spaces with '-' and, optionally, makes name all lower case
* @param name string to change
* @param lowercase true to change all upper case chars to lower
* NOTE: this function changes its argument and thus relies
* on pass by value
*/
*/
string mkPropertyName(string name, bool lowercase);
/**
* Get a property node.
*
@ -87,10 +91,10 @@ class FGPropertyManager : public SGPropertyNode {
* @param create true to create the node if it doesn't exist.
* @return The node, or 0 if none exists and none was created.
*/
FGPropertyManager*
FGPropertyManager*
GetNode (const string &path, bool create = false);
FGPropertyManager*
FGPropertyManager*
GetNode (const string &relpath, int index, bool create = false);
/**
@ -105,7 +109,7 @@ class FGPropertyManager : public SGPropertyNode {
* Get the name of a node
*/
string GetName( void );
/**
* Get the fully qualified name of a node
* This function is very slow, so is probably useful for debugging only.
@ -455,25 +459,25 @@ class FGPropertyManager : public SGPropertyNode {
Tie (const string &name, double *pointer, bool useDefault = true);
//============================================================================
//
// All of the following functions *must* be inlined, otherwise linker
//
// All of the following functions *must* be inlined, otherwise linker
// errors will result
//
//============================================================================
/* template <class V> void
Tie (const string &name, V (*getter)(), void (*setter)(V) = 0,
bool useDefault = true);
template <class V> void
Tie (const string &name, int index, V (*getter)(int),
void (*setter)(int, V) = 0, bool useDefault = true);
template <class T, class V> void
Tie (const string &name, T * obj, V (T::*getter)() const,
void (T::*setter)(V) = 0, bool useDefault = true);
template <class T, class V> void
template <class T, class V> void
Tie (const string &name, T * obj, int index,
V (T::*getter)(int) const, void (T::*setter)(int, V) = 0,
bool useDefault = true); */
@ -490,12 +494,12 @@ class FGPropertyManager : public SGPropertyNode {
* @param name The property name to tie (full path).
* @param getter The getter function, or 0 if the value is unreadable.
* @param setter The setter function, or 0 if the value is unmodifiable.
* @param useDefault true if the setter should be invoked with any existing
* @param useDefault true if the setter should be invoked with any existing
* property value should be; false if the old value should be
* discarded; defaults to true.
*/
template <class V> inline void
template <class V> inline void
Tie (const string &name, V (*getter)(), void (*setter)(V) = 0,
bool useDefault = true)
{
@ -522,11 +526,11 @@ class FGPropertyManager : public SGPropertyNode {
* setter functions.
* @param getter The getter function, or 0 if the value is unreadable.
* @param setter The setter function, or 0 if the value is unmodifiable.
* @param useDefault true if the setter should be invoked with any existing
* @param useDefault true if the setter should be invoked with any existing
* property value should be; false if the old value should be
* discarded; defaults to true.
*/
template <class V> inline void Tie (const string &name,
template <class V> inline void Tie (const string &name,
int index, V (*getter)(int),
void (*setter)(int, V) = 0, bool useDefault = true)
{
@ -554,7 +558,7 @@ class FGPropertyManager : public SGPropertyNode {
* unreadable.
* @param setter The object's setter method, or 0 if the value is
* unmodifiable.
* @param useDefault true if the setter should be invoked with any existing
* @param useDefault true if the setter should be invoked with any existing
* property value should be; false if the old value should be
* discarded; defaults to true.
*/
@ -569,7 +573,7 @@ class FGPropertyManager : public SGPropertyNode {
"Failed to tie property " << name << " to object methods" << endl;
}
}
/**
* Tie a property to a pair of indexed object methods.
*
@ -585,11 +589,11 @@ class FGPropertyManager : public SGPropertyNode {
* setter methods.
* @param getter The getter method, or 0 if the value is unreadable.
* @param setter The setter method, or 0 if the value is unmodifiable.
* @param useDefault true if the setter should be invoked with any existing
* @param useDefault true if the setter should be invoked with any existing
* property value should be; false if the old value should be
* discarded; defaults to true.
*/
template <class T, class V> inline void
template <class T, class V> inline void
Tie (const string &name, T * obj, int index,
V (T::*getter)(int) const, void (T::*setter)(int, V) = 0,
bool useDefault = true)
@ -601,7 +605,7 @@ class FGPropertyManager : public SGPropertyNode {
"Failed to tie property " << name << " to indexed object methods" << endl;
}
}
};
};
}
#endif // FGPROPERTYMANAGER_H

86
src/FDM/JSBSim/FGPropulsion.cpp
View file

@ -53,6 +53,12 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGPropulsion.h"
#include "FGRocket.h"
#include "FGSimTurbine.h"
#include "FGTurbine.h"
#include "FGPropeller.h"
#include "FGNozzle.h"
#include "FGPiston.h"
#include "FGPropertyManager.h"
#if defined (__APPLE__)
@ -85,6 +91,7 @@ FGPropulsion::FGPropulsion(FGFDMExec* exec) : FGModel(exec)
numOxiTanks = numFuelTanks = 0;
dt = 0.0;
ActiveEngine = -1; // -1: ALL, 0: Engine 1, 1: Engine 2 ...
tankJ.InitMatrix();
bind();
@ -119,6 +126,7 @@ bool FGPropulsion::Run(void)
vForces += Thrusters[i]->GetBodyForces(); // sum body frame forces
vMoments += Thrusters[i]->GetMoments(); // sum body frame moments
}
return false;
} else {
return true;
@ -317,7 +325,7 @@ bool FGPropulsion::Load(FGConfigFile* AC_cfg)
Thrusters.push_back(new FGNozzle(FDMExec, &Thruster_cfg ));
} else if (thrType == "FG_DIRECT") {
Thrusters.push_back(new FGThruster( FDMExec, &Thruster_cfg) );
}
}
AC_cfg->GetNextConfigLine();
while ((token = AC_cfg->GetValue()) != string("/AC_THRUSTER")) {
@ -355,6 +363,7 @@ bool FGPropulsion::Load(FGConfigFile* AC_cfg)
AC_cfg->GetNextConfigLine();
}
CalculateTankInertias();
if (!ThrottleAdded) FCS->AddThrottle(); // need to have at least one throttle
return true;
@ -484,14 +493,14 @@ string FGPropulsion::GetPropulsionValues(void)
FGColumnVector3& FGPropulsion::GetTanksMoment(void)
{
iTank = Tanks.begin();
vXYZtank.InitMatrix();
vXYZtank_arm.InitMatrix();
while (iTank < Tanks.end()) {
vXYZtank(eX) += (*iTank)->GetX()*(*iTank)->GetContents();
vXYZtank(eY) += (*iTank)->GetY()*(*iTank)->GetContents();
vXYZtank(eZ) += (*iTank)->GetZ()*(*iTank)->GetContents();
vXYZtank_arm(eX) += (*iTank)->GetXYZ(eX)*(*iTank)->GetContents();
vXYZtank_arm(eY) += (*iTank)->GetXYZ(eY)*(*iTank)->GetContents();
vXYZtank_arm(eZ) += (*iTank)->GetXYZ(eZ)*(*iTank)->GetContents();
iTank++;
}
return vXYZtank;
return vXYZtank_arm;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -510,67 +519,20 @@ double FGPropulsion::GetTanksWeight(void)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGPropulsion::GetTanksIxx(const FGColumnVector3& vXYZcg)
FGMatrix33& FGPropulsion::CalculateTankInertias(void)
{
double I = 0.0;
iTank = Tanks.begin();
while (iTank < Tanks.end()) {
I += ((*iTank)->GetX() - vXYZcg(eX))*((*iTank)->GetX() - vXYZcg(eX)) * (*iTank)->GetContents()/(144.0*Inertial->gravity());
iTank++;
}
return I;
}
unsigned int size;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
size = Tanks.size();
if (size == 0) return tankJ;
double FGPropulsion::GetTanksIyy(const FGColumnVector3& vXYZcg)
{
double I = 0.0;
iTank = Tanks.begin();
while (iTank < Tanks.end()) {
I += ((*iTank)->GetY() - vXYZcg(eY))*((*iTank)->GetY() - vXYZcg(eY)) * (*iTank)->GetContents()/(144.0*Inertial->gravity());
iTank++;
}
return I;
}
tankJ = FGMatrix33();
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for (unsigned int i=0; i<size; i++)
tankJ += MassBalance->GetPointmassInertia( lbtoslug * Tanks[i]->GetContents(),
Tanks[i]->GetXYZ() );
double FGPropulsion::GetTanksIzz(const FGColumnVector3& vXYZcg)
{
double I = 0.0;
iTank = Tanks.begin();
while (iTank < Tanks.end()) {
I += ((*iTank)->GetZ() - vXYZcg(eZ))*((*iTank)->GetZ() - vXYZcg(eZ)) * (*iTank)->GetContents()/(144.0*Inertial->gravity());
iTank++;
}
return I;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGPropulsion::GetTanksIxz(const FGColumnVector3& vXYZcg)
{
double I = 0.0;
iTank = Tanks.begin();
while (iTank < Tanks.end()) {
I += ((*iTank)->GetX() - vXYZcg(eX))*((*iTank)->GetZ() - vXYZcg(eZ)) * (*iTank)->GetContents()/(144.0*Inertial->gravity());
iTank++;
}
return I;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGPropulsion::GetTanksIxy(const FGColumnVector3& vXYZcg)
{
double I = 0.0;
iTank = Tanks.begin();
while (iTank != Tanks.end()) {
I += ((*iTank)->GetX() - vXYZcg(eX))*((*iTank)->GetY() - vXYZcg(eY)) * (*iTank)->GetContents()/(144.0*Inertial->gravity());
iTank++;
}
return I;
return tankJ;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

45
src/FDM/JSBSim/FGPropulsion.h
View file

@ -3,30 +3,30 @@
Header: FGPropulsion.h
Author: Jon S. Berndt
Date started: 08/20/00
------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
HISTORY
--------------------------------------------------------------------------------
08/20/00 JSB Created
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
@ -53,14 +53,10 @@ INCLUDES
#endif
#include "FGModel.h"
#include "FGRocket.h"
#include "FGPiston.h"
#include "FGTurbine.h"
#include "FGSimTurbine.h"
#include "FGEngine.h"
#include "FGTank.h"
#include "FGPropeller.h"
#include "FGNozzle.h"
#include "FGThruster.h"
#include "FGMatrix33.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
@ -177,11 +173,11 @@ public:
/** Loops the engines/thrusters until thrust output steady (used for trimming) */
bool GetSteadyState(void);
/** starts the engines in IC mode (dt=0). All engine-specific setup must
be done before calling this (i.e. magnetos, starter engage, etc.) */
bool ICEngineStart(void);
string GetPropulsionStrings(void);
string GetPropulsionValues(void);
@ -189,16 +185,10 @@ public:
inline double GetForces(int n) const { return vForces(n);}
inline FGColumnVector3& GetMoments(void) {return vMoments;}
inline double GetMoments(int n) const {return vMoments(n);}
FGColumnVector3& GetTanksMoment(void);
double GetTanksWeight(void);
double GetTanksIxx(const FGColumnVector3& vXYZcg);
double GetTanksIyy(const FGColumnVector3& vXYZcg);
double GetTanksIzz(const FGColumnVector3& vXYZcg);
double GetTanksIxz(const FGColumnVector3& vXYZcg);
double GetTanksIxy(const FGColumnVector3& vXYZcg);
inline int GetActiveEngine(void) const
{
return ActiveEngine;
@ -210,10 +200,11 @@ public:
void SetStarter(int setting);
void SetCutoff(int setting=0);
void SetActiveEngine(int engine);
FGMatrix33& CalculateTankInertias(void);
void bind();
void unbind();
private:
vector <FGEngine*> Engines;
vector <FGTank*> Tanks;
@ -230,7 +221,9 @@ private:
double dt;
FGColumnVector3 vForces;
FGColumnVector3 vMoments;
FGColumnVector3 vXYZtank;
FGColumnVector3 vTankXYZ;
FGColumnVector3 vXYZtank_arm;
FGMatrix33 tankJ;
void Debug(int from);
};
}

24
src/FDM/JSBSim/FGRotation.cpp
View file

@ -59,13 +59,8 @@ INCLUDES
#include "FGAtmosphere.h"
#include "FGState.h"
#include "FGFDMExec.h"
#include "FGFCS.h"
#include "FGAircraft.h"
#include "FGMassBalance.h"
#include "FGTranslation.h"
#include "FGPosition.h"
#include "FGAuxiliary.h"
#include "FGOutput.h"
#include "FGPropertyManager.h"
@ -92,7 +87,7 @@ FGRotation::FGRotation(FGFDMExec* fdmex) : FGModel(fdmex)
vPQRdot_prev[3].InitMatrix();
bind();
Debug(0);
}
@ -108,22 +103,14 @@ FGRotation::~FGRotation()
bool FGRotation::Run(void)
{
double L2, N1;
double tTheta;
if (!FGModel::Run()) {
GetState();
L2 = vMoments(eL) + Ixz*vPQR(eP)*vPQR(eQ) - (Izz-Iyy)*vPQR(eR)*vPQR(eQ);
N1 = vMoments(eN) - (Iyy-Ixx)*vPQR(eP)*vPQR(eQ) - Ixz*vPQR(eR)*vPQR(eQ);
vPQRdot(eP) = (L2*Izz - N1*Ixz) / (Ixx*Izz - Ixz*Ixz);
vPQRdot(eQ) = (vMoments(eM) - (Ixx-Izz)*vPQR(eP)*vPQR(eR)
- Ixz*(vPQR(eP)*vPQR(eP) - vPQR(eR)*vPQR(eR)))/Iyy;
vPQRdot(eR) = (N1*Ixx + L2*Ixz) / (Ixx*Izz - Ixz*Ixz);
vPQRdot = MassBalance->GetJinv()*(vMoments - vPQR*(MassBalance->GetJ()*vPQR));
vPQR += State->Integrate(FGState::TRAPZ, dt*rate, vPQRdot, vPQRdot_prev);
vAeroPQR = vPQR + Atmosphere->GetTurbPQR();
State->IntegrateQuat(vPQR, rate);
@ -155,11 +142,6 @@ void FGRotation::GetState(void)
{
dt = State->Getdt();
vMoments = Aircraft->GetMoments();
Ixx = MassBalance->GetIxx();
Iyy = MassBalance->GetIyy();
Izz = MassBalance->GetIzz();
Ixz = MassBalance->GetIxz();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

13
src/FDM/JSBSim/FGRotation.h
View file

@ -50,9 +50,7 @@ INCLUDES
#endif
#include "FGModel.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
@ -117,11 +115,11 @@ public:
vAeroPQR(eQ)=q;
vAeroPQR(eR)=r;}
inline void SetEuler(FGColumnVector3 tt) {vEuler = tt;}
inline double Getphi(void) const {return vEuler(1);}
inline double Gettht(void) const {return vEuler(2);}
inline double Getpsi(void) const {return vEuler(3);}
inline double GetCosphi(void) const {return cPhi;}
inline double GetCostht(void) const {return cTht;}
inline double GetCospsi(void) const {return cPsi;}
@ -129,7 +127,7 @@ public:
inline double GetSinphi(void) const {return sPhi;}
inline double GetSintht(void) const {return sTht;}
inline double GetSinpsi(void) const {return sPsi;}
void bind(void);
void unbind(void);
@ -141,12 +139,11 @@ private:
FGColumnVector3 vMoments;
FGColumnVector3 vEuler;
FGColumnVector3 vEulerRates;
double cTht,sTht;
double cPhi,sPhi;
double cPsi,sPsi;
double Ixx, Iyy, Izz, Ixz;
double dt;
void GetState(void);

1
src/FDM/JSBSim/FGSimTurbine.cpp
View file

@ -303,6 +303,7 @@ double FGSimTurbine::Seek(double *var, double target, double accel, double decel
void FGSimTurbine::SetDefaults(void)
{
Name = "Not defined";
N1 = N2 = 0.0;
Type = etSimTurbine;
MilThrust = 10000.0;
MaxThrust = 10000.0;

68
src/FDM/JSBSim/FGState.cpp
View file

@ -1,37 +1,37 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGState.cpp
Author: Jon Berndt
Date started: 11/17/98
Called by: FGFDMExec and accessed by all models.
------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
See header file.
HISTORY
--------------------------------------------------------------------------------
11/17/98 JSB Created
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
@ -70,7 +70,6 @@ FGState::FGState(FGFDMExec* fdex)
{
FDMExec = fdex;
a = 1000.0;
sim_time = 0.0;
dt = 1.0/120.0;
@ -89,7 +88,7 @@ FGState::FGState(FGFDMExec* fdex)
for(int i=0;i<4;i++) vQdot_prev[i].InitMatrix();
bind();
Debug(0);
}
@ -114,25 +113,26 @@ void FGState::Initialize(double U, double V, double W,
double alpha, beta;
double qbar, Vt;
FGColumnVector3 vAeroUVW;
FGColumnVector3 vUVW;
Position->SetLatitude(Latitude);
Position->SetLongitude(Longitude);
Position->Seth(H);
Atmosphere->Run();
vLocalEuler << phi << tht << psi;
Rotation->SetEuler(vLocalEuler);
InitMatrices(phi, tht, psi);
vUVW << U << V << W;
Translation->SetUVW(vUVW);
Atmosphere->SetWindNED(wnorth, weast, wdown);
vAeroUVW = vUVW + mTl2b*Atmosphere->GetWindNED();
if (vAeroUVW(eW) != 0.0)
alpha = vAeroUVW(eU)*vAeroUVW(eU) > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0;
else
@ -164,7 +164,7 @@ void FGState::Initialize(FGInitialCondition *FGIC)
double U, V, W, h;
double latitude, longitude;
double wnorth,weast, wdown;
latitude = FGIC->GetLatitudeRadIC();
longitude = FGIC->GetLongitudeRadIC();
h = FGIC->GetAltitudeFtIC();
@ -177,12 +177,12 @@ void FGState::Initialize(FGInitialCondition *FGIC)
wnorth = FGIC->GetWindNFpsIC();
weast = FGIC->GetWindEFpsIC();
wdown = FGIC->GetWindDFpsIC();
Position->SetSeaLevelRadius( FGIC->GetSeaLevelRadiusFtIC() );
Position->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() +
Position->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() +
FGIC->GetTerrainAltitudeFtIC() );
// need to fix the wind speed args, here.
// need to fix the wind speed args, here.
Initialize(U, V, W, phi, tht, psi, latitude, longitude, h, wnorth, weast, wdown);
}
@ -319,10 +319,10 @@ FGMatrix33& FGState::GetTb2s(void)
{
float alpha,beta;
float ca, cb, sa, sb;
alpha = Translation->Getalpha();
beta = Translation->Getbeta();
ca = cos(alpha);
sa = sin(alpha);
cb = cos(beta);
@ -347,14 +347,14 @@ void FGState::ReportState(void)
{
#if !defined(__BORLANDCPP__)
char out[80], flap[10], gear[12];
cout << endl << " JSBSim State" << endl;
snprintf(out,80," Weight: %7.0f lbs. CG: %5.1f, %5.1f, %5.1f inches\n",
FDMExec->GetMassBalance()->GetWeight(),
FDMExec->GetMassBalance()->GetXYZcg(1),
FDMExec->GetMassBalance()->GetXYZcg(2),
FDMExec->GetMassBalance()->GetXYZcg(3));
cout << out;
cout << out;
if ( FCS->GetDfPos() <= 0.01)
snprintf(flap,10,"Up");
else
@ -384,7 +384,7 @@ void FGState::ReportState(void)
snprintf(out,80, " Flight Path Angle: %6.2f deg Climb Rate: %5.0f ft/min\n",
Position->GetGamma()*radtodeg,
Position->Gethdot()*60 );
cout << out;
cout << out;
snprintf(out,80, " Normal Load Factor: %4.2f g's Pitch Rate: %5.2f deg/s\n",
Aircraft->GetNlf(),
Rotation->GetPQR(2)*radtodeg );
@ -392,32 +392,32 @@ void FGState::ReportState(void)
snprintf(out,80, " Heading: %3.0f deg true Sideslip: %5.2f deg Yaw Rate: %5.2f deg/s\n",
Rotation->Getpsi()*radtodeg,
Translation->Getbeta()*radtodeg,
Rotation->GetPQR(3)*radtodeg );
Rotation->GetPQR(3)*radtodeg );
cout << out;
snprintf(out,80, " Bank Angle: %5.2f deg Roll Rate: %5.2f deg/s\n",
Rotation->Getphi()*radtodeg,
Rotation->Getphi()*radtodeg,
Rotation->GetPQR(1)*radtodeg );
cout << out;
snprintf(out,80, " Elevator: %5.2f deg Left Aileron: %5.2f deg Rudder: %5.2f deg\n",
FCS->GetDePos(ofRad)*radtodeg,
FCS->GetDaLPos(ofRad)*radtodeg,
FCS->GetDrPos(ofRad)*radtodeg );
cout << out;
cout << out;
snprintf(out,80, " Throttle: %5.2f%c\n",
FCS->GetThrottlePos(0)*100,'%' );
cout << out;
snprintf(out,80, " Wind Components: %5.2f kts head wind, %5.2f kts cross wind\n",
FDMExec->GetAuxiliary()->GetHeadWind()*fpstokts,
FDMExec->GetAuxiliary()->GetCrossWind()*fpstokts );
cout << out;
cout << out;
snprintf(out,80, " Ground Speed: %4.0f knots , Ground Track: %3.0f deg true\n",
Position->GetVground()*fpstokts,
Position->GetGroundTrack()*radtodeg );
cout << out;
cout << out;
#endif
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -428,7 +428,7 @@ void FGState::bind(void)
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGState::unbind(void)
{
PropertyManager->Untie("sim-time-sec");

10
src/FDM/JSBSim/FGState.h
View file

@ -143,9 +143,6 @@ public:
*/
void Initialize(FGInitialCondition *FGIC);
/// returns the speed of sound in feet per second.
inline double Geta(void) { return a; }
/// Returns the simulation time in seconds.
inline double Getsim_time(void) const { return sim_time; }
/// Returns the simulation delta T.
@ -156,11 +153,6 @@ public:
/// Resumes the simulation by resetting delta T to the correct value.
inline void Resume(void) {dt = saved_dt;}
/** Sets the speed of sound.
@param speed the speed of sound in feet per second.
*/
inline void Seta(double speed) { a = speed; }
/** Sets the current sim time.
@param cur_time the current time
@return the current time.
@ -335,7 +327,6 @@ public:
void unbind();
private:
double a; // speed of sound
double sim_time, dt;
double saved_dt;
@ -347,7 +338,6 @@ private:
FGColumnVector4 vQtrn;
FGColumnVector4 vQdot_prev[4];
FGColumnVector4 vQdot;
FGColumnVector3 vUVW;
FGColumnVector3 vLocalVelNED;
FGColumnVector3 vLocalEuler;

13
src/FDM/JSBSim/FGTank.cpp
View file

@ -56,13 +56,14 @@ CLASS IMPLEMENTATION
FGTank::FGTank(FGConfigFile* AC_cfg)
{
string token;
double X, Y, Z;
type = AC_cfg->GetValue("TYPE");
if (type == "FUEL") Type = ttFUEL;
else if (type == "OXIDIZER") Type = ttOXIDIZER;
else Type = ttUNKNOWN;
AC_cfg->GetNextConfigLine();
while ((token = AC_cfg->GetValue()) != string("/AC_TANK")) {
if (token == "XLOC") *AC_cfg >> X;
@ -73,7 +74,9 @@ FGTank::FGTank(FGConfigFile* AC_cfg)
else if (token == "CONTENTS") *AC_cfg >> Contents;
else cerr << "Unknown identifier: " << token << " in tank definition." << endl;
}
vXYZ << X << Y << Z;
Selected = true;
if (Capacity != 0) {
@ -81,7 +84,7 @@ FGTank::FGTank(FGConfigFile* AC_cfg)
} else {
Contents = 0;
PctFull = 0;
}
}
Debug(0);
}
@ -137,7 +140,7 @@ void FGTank::Debug(int from)
if (from == 0) { // Constructor
cout << " " << type << " tank holds " << Capacity << " lbs. " << type << endl;
cout << " currently at " << PctFull << "% of maximum capacity" << endl;
cout << " Tank location (X, Y, Z): " << X << ", " << Y << ", " << Z << endl;
cout << " Tank location (X, Y, Z): " << vXYZ(eX) << ", " << vXYZ(eY) << ", " << vXYZ(eZ) << endl;
cout << " Effective radius: " << Radius << " inches" << endl;
}
}

10
src/FDM/JSBSim/FGTank.h
View file

@ -44,8 +44,9 @@ SENTRY
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGConfigFile.h"
#include "FGJSBBase.h"
#include "FGConfigFile.h"
#include "FGColumnVector3.h"
#ifdef FGFS
# include <simgear/compiler.h>
@ -98,9 +99,8 @@ public:
bool GetSelected(void) {return Selected;}
double GetPctFull(void) {return PctFull;}
double GetContents(void) {return Contents;}
double inline GetX(void) {return X;}
double inline GetY(void) {return Y;}
double inline GetZ(void) {return Z;}
const FGColumnVector3& GetXYZ(void) {return vXYZ;}
double GetXYZ(int idx) {return vXYZ(idx);}
void SetContents(double contents) { Contents = contents; }
@ -109,7 +109,7 @@ public:
private:
TankType Type;
string type;
double X, Y, Z;
FGColumnVector3 vXYZ;
double Capacity;
double Radius;
double PctFull;

21
src/FDM/JSBSim/FGTranslation.cpp
View file

@ -115,17 +115,7 @@ bool FGTranslation::Run(void)
{
if (!FGModel::Run()) {
mVel(1,1) = 0.0;
mVel(1,2) = -vUVW(eW);
mVel(1,3) = vUVW(eV);
mVel(2,1) = vUVW(eW);
mVel(2,2) = 0.0;
mVel(2,3) = -vUVW(eU);
mVel(3,1) = -vUVW(eV);
mVel(3,2) = vUVW(eU);
mVel(3,3) = 0.0;
vUVWdot = mVel*Rotation->GetPQR() + Aircraft->GetBodyAccel();
vUVWdot = vUVW*Rotation->GetPQR() + Aircraft->GetBodyAccel();
vUVW += State->Integrate(FGState::TRAPZ, State->Getdt()*rate, vUVWdot, vUVWdot_prev);
@ -139,7 +129,6 @@ bool FGTranslation::Run(void)
beta = vAeroUVW(eU)*vAeroUVW(eU)+vAeroUVW(eW)*vAeroUVW(eW) > 0.0 ? atan2(vAeroUVW(eV),
sqrt(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW))) : 0.0;
// stolen, quite shamelessly, from LaRCsim
double mUW = (vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW));
double signU=1;
if (vAeroUVW(eU) != 0.0)
@ -160,10 +149,10 @@ bool FGTranslation::Run(void)
qbar = 0.5*Atmosphere->GetDensity()*Vt*Vt;
qbarUW = 0.5*Atmosphere->GetDensity()*(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW));
qbarUV = 0.5*Atmosphere->GetDensity()*(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eV)*vAeroUVW(eV));
Mach = Vt / State->Geta();
vMachUVW(eU) = vAeroUVW(eU) / State->Geta();
vMachUVW(eV) = vAeroUVW(eV) / State->Geta();
vMachUVW(eW) = vAeroUVW(eW) / State->Geta();
Mach = Vt / Atmosphere->GetSoundSpeed();
vMachUVW(eU) = vAeroUVW(eU) / Atmosphere->GetSoundSpeed();
vMachUVW(eV) = vAeroUVW(eV) / Atmosphere->GetSoundSpeed();
vMachUVW(eW) = vAeroUVW(eW) / Atmosphere->GetSoundSpeed();
if (debug_lvl > 1) Debug(1);

4
src/FDM/JSBSim/FGTranslation.h
View file

@ -54,9 +54,7 @@ INCLUDES
#endif
#include "FGModel.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
@ -142,13 +140,11 @@ private:
FGColumnVector3 vUVW;
FGColumnVector3 vUVWdot;
FGColumnVector3 vUVWdot_prev[4];
FGMatrix33 mVel;
FGColumnVector3 vAeroUVW;
FGColumnVector3 vMachUVW;
double Vt, Mach;
double qbar, qbarUW, qbarUV;
double dt;
double alpha, beta;
double adot,bdot;
void Debug(int from);

5
src/FDM/JSBSim/FGTrim.cpp
View file

@ -52,8 +52,11 @@ INCLUDES
#include "FGTrim.h"
#include "FGAircraft.h"
#include "FGMassBalance.h"
#include "FGGroundReactions.h"
#include "FGInertial.h"
#include "FGAerodynamics.h"
#include "FGColumnVector3.h"
#if _MSC_VER
#pragma warning (disable : 4786 4788)
#endif
@ -548,7 +551,6 @@ bool FGTrim::checkLimits(void) {
void FGTrim::setupPullup() {
float g,q,cgamma;
FGColumnVector3 vPQR;
g=fdmex->GetInertial()->gravity();
cgamma=cos(fgic->GetFlightPathAngleRadIC());
cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
@ -594,7 +596,6 @@ void FGTrim::updateRates(void){
fdmex->GetRotation()->SetPQR(p,q,r);
} else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
float g,q,cgamma;
FGColumnVector3 vPQR;
g=fdmex->GetInertial()->gravity();
cgamma=cos(fgic->GetFlightPathAngleRadIC());
q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();

133
src/FDM/JSBSim/JSBSim.cxx
View file

@ -62,6 +62,11 @@
#include <FDM/JSBSim/FGLGear.h>
#include <FDM/JSBSim/FGPropertyManager.h>
#include <FDM/JSBSim/FGEngine.h>
#include <FDM/JSBSim/FGPiston.h>
#include <FDM/JSBSim/FGSimTurbine.h>
#include <FDM/JSBSim/FGRocket.h>
#include <FDM/JSBSim/FGNozzle.h>
#include <FDM/JSBSim/FGPropeller.h>
#include <FDM/JSBSim/FGRotor.h>
#include "JSBSim.hxx"
@ -74,11 +79,11 @@ FMAX (double a, double b)
/******************************************************************************/
FGJSBsim::FGJSBsim( double dt )
FGJSBsim::FGJSBsim( double dt )
: FGInterface(dt)
{
bool result;
// Set up the debugging level
// FIXME: this will not respond to
// runtime changes
@ -105,7 +110,7 @@ FGJSBsim::FGJSBsim( double dt )
}
fdmex = new FGFDMExec( (FGPropertyManager*)globals->get_props() );
State = fdmex->GetState();
Atmosphere = fdmex->GetAtmosphere();
FCS = fdmex->GetFCS();
@ -117,11 +122,11 @@ FGJSBsim::FGJSBsim( double dt )
Position = fdmex->GetPosition();
Auxiliary = fdmex->GetAuxiliary();
Aerodynamics = fdmex->GetAerodynamics();
GroundReactions = fdmex->GetGroundReactions();
GroundReactions = fdmex->GetGroundReactions();
fgic=fdmex->GetIC();
needTrim=true;
SGPath aircraft_path( globals->get_fg_root() );
aircraft_path.append( "Aircraft" );
@ -132,7 +137,7 @@ FGJSBsim::FGJSBsim( double dt )
result = fdmex->LoadModel( aircraft_path.str(),
engine_path.str(),
fgGetString("/sim/aero") );
if (result) {
SG_LOG( SG_FLIGHT, SG_INFO, " loaded aero.");
} else {
@ -147,7 +152,7 @@ FGJSBsim::FGJSBsim( double dt )
int Neng = Propulsion->GetNumEngines();
SG_LOG( SG_FLIGHT, SG_INFO, "num engines = " << Neng );
if ( GroundReactions->GetNumGearUnits() <= 0 ) {
SG_LOG( SG_FLIGHT, SG_ALERT, "num gear units = "
<< GroundReactions->GetNumGearUnits() );
@ -169,7 +174,7 @@ FGJSBsim::FGJSBsim( double dt )
node->setDoubleValue("level-gal_us", Propulsion->GetTank(i)->GetContents() / 6.6);
}
}
fgSetDouble("/fdm/trim/pitch-trim", FCS->GetPitchTrimCmd());
fgSetDouble("/fdm/trim/throttle", FCS->GetThrottleCmd(0));
fgSetDouble("/fdm/trim/aileron", FCS->GetDaCmd());
@ -184,10 +189,10 @@ FGJSBsim::FGJSBsim( double dt )
throttle_trim = fgGetNode("/fdm/trim/throttle", true );
aileron_trim = fgGetNode("/fdm/trim/aileron", true );
rudder_trim = fgGetNode("/fdm/trim/rudder", true );
stall_warning = fgGetNode("/sim/alarms/stall-warning",true);
stall_warning->setDoubleValue(0);
flap_pos_pct=fgGetNode("/surface-positions/flap-pos-norm",true);
elevator_pos_pct=fgGetNode("/surface-positions/elevator-pos-norm",true);
@ -199,7 +204,7 @@ FGJSBsim::FGJSBsim( double dt )
speedbrake_pos_pct
=fgGetNode("/surface-positions/speedbrake-pos-norm",true);
spoilers_pos_pct=fgGetNode("/surface-positions/spoilers-pos-norm",true);
elevator_pos_pct->setDoubleValue(0);
left_aileron_pos_pct->setDoubleValue(0);
right_aileron_pos_pct->setDoubleValue(0);
@ -213,7 +218,7 @@ FGJSBsim::FGJSBsim( double dt )
density = fgGetNode("/environment/density-slugft3",true);
turbulence_gain = fgGetNode("/environment/turbulence/magnitude-norm",true);
turbulence_rate = fgGetNode("/environment/turbulence/rate-hz",true);
wind_from_north= fgGetNode("/environment/wind-from-north-fps",true);
wind_from_east = fgGetNode("/environment/wind-from-east-fps" ,true);
wind_from_down = fgGetNode("/environment/wind-from-down-fps" ,true);
@ -239,7 +244,7 @@ FGJSBsim::~FGJSBsim(void)
void FGJSBsim::init()
{
double tmp;
SG_LOG( SG_FLIGHT, SG_INFO, "Starting and initializing JSBsim" );
// Explicitly call the superclass's
@ -265,7 +270,7 @@ void FGJSBsim::init()
Atmosphere->UseInternal();
}
#endif
fgic->SetVnorthFpsIC( wind_from_north->getDoubleValue() );
fgic->SetVeastFpsIC( wind_from_east->getDoubleValue() );
fgic->SetVdownFpsIC( wind_from_down->getDoubleValue() );
@ -274,7 +279,7 @@ void FGJSBsim::init()
//Atmosphere->SetExPressure(get_Static_pressure());
//Atmosphere->SetExDensity(get_Density());
SG_LOG(SG_FLIGHT,SG_INFO,"T,p,rho: " << fdmex->GetAtmosphere()->GetTemperature()
<< ", " << fdmex->GetAtmosphere()->GetPressure()
<< ", " << fdmex->GetAtmosphere()->GetPressure()
<< ", " << fdmex->GetAtmosphere()->GetDensity() );
common_init();
@ -308,9 +313,9 @@ void FGJSBsim::init()
<< Auxiliary->GetVcalibratedKTS() << " knots" );
break;
}
stall_warning->setDoubleValue(0);
SG_LOG( SG_FLIGHT, SG_INFO, " Bank Angle: "
<< Rotation->Getphi()*RADTODEG << " deg" );
SG_LOG( SG_FLIGHT, SG_INFO, " Pitch Angle: "
@ -328,8 +333,8 @@ void FGJSBsim::init()
SG_LOG( SG_FLIGHT, SG_INFO, " set dt" );
SG_LOG( SG_FLIGHT, SG_INFO, "Finished initializing JSBSim" );
SG_LOG( SG_FLIGHT, SG_INFO, "FGControls::get_gear_down()= " <<
SG_LOG( SG_FLIGHT, SG_INFO, "FGControls::get_gear_down()= " <<
globals->get_controls()->get_gear_down() );
}
@ -351,20 +356,20 @@ void FGJSBsim::update( double dt )
copy_to_JSBsim();
trimmed->setBoolValue(false);
if ( needTrim ) {
if ( startup_trim->getBoolValue() ) {
SG_LOG(SG_FLIGHT, SG_INFO,
"Ready to trim, terrain altitude is: "
"Ready to trim, terrain altitude is: "
<< cur_fdm_state->get_Runway_altitude() * SG_METER_TO_FEET );
fgic->SetTerrainAltitudeFtIC( cur_fdm_state->get_ground_elev_ft() );
do_trim();
} else {
fdmex->RunIC(); //apply any changes made through the set_ functions
}
needTrim = false;
}
needTrim = false;
}
for ( i=0; i < multiloop; i++ ) {
fdmex->Run();
}
@ -414,8 +419,8 @@ bool FGJSBsim::copy_to_JSBsim()
FCS->SetDrCmd( -globals->get_controls()->get_rudder() );
FCS->SetYawTrimCmd( -globals->get_controls()->get_rudder_trim() );
FCS->SetDfCmd( globals->get_controls()->get_flaps() );
FCS->SetDsbCmd( globals->get_controls()->get_speedbrake() );
FCS->SetDspCmd( globals->get_controls()->get_spoilers() );
FCS->SetDsbCmd( globals->get_controls()->get_speedbrake() );
FCS->SetDspCmd( globals->get_controls()->get_spoilers() );
// Parking brake sets minimum braking
// level for mains.
@ -467,7 +472,7 @@ bool FGJSBsim::copy_to_JSBsim()
_set_Runway_altitude( cur_fdm_state->get_Runway_altitude() );
Position->SetSeaLevelRadius( get_Sea_level_radius() );
Position->SetRunwayRadius( get_Runway_altitude()
Position->SetRunwayRadius( get_Runway_altitude()
+ get_Sea_level_radius() );
Atmosphere->SetExTemperature(
@ -506,13 +511,13 @@ bool FGJSBsim::copy_to_JSBsim()
bool FGJSBsim::copy_from_JSBsim()
{
unsigned int i, j;
/*
_set_Inertias( MassBalance->GetMass(),
MassBalance->GetIxx(),
MassBalance->GetIyy(),
MassBalance->GetIzz(),
MassBalance->GetIxz() );
*/
_set_CG_Position( MassBalance->GetXYZcg(1),
MassBalance->GetXYZcg(2),
MassBalance->GetXYZcg(3) );
@ -524,7 +529,7 @@ bool FGJSBsim::copy_from_JSBsim()
_set_Accels_CG_Body_N ( Aircraft->GetNcg(1),
Aircraft->GetNcg(2),
Aircraft->GetNcg(3) );
_set_Accels_Pilot_Body( Auxiliary->GetPilotAccel(1),
Auxiliary->GetPilotAccel(2),
Auxiliary->GetPilotAccel(3) );
@ -541,6 +546,11 @@ bool FGJSBsim::copy_from_JSBsim()
Translation->GetUVW(2),
Translation->GetUVW(3) );
// Make the HUD work ...
_set_Velocities_Ground( Position->GetVn(),
Position->GetVe(),
-Position->GetVd() );
_set_V_rel_wind( Translation->GetVt() );
_set_V_equiv_kts( Auxiliary->GetVequivalentKTS() );
@ -633,6 +643,7 @@ bool FGJSBsim::copy_from_JSBsim()
node->setBoolValue("ignition", eng->GetIgnition());
node->setDoubleValue("nozzle-pos-norm", eng->GetNozzle());
node->setDoubleValue("inlet-pos-norm", eng->GetInlet());
node->setDoubleValue("oil-pressure-psi", eng->getOilPressure_psi());
node->setBoolValue("reversed", eng->GetReversed());
node->setBoolValue("cutoff", eng->GetCutoff());
globals->get_controls()->set_reverser(i, eng->GetReversed() );
@ -665,7 +676,7 @@ bool FGJSBsim::copy_from_JSBsim()
FGPropeller* prop = (FGPropeller*)thruster;
tnode->setDoubleValue("rpm", thruster->GetRPM());
tnode->setDoubleValue("pitch", prop->GetPitch());
tnode->setDoubleValue("torque", prop->GetTorque());
tnode->setDoubleValue("torque", prop->GetTorque());
} // end FGPropeller code block
break;
case FGThruster::ttRotor:
@ -691,22 +702,22 @@ bool FGJSBsim::copy_from_JSBsim()
double contents = Propulsion->GetTank(i)->GetContents();
node->setDoubleValue("level-gal_us", contents/6.6);
node->setDoubleValue("level-lb", contents);
// node->setDoubleValue("temperature_degC",
// node->setDoubleValue("temperature_degC",
}
}
update_gear();
stall_warning->setDoubleValue( Aerodynamics->GetStallWarn() );
elevator_pos_pct->setDoubleValue( FCS->GetDePos(ofNorm) );
left_aileron_pos_pct->setDoubleValue( FCS->GetDaLPos(ofNorm) );
right_aileron_pos_pct->setDoubleValue( -1*FCS->GetDaLPos(ofNorm) );
right_aileron_pos_pct->setDoubleValue( FCS->GetDaRPos(ofNorm) );
rudder_pos_pct->setDoubleValue( -1*FCS->GetDrPos(ofNorm) );
flap_pos_pct->setDoubleValue( FCS->GetDfPos(ofNorm) );
speedbrake_pos_pct->setDoubleValue( FCS->GetDsbPos(ofNorm) );
spoilers_pos_pct->setDoubleValue( FCS->GetDspPos(ofNorm) );
return true;
}
@ -738,21 +749,21 @@ void FGJSBsim::set_Latitude(double lat)
// In case we're not trimming
FGInterface::set_Latitude(lat);
if ( altitude->getDoubleValue() > -9990 ) {
alt = altitude->getDoubleValue();
} else {
alt = 0.0;
}
update_ic();
SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Latitude: " << lat );
SG_LOG(SG_FLIGHT,SG_INFO," cur alt (ft) = " << alt );
sgGeodToGeoc( lat, alt * SG_FEET_TO_METER,
sgGeodToGeoc( lat, alt * SG_FEET_TO_METER,
&sea_level_radius_meters, &lat_geoc );
_set_Sea_level_radius( sea_level_radius_meters * SG_METER_TO_FEET );
fgic->SetSeaLevelRadiusFtIC( sea_level_radius_meters * SG_METER_TO_FEET );
fgic->SetSeaLevelRadiusFtIC( sea_level_radius_meters * SG_METER_TO_FEET );
_set_Runway_altitude( cur_fdm_state->get_Runway_altitude() );
fgic->SetTerrainAltitudeFtIC( cur_fdm_state->get_ground_elev_ft() );
fgic->SetLatitudeRadIC( lat_geoc );
@ -782,7 +793,7 @@ void FGJSBsim::set_Altitude(double alt)
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Altitude: " << alt );
SG_LOG(SG_FLIGHT,SG_INFO, " lat (deg) = " << latitude->getDoubleValue() );
// In case we're not trimming
FGInterface::set_Altitude(alt);
@ -815,7 +826,7 @@ void FGJSBsim::set_V_calibrated_kts(double vc)
void FGJSBsim::set_Mach_number(double mach)
{
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Mach_number: " << mach );
// In case we're not trimming
FGInterface::set_Mach_number(mach);
@ -828,7 +839,7 @@ void FGJSBsim::set_Velocities_Local( double north, double east, double down )
{
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local: "
<< north << ", " << east << ", " << down );
// In case we're not trimming
FGInterface::set_Velocities_Local(north, east, down);
@ -843,7 +854,7 @@ void FGJSBsim::set_Velocities_Wind_Body( double u, double v, double w)
{
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Wind_Body: "
<< u << ", " << v << ", " << w );
// In case we're not trimming
FGInterface::set_Velocities_Wind_Body(u, v, w);
@ -859,7 +870,7 @@ void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi )
{
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Euler_Angles: "
<< phi << ", " << theta << ", " << psi );
// In case we're not trimming
FGInterface::set_Euler_Angles(phi, theta, psi);
@ -874,7 +885,7 @@ void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi )
void FGJSBsim::set_Climb_Rate( double roc)
{
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Climb_Rate: " << roc );
// In case we're not trimming
FGInterface::set_Climb_Rate(roc);
@ -884,18 +895,18 @@ void FGJSBsim::set_Climb_Rate( double roc)
//out the other.
if( !(fabs(roc) > 1 && fabs(fgic->GetFlightPathAngleRadIC()) < 0.01) ) {
fgic->SetClimbRateFpsIC(roc);
}
}
needTrim=true;
}
void FGJSBsim::set_Gamma_vert_rad( double gamma)
{
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Gamma_vert_rad: " << gamma );
update_ic();
if( !(fabs(gamma) < 0.01 && fabs(fgic->GetClimbRateFpsIC()) > 1) ) {
fgic->SetFlightPathAngleRadIC(gamma);
}
}
needTrim=true;
}
@ -915,7 +926,7 @@ void FGJSBsim::init_gear(void )
node->setBoolValue("has-brake", gr->GetGearUnit(i)->GetBrakeGroup() > 0);
node->setDoubleValue("position-norm", FCS->GetGearPos());
node->setDoubleValue("tire-pressure-norm", gr->GetGearUnit(i)->GetTirePressure());
}
}
}
void FGJSBsim::update_gear(void)
@ -927,7 +938,7 @@ void FGJSBsim::update_gear(void)
node->getChild("wow", 0, true)->setBoolValue(gr->GetGearUnit(i)->GetWOW());
node->getChild("position-norm", 0, true)->setDoubleValue(FCS->GetGearPos());
gr->GetGearUnit(i)->SetTirePressure(node->getDoubleValue("tire-pressure-norm"));
}
}
}
void FGJSBsim::do_trim(void)
@ -967,19 +978,19 @@ void FGJSBsim::do_trim(void)
globals->get_controls()->set_rudder( FCS->GetDrCmd());
SG_LOG( SG_FLIGHT, SG_INFO, " Trim complete" );
}
}
void FGJSBsim::update_ic(void)
{
if ( !needTrim ) {
fgic->SetLatitudeRadIC(get_Lat_geocentric() );
fgic->SetLongitudeRadIC( get_Longitude() );
fgic->SetAltitudeFtIC( get_Altitude() );
fgic->SetVcalibratedKtsIC( get_V_calibrated_kts() );
fgic->SetPitchAngleRadIC( get_Theta() );
fgic->SetRollAngleRadIC( get_Phi() );
fgic->SetTrueHeadingRadIC( get_Psi() );
fgic->SetLatitudeRadIC(get_Lat_geocentric() );
fgic->SetLongitudeRadIC( get_Longitude() );
fgic->SetAltitudeFtIC( get_Altitude() );
fgic->SetVcalibratedKtsIC( get_V_calibrated_kts() );
fgic->SetPitchAngleRadIC( get_Theta() );
fgic->SetRollAngleRadIC( get_Phi() );
fgic->SetTrueHeadingRadIC( get_Psi() );
fgic->SetClimbRateFpsIC( get_Climb_Rate() );
}
}
}

515
src/FDM/JSBSim/filtersjb/FGCondition.cpp
View file

@ -1,258 +1,257 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGCondition.cpp
Author: Jon S. Berndt
Date started: 1/2/2003
-------------- Copyright (C) 2003 Jon S. Berndt (jsb@hal-pc.org) --------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
HISTORY
--------------------------------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
COMMENTS, REFERENCES, and NOTES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGCondition.h"
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_CONDITION;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
string FGCondition::indent = " ";
FGCondition::FGCondition(FGConfigFile* AC_cfg, FGPropertyManager* PropertyManager) :
PropertyManager(PropertyManager)
{
string property1, property2;
mComparison["EQ"] = eEQ;
mComparison["NE"] = eNE;
mComparison["GT"] = eGT;
mComparison["GE"] = eGE;
mComparison["LT"] = eLT;
mComparison["LE"] = eLE;
mComparison["=="] = eEQ;
mComparison["!="] = eNE;
mComparison[">"] = eGT;
mComparison[">="] = eGE;
mComparison["<"] = eLT;
mComparison["<="] = eLE;
TestParam1 = TestParam2 = 0L;
TestValue = 0.0;
Comparison = ecUndef;
Logic = elUndef;
conditions.clear();
if (AC_cfg->GetValue("CONDITION_GROUP").empty()) { // define a condition
*AC_cfg >> property1 >> conditional >> property2;
TestParam1 = PropertyManager->GetNode(property1, true);
Comparison = mComparison[conditional];
if (property2.find_first_not_of("-.0123456789eE") == string::npos) {
TestValue = atof(property2.c_str());
} else {
TestParam2 = PropertyManager->GetNode(property2, true);
}
isGroup = false;
} else { // define a condition group
if (AC_cfg->GetValue("LOGIC") == "OR") Logic = eOR;
else if (AC_cfg->GetValue("LOGIC") == "AND") Logic = eAND;
AC_cfg->GetNextConfigLine();
while (AC_cfg->GetValue() != string("/CONDITION_GROUP")) {
conditions.push_back(*(new FGCondition(AC_cfg, PropertyManager)));
}
isGroup = true;
AC_cfg->GetNextConfigLine();
}
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGCondition::~FGCondition(void)
{
Debug(1);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGCondition::Evaluate(void )
{
vector <FGCondition>::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 <FGCondition>::iterator iConditions;
string scratch;
bool first = false;
if (isGroup) {
switch(Logic) {
case (elUndef):
scratch = " UNSET";
cerr << "unset logic for test condition" << endl;
break;
case (eAND):
scratch = " if all of the following are true";
break;
case (eOR):
scratch = " if any of the following are true:";
break;
default:
scratch = " UNKNOWN";
cerr << "Unknown logic for test condition" << endl;
}
iConditions = conditions.begin();
cout << scratch << endl;
while (iConditions < conditions.end()) {
iConditions->PrintCondition();
*iConditions++;
}
} else {
if (TestParam2 != 0L)
cout << TestParam1->GetName() << " " << conditional << " " << TestParam2->GetName();
else
cout << TestParam1->GetName() << " " << conditional << " " << TestValue;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// The bitmasked value choices are as follows:
// unset: In this case (the default) JSBSim would only print
// out the normally expected messages, essentially echoing
// the config files as they are read. If the environment
// variable is not set, debug_lvl is set to 1 internally
// 0: This requests JSBSim not to output any messages
// whatsoever.
// 1: This value explicity requests the normal JSBSim
// startup messages
// 2: This value asks for a message to be printed out when
// a class is instantiated
// 4: When this value is set, a message is displayed when a
// FGModel object executes its Run() method
// 8: When this value is set, various runtime state variables
// are printed out periodically
// 16: When set various parameters are sanity checked and
// a message is printed out when they go out of bounds
void FGCondition::Debug(int from)
{
if (debug_lvl <= 0) return;
if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) { // Constructor
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
if (from == 0) cout << "Instantiated: FGCondition" << endl;
if (from == 1) cout << "Destroyed: FGCondition" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 ) { // Runtime state variables
}
if (debug_lvl & 16) { // Sanity checking
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
cout << IdSrc << endl;
cout << IdHdr << endl;
}
}
}
} //namespace JSBSim
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGCondition.cpp
Author: Jon S. Berndt
Date started: 1/2/2003
-------------- Copyright (C) 2003 Jon S. Berndt (jsb@hal-pc.org) --------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
HISTORY
--------------------------------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
COMMENTS, REFERENCES, and NOTES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGCondition.h"
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_CONDITION;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
string FGCondition::indent = " ";
FGCondition::FGCondition(FGConfigFile* AC_cfg, FGPropertyManager* PropertyManager) :
PropertyManager(PropertyManager)
{
string property1, property2;
mComparison["EQ"] = eEQ;
mComparison["NE"] = eNE;
mComparison["GT"] = eGT;
mComparison["GE"] = eGE;
mComparison["LT"] = eLT;
mComparison["LE"] = eLE;
mComparison["=="] = eEQ;
mComparison["!="] = eNE;
mComparison[">"] = eGT;
mComparison[">="] = eGE;
mComparison["<"] = eLT;
mComparison["<="] = eLE;
TestParam1 = TestParam2 = 0L;
TestValue = 0.0;
Comparison = ecUndef;
Logic = elUndef;
conditions.clear();
if (AC_cfg->GetValue("CONDITION_GROUP").empty()) { // define a condition
*AC_cfg >> property1 >> conditional >> property2;
TestParam1 = PropertyManager->GetNode(property1, true);
Comparison = mComparison[conditional];
if (property2.find_first_not_of("-.0123456789eE") == string::npos) {
TestValue = atof(property2.c_str());
} else {
TestParam2 = PropertyManager->GetNode(property2, true);
}
isGroup = false;
} else { // define a condition group
if (AC_cfg->GetValue("LOGIC") == "OR") Logic = eOR;
else if (AC_cfg->GetValue("LOGIC") == "AND") Logic = eAND;
AC_cfg->GetNextConfigLine();
while (AC_cfg->GetValue() != string("/CONDITION_GROUP")) {
conditions.push_back(FGCondition(AC_cfg, PropertyManager));
}
isGroup = true;
AC_cfg->GetNextConfigLine();
}
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGCondition::~FGCondition(void)
{
Debug(1);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGCondition::Evaluate(void )
{
vector <FGCondition>::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 <FGCondition>::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

147
src/FDM/JSBSim/filtersjb/FGKinemat.cpp
View file

@ -1,42 +1,45 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGKinemat.cpp
Author: Tony Peden, for flight control system authored by Jon S. Berndt
Date started: 12/02/01
------------- Copyright (C) 2000 Anthony K. Peden -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
HISTORY
--------------------------------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
COMMENTS, REFERENCES, and NOTES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include <math.h>
#include <float.h>
#include "FGKinemat.h"
namespace JSBSim {
@ -57,9 +60,9 @@ FGKinemat::FGKinemat(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
Detents.clear();
TransitionTimes.clear();
OutputPct=0;
InTransit=0;
DoScale = true;
Type = AC_cfg->GetValue("TYPE");
Name = AC_cfg->GetValue("NAME");
@ -74,10 +77,13 @@ FGKinemat::FGKinemat(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
} else {
*AC_cfg >> token;
InputNodes.push_back( resolveSymbol(token) );
}
}
} else if ( token == "DETENTS" ) {
*AC_cfg >> NumDetents;
if (NumDetents < 2) {
cerr << "Kinemat must have at least 2 DETENTS" << endl;
}
for (int i=0;i<NumDetents;i++) {
*AC_cfg >> tmpDetent;
*AC_cfg >> tmpTime;
@ -88,7 +94,10 @@ FGKinemat::FGKinemat(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
IsOutput = true;
*AC_cfg >> sOutputIdx;
OutputNode = PropertyManager->GetNode(sOutputIdx);
OutputNode = PropertyManager->GetNode(sOutputIdx, true);
} else if (token == "NOSCALE") {
DoScale = false;
}
}
FGFCSComponent::bind();
@ -106,71 +115,58 @@ FGKinemat::~FGKinemat()
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGKinemat::Run(void ) {
double dt=fcs->GetState()->Getdt();
double output_transit_rate=0;
bool FGKinemat::Run(void )
{
double dt = fcs->GetState()->Getdt();
Input = InputNodes[0]->getDoubleValue();
InputCmd = Input*Detents[NumDetents-1];
OutputPos = OutputNode->getDoubleValue();
if (InputCmd < Detents[0]) {
fi=0;
InputCmd=Detents[0];
lastInputCmd=InputCmd;
OutputPos=Detents[0];
Output=OutputPos;
} else if (InputCmd > Detents[NumDetents-1]) {
fi=NumDetents-1;
InputCmd=Detents[fi];
lastInputCmd=InputCmd;
OutputPos=Detents[fi];
Output=OutputPos;
} else {
//cout << "FGKinemat::Run Handle: " << InputCmd << " Position: " << OutputPos << endl;
if (dt <= 0)
OutputPos=InputCmd;
else {
if (InputCmd != lastInputCmd) {
InTransit=1;
}
//cout << "FGKinemat::Run, InTransit: " << InTransit << endl;
if (InTransit) {
//fprintf(stderr,"InputCmd: %g, OutputPos: %g\n",InputCmd,OutputPos);
fi=0;
while (Detents[fi] < InputCmd) {
fi++;
}
if (OutputPos < InputCmd) {
if (TransitionTimes[fi] > 0)
output_transit_rate=(Detents[fi] - Detents[fi-1])/TransitionTimes[fi];
else
output_transit_rate=(Detents[fi] - Detents[fi-1])/5;
//cout << "FGKinemat::Run, output_transit_rate: " << output_transit_rate << endl;
} else {
if (TransitionTimes[fi+1] > 0)
output_transit_rate=(Detents[fi] - Detents[fi+1])/TransitionTimes[fi+1];
else
output_transit_rate=(Detents[fi] - Detents[fi+1])/5;
}
if (fabs(OutputPos - InputCmd) > fabs(dt*output_transit_rate) ) {
OutputPos+=output_transit_rate*dt;
//cout << "FGKinemat::Run, OutputPos: " << OutputPos
// << " dt: " << dt << endl;
} else {
InTransit=0;
OutputPos=InputCmd;
}
}
if (DoScale) Input *= Detents[NumDetents-1];
Output = OutputNode->getDoubleValue();
if (Input < Detents[0])
Input = Detents[0];
else if (Detents[NumDetents-1] < Input)
Input = Detents[NumDetents-1];
// Process all detent intervals the movement traverses until either the
// final value is reached or the time interval has finished.
while (0.0 < dt && Input != Output) {
// Find the area where Output is in
int ind;
for (ind = 1; (Input < Output) ? Detents[ind] < Output : Detents[ind] <= Output ; ++ind)
if (NumDetents <= ind)
break;
// A transition time of 0.0 means an infinite rate.
// The output is reached in one step
if (TransitionTimes[ind] <= 0.0) {
Output = Input;
break;
} else {
// Compute the rate in this area
double Rate = (Detents[ind] - Detents[ind-1])/TransitionTimes[ind];
// Compute the maximum input value inside this area
double ThisInput = Input;
if (ThisInput < Detents[ind-1]) ThisInput = Detents[ind-1];
if (Detents[ind] < ThisInput) ThisInput = Detents[ind];
// Compute the time to reach the value in ThisInput
double ThisDt = fabs((ThisInput-Output)/Rate);
// and clip to the timestep size
if (dt < ThisDt) ThisDt = dt;
dt -= ThisDt;
// Do the output calculation
if (Output < Input)
Output += ThisDt*Rate;
else
Output -= ThisDt*Rate;
}
lastInputCmd = InputCmd;
Output = OutputPos;
}
if ( Detents[NumDetents-1] > 0 ) {
OutputPct = Output / Detents[NumDetents-1];
}
OutputPct = (Output-Detents[0])/(Detents[NumDetents-1]-Detents[0]);
if (IsOutput) SetOutput();
return true;
@ -207,6 +203,7 @@ void FGKinemat::Debug(int from)
cout << " " << Detents[i] << " " << TransitionTimes[i] << endl;
}
if (IsOutput) cout << " OUTPUT: " << OutputNode->getName() << endl;
if (!DoScale) cout << " NOSCALE" << endl;
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification

9
src/FDM/JSBSim/filtersjb/FGKinemat.h
View file

@ -82,18 +82,15 @@ public:
double GetOutputPct() const { return OutputPct; }
bool Run (void );
bool Run (void);
private:
FGConfigFile* AC_cfg;
vector<double> Detents;
vector<double> TransitionTimes;
int NumDetents,fi;
double lastInputCmd;
double InputCmd;
double OutputPos;
int NumDetents;
double OutputPct;
bool InTransit;
bool DoScale;
void Debug(int from);
};

560
src/FDM/JSBSim/filtersjb/FGSwitch.cpp
View file

@ -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):
<COMPONENT NAME="switch1" TYPE="SWITCH">
<TEST LOGIC="{AND|OR|DEFAULT}" OUTPUT="{property|value}">
{property} {conditional} {property|value}
<CONDITION_GROUP LOGIC="{AND|OR}">
{property} {conditional} {property|value}
...
</CONDITION_GROUP>
...
</TEST>
<TEST LOGIC="{AND|OR}" OUTPUT="{property|value}">
{property} {conditional} {property|value}
...
</TEST>
...
</COMPONENT>
Also, see the header file (FGSwitch.h) for further details.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGSwitch.h"
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_SWITCH;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
FGSwitch::FGSwitch(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
AC_cfg(AC_cfg)
{
string token, value;
struct test *current_test;
struct FGCondition *current_condition;
Type = AC_cfg->GetValue("TYPE");
Name = AC_cfg->GetValue("NAME");
AC_cfg->GetNextConfigLine();
while ((token = AC_cfg->GetValue()) != string("/COMPONENT")) {
// See the above documentation, or the API docs, for information on what
// the SWITCH component is supposed to look like in the configuration file.
// Below, the switch component is read in.
if (token == "TEST") {
tests.push_back(*(new test));
current_test = &tests.back();
if (AC_cfg->GetValue("LOGIC") == "OR") {
current_test->Logic = eOR;
} else if (AC_cfg->GetValue("LOGIC") == "AND") {
current_test->Logic = eAND;
} else if (AC_cfg->GetValue("LOGIC") == "DEFAULT") {
current_test->Logic = eDefault;
} else { // error
cerr << "Unrecognized LOGIC token in switch component: " << Name << endl;
}
value = AC_cfg->GetValue("VALUE");
if (value.empty()) {
cerr << "No VALUE supplied for switch component: " << Name << endl;
} else {
if (value.find_first_not_of("-.0123456789eE") == string::npos) {
// if true (and execution falls into this block), "value" is a number.
current_test->OutputVal = atof(value.c_str());
} else {
// "value" must be a property if execution passes to here.
if (value[0] == '-') {
current_test->sign = -1.0;
value.erase(0,1);
} else {
current_test->sign = 1.0;
}
current_test->OutputProp = PropertyManager->GetNode(value);
}
}
AC_cfg->GetNextConfigLine();
while (AC_cfg->GetValue() != string("/TEST")) {
current_test->conditions.push_back(*(new FGCondition(AC_cfg, PropertyManager)));
}
}
AC_cfg->GetNextConfigLine();
}
FGFCSComponent::bind();
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGSwitch::~FGSwitch()
{
Debug(1);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGSwitch::Run(void )
{
vector <test>::iterator iTests = tests.begin();
vector <FGCondition>::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 <test>::iterator iTests = tests.begin();
vector <FGCondition>::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):
<COMPONENT NAME="switch1" TYPE="SWITCH">
<TEST LOGIC="{AND|OR|DEFAULT}" OUTPUT="{property|value}">
{property} {conditional} {property|value}
<CONDITION_GROUP LOGIC="{AND|OR}">
{property} {conditional} {property|value}
...
</CONDITION_GROUP>
...
</TEST>
<TEST LOGIC="{AND|OR}" OUTPUT="{property|value}">
{property} {conditional} {property|value}
...
</TEST>
...
</COMPONENT>
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 <test>::iterator iTests = tests.begin();
vector <FGCondition>::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 <test>::iterator iTests = tests.begin();
vector <FGCondition>::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

12
src/FDM/JSBSim/filtersjb/FGSwitch.h
View file

@ -120,7 +120,7 @@ whatever value fcs/roll-ap-error-summer is.
@author Jon S. Berndt
@version $Id$
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
@ -133,20 +133,20 @@ public:
bool Run(void);
enum eLogic {elUndef=0, eAND, eOR, eDefault};
enum eComparison {ecUndef=0, eEQ, eNE, eGT, eGE, eLT, eLE};
private:
FGFCS* fcs;
FGConfigFile* AC_cfg;
enum eLogic {elUndef=0, eAND, eOR, eDefault};
enum eComparison {ecUndef=0, eEQ, eNE, eGT, eGE, eLT, eLE};
struct test {
vector <FGCondition> conditions;
eLogic Logic;
double OutputVal;
FGPropertyManager *OutputProp;
float sign;
double GetValue(void) {
if (OutputProp == 0L) return OutputVal;
else return OutputProp->getDoubleValue()*sign;
@ -162,7 +162,7 @@ private:
};
vector <test> tests;
void Debug(int from);
};
}