%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#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 {
#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
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
+
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#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 {
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;
{
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")) {
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;
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);
<< endl;
}
}
-
+
+ MassBalance->SetAircraftBaseInertias(FGMatrix33( bixx, -bixy, -bixz,
+ -bixy, biyy, -biyz,
+ -bixz, -biyz, bizz ));
+
// calculate some derived parameters
if (cbar != 0.0) {
lbarh = HTailArm/cbar;
vbarh = HTailArm*HTailArea / (cbar*WingArea);
vbarv = VTailArm*VTailArea / (cbar*WingArea);
}
- }
+ }
return true;
}
(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);
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");
#endif
#include "FGModel.h"
-#include "FGPropulsion.h"
#include "FGConfigFile.h"
-#include "FGMatrix33.h"
#include "FGColumnVector3.h"
-#include "FGColumnVector4.h"
-#include "FGLGear.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
FGColumnVector3 vXYZrp;
FGColumnVector3 vXYZvrp;
FGColumnVector3 vXYZep;
- FGColumnVector3 vEuler;
FGColumnVector3 vDXYZcg;
FGColumnVector3 vBodyAccel;
FGColumnVector3 vNcg;
#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 {
soundspeed = sqrt(SHRatio*Reng*(*temperature));
- State->Seta(soundspeed);
-
Debug(2);
return false;
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;
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;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGModel.h"
-#include "FGMatrix33.h"
#include "FGColumnVector3.h"
-#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
#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 {
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGModel.h"
-#include "FGMatrix33.h"
#include "FGColumnVector3.h"
-#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-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
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+Module: FGColumnVector3.cpp\r
+Author: Originally by Tony Peden [formatted here (and broken??) by JSB]\r
+Date started: 1998\r
+Purpose: FGColumnVector3 class\r
+Called by: Various\r
+\r
+FUNCTIONAL DESCRIPTION\r
+--------------------------------------------------------------------------------\r
+\r
+HISTORY\r
+--------------------------------------------------------------------------------\r
+??/??/?? TP Created\r
+03/16/2000 JSB Added exception throwing\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+INCLUDES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+#include "FGColumnVector3.h"\r
+\r
+namespace JSBSim {\r
+\r
+static const char *IdSrc = "$Id$";\r
+static const char *IdHdr = ID_COLUMNVECTOR3;\r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+CLASS IMPLEMENTATION\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+FGColumnVector3::FGColumnVector3(void)\r
+{\r
+ data[0] = data[1] = data[2] = 0.0;\r
+ Debug(0);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGColumnVector3 FGColumnVector3::operator/(const double scalar) const\r
+{\r
+ if (scalar != 0.0)\r
+ return operator*( 1.0/scalar );\r
+\r
+ cerr << "Attempt to divide by zero in method "\r
+ "FGColumnVector3::operator/(const double scalar), "\r
+ "object " << this << endl; \r
+ return FGColumnVector3();\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGColumnVector3& FGColumnVector3::operator/=(const double scalar)\r
+{\r
+ if (scalar != 0.0)\r
+ operator*=( 1.0/scalar );\r
+ else\r
+ cerr << "Attempt to divide by zero in method "\r
+ "FGColumnVector3::operator/=(const double scalar), "\r
+ "object " << this << endl;\r
+\r
+ return *this;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+double FGColumnVector3::Magnitude(void) const\r
+{\r
+ if (data[1] == 0.0 && data[2] == 0.0 && data[3] == 0.0)\r
+ return 0.0;\r
+ else\r
+ return sqrt( Entry(1)*Entry(1) + Entry(2)*Entry(2) + Entry(3)*Entry(3) );\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGColumnVector3& FGColumnVector3::Normalize(void)\r
+{\r
+ double Mag = Magnitude();\r
+\r
+ if (Mag != 0.0)\r
+ operator*=( 1.0/Mag );\r
+\r
+ return *this;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGColumnVector3 FGColumnVector3::multElementWise(const FGColumnVector3& V) const\r
+{\r
+ return FGColumnVector3(Entry(1) * V(1), Entry(2) * V(2), Entry(3) * V(3));\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+ostream& operator<<(ostream& os, const FGColumnVector3& col)\r
+{\r
+ os << col(1) << " , " << col(2) << " , " << col(3);\r
+ return os;\r
+} \r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+// The bitmasked value choices are as follows:\r
+// unset: In this case (the default) JSBSim would only print\r
+// out the normally expected messages, essentially echoing\r
+// the config files as they are read. If the environment\r
+// variable is not set, debug_lvl is set to 1 internally\r
+// 0: This requests JSBSim not to output any messages\r
+// whatsoever.\r
+// 1: This value explicity requests the normal JSBSim\r
+// startup messages\r
+// 2: This value asks for a message to be printed out when\r
+// a class is instantiated\r
+// 4: When this value is set, a message is displayed when a\r
+// FGModel object executes its Run() method\r
+// 8: When this value is set, various runtime state variables\r
+// are printed out periodically\r
+// 16: When set various parameters are sanity checked and\r
+// a message is printed out when they go out of bounds\r
+\r
+void FGColumnVector3::Debug(int from)\r
+{\r
+ if (debug_lvl <= 0) return;\r
+\r
+ if (debug_lvl & 1) { // Standard console startup message output\r
+ }\r
+ if (debug_lvl & 2 ) { // Instantiation/Destruction notification\r
+ if (from == 0) cout << "Instantiated: FGColumnVector3" << endl;\r
+ if (from == 1) cout << "Destroyed: FGColumnVector3" << endl;\r
+ }\r
+ if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects\r
+ }\r
+ if (debug_lvl & 8 ) { // Runtime state variables\r
+ }\r
+ if (debug_lvl & 16) { // Sanity checking\r
+ }\r
+ if (debug_lvl & 64) {\r
+ if (from == 0) { // Constructor\r
+ cout << IdSrc << endl;\r
+ cout << IdHdr << endl;\r
+ }\r
+ }\r
+}\r
+\r
+} // namespace JSBSim\r
HISTORY
--------------------------------------------------------------------------------
+??/??/???? ?? Initial version and more.
+03/06/2004 MF Rework, document and do much inlineing.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
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)
using std::cerr;
using std::cout;
using std::endl;
+ using std::sqrt;
# endif
using std::string;
#endif
class FGColumnVector3 : public FGJSBBase
{
public:
+ /** Default initializer.
+
+ Create a zero vector.
+ */
FGColumnVector3(void);
- FGColumnVector3(double X, double Y, double Z);
- FGColumnVector3(const FGColumnVector3& b);
- ~FGColumnVector3(void);
-
- FGColumnVector3 operator=(const FGColumnVector3& b);
-
- FGColumnVector3 operator*(const double scalar);
- FGColumnVector3 operator*(const FGColumnVector3& V); // Cross product operator
- FGColumnVector3 operator/(const double scalar);
- FGColumnVector3 operator+(const FGColumnVector3& B); // must not return reference
- FGColumnVector3 operator-(const FGColumnVector3& B);
-
- void operator-=(const FGColumnVector3 &B);
- void operator+=(const FGColumnVector3 &B);
- void operator*=(const FGColumnVector3 &B);
- void operator*=(const double scalar);
- void operator/=(const double scalar);
- FGColumnVector3& operator<<(const double ff);
+ /** Initialization by given values.
+
+ @param X value of the x-conponent.
+ @param Y value of the y-conponent.
+ @param Z value of the z-conponent.
+
+ Create a vector from the doubles given in the arguments.
+ */
+ FGColumnVector3(double X, double Y, double Z) {
+ data[0] = X;
+ data[1] = Y;
+ data[2] = Z;
+ Debug(0);
+ }
+
+ /** Copy constructor.
+
+ @param v Vector which is used for initialization.
+
+ Create copy of the vector given in the argument.
+ */
+ FGColumnVector3(const FGColumnVector3& v) {
+ data[0] = v.data[0];
+ data[1] = v.data[1];
+ data[2] = v.data[2];
+ Debug(0);
+ }
+
+ /** Destructor.
+ */
+ ~FGColumnVector3(void) { Debug(1); }
+
+
+ /** Read access the entries of the vector.
+
+ @param idx the component index.
+
+ Return the value of the matrix entry at the given index.
+ Indices are counted starting with 1.
+
+ Note that the index given in the argument is unchecked.
+ */
+ double operator()(unsigned int idx) const { return Entry(idx); }
+
+ /** Write access the entries of the vector.
+
+ @param idx the component index.
+
+ Return a reference to the vector entry at the given index.
+ Indices are counted starting with 1.
+
+ Note that the index given in the argument is unchecked.
+ */
+ double& operator()(unsigned int idx) { return Entry(idx); }
+
+ /** Read access the entries of the vector.
+
+ @param idx the component index.
+
+ Return the value of the matrix entry at the given index.
+ Indices are counted starting with 1.
+
+ This function is just a shortcut for the @ref double
+ operator()(unsigned int idx) const function. It is
+ used internally to access the elements in a more convenient way.
+
+ Note that the index given in the argument is unchecked.
+ */
+ double Entry(unsigned int idx) const { return data[idx-1]; }
+
+ /** Write access the entries of the vector.
+
+ @param idx the component index.
+
+ Return a reference to the vector entry at the given index.
+ Indices are counted starting with 1.
+
+ This function is just a shortcut for the @ref double&
+ operator()(unsigned int idx) function. It is
+ used internally to access the elements in a more convenient way.
+
+ Note that the index given in the argument is unchecked.
+ */
+ double& Entry(unsigned int idx) { return data[idx-1]; }
+
+ /** Assignment operator.
+
+ @param b source vector.
+
+ Copy the content of the vector given in the argument into *this.
+ */
+ FGColumnVector3& operator=(const FGColumnVector3& b) {
+ data[0] = b.data[0];
+ data[1] = b.data[1];
+ data[2] = b.data[2];
+ return *this;
+ }
+
+ /** Multiplication by a scalar.
+
+ @param scalar scalar value to multiply the vector with.
+ @return The resulting vector from the multiplication with that scalar.
+
+ Multiply the vector with the scalar given in the argument.
+ */
+ FGColumnVector3 operator*(const double scalar) const {
+ return FGColumnVector3(scalar*Entry(1), scalar*Entry(2), scalar*Entry(3));
+ }
+
+ /** Multiply by 1/scalar.
+
+ @param scalar scalar value to devide the vector through.
+ @return The resulting vector from the division through that scalar.
+
+ Multiply the vector with the 1/scalar given in the argument.
+ */
+ FGColumnVector3 operator/(const double scalar) const;
+
+ /** Cross product multiplication.
+
+ @param v vector to multiply with.
+ @return The resulting vector from the cross product multiplication.
+
+ Compute and return the cross product of the current vector with
+ the given argument.
+ */
+ FGColumnVector3 operator*(const FGColumnVector3& V) const {
+ return FGColumnVector3( Entry(2) * V(3) - Entry(3) * V(2),
+ Entry(3) * V(1) - Entry(1) * V(3),
+ Entry(1) * V(2) - Entry(2) * V(1) );
+ }
+
+ /** Addition operator.
+ */
+ FGColumnVector3 operator+(const FGColumnVector3& B) const {
+ return FGColumnVector3( Entry(1) + B(1), Entry(2) + B(2), Entry(3) + B(3) );
+ }
+
+ /** Subtraction operator.
+ */
+ FGColumnVector3 operator-(const FGColumnVector3& B) const {
+ return FGColumnVector3( Entry(1) - B(1), Entry(2) - B(2), Entry(3) - B(3) );
+ }
- inline void InitMatrix(void) { data[1]=0; data[2]=0; data[3]=0; }
- inline void InitMatrix(double ff) { data[1]=ff; data[2]=ff; data[3]=ff; }
+ /** Subtract an other vector.
+ */
+ FGColumnVector3& operator-=(const FGColumnVector3 &B) {
+ Entry(1) -= B(1);
+ Entry(2) -= B(2);
+ Entry(3) -= B(3);
+ return *this;
+ }
- double Magnitude(void);
- FGColumnVector3 Normalize(void);
+ /** Add an other vector.
+ */
+ FGColumnVector3& operator+=(const FGColumnVector3 &B) {
+ Entry(1) += B(1);
+ Entry(2) += B(2);
+ Entry(3) += B(3);
+ return *this;
+ }
- friend FGColumnVector3 operator*(const double scalar, const FGColumnVector3& A);
+ /** Scale by a scalar.
+ */
+ FGColumnVector3& operator*=(const double scalar) {
+ Entry(1) *= scalar;
+ Entry(2) *= scalar;
+ Entry(3) *= scalar;
+ return *this;
+ }
- friend ostream& operator<<(ostream& os, const FGColumnVector3& col);
+ /** Scale by a 1/scalar.
+ */
+ FGColumnVector3& operator/=(const double scalar);
- inline double operator()(int m) const { return data[m]; }
- inline double& operator()(int m) { return data[m]; }
+ void InitMatrix(void) { data[0] = data[1] = data[2] = 0.0; }
+ void InitMatrix(double a) { data[0] = data[1] = data[2] = a; }
+ void InitMatrix(double a, double b, double c) {
+ data[0]=a; data[1]=b; data[2]=c;
+ }
- FGColumnVector3 multElementWise(const FGColumnVector3& V);
+ /** Length of the vector.
+
+ Compute and return the euclidean norm of this vector.
+ */
+ double Magnitude(void) const;
+
+ /** Normialze.
+
+ Normalize the vector to have the Magnitude() == 1.0. If the vector
+ is equal to zero it is left untouched.
+ */
+ FGColumnVector3& Normalize(void);
+
+ // ??? Is this something sensible ??
+ FGColumnVector3 multElementWise(const FGColumnVector3& V) const;
+
+ // little trick here.
+ struct AssignRef {
+ AssignRef(FGColumnVector3& r, int i) : Ref(r), idx(i) {}
+ AssignRef operator<<(const double ff) {
+ Ref.Entry(idx) = ff;
+ return AssignRef(Ref, idx+1);
+ }
+ FGColumnVector3& Ref;
+ int idx;
+ };
+ AssignRef operator<<(const double ff) {
+ Entry(1) = ff;
+ return AssignRef(*this, 2);
+ }
private:
- double data[4];
- int rowCtr;
+ double data[3];
+
void Debug(int from);
};
+
+/** Scalar multiplication.
+
+ @param scalar scalar value to multiply with.
+ @param A Vector to multiply.
+
+ Multiply the Vector with a scalar value.
+*/
+inline FGColumnVector3 operator*(double scalar, const FGColumnVector3& A) {
+ // use already defined operation.
+ return A*scalar;
}
+
+/** Write vector to a stream.
+
+ @param os Stream to write to.
+ @param M Matrix to write.
+
+ Write the matrix to a stream.
+*/
+ostream& operator<<(ostream& os, const FGColumnVector3& col);
+
+} // namespace JSBSim
+
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endif
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);
}
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)
using std::cerr;
using std::cout;
using std::endl;
+ using std::sqrt;
# endif
using std::string;
#endif
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; }
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
+
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"
LeftBrake = RightBrake = CenterBrake = 0.0;
APAttitudeSetPt = APAltitudeSetPt = APHeadingSetPt = APAirspeedSetPt = 0.0;
DoNormalize=true;
-
+
eMode = mNone;
bind();
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);
}
for (i=0;i<APComponents.size();i++) delete APComponents[i];
for (i=0;i<FCSComponents.size();i++) delete FCSComponents[i];
-
+
Debug(1);
}
<< " engines exist, but attempted throttle position setting is for engine "
<< engineNum << endl;
}
- return 0.0;
+ return 0.0;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
return false;
} else {
AC_cfg = FCS_cfg; // set local config file object pointer to FCS config
- // file object pointer
+ // file object pointer
}
}
} else {
} 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)) {
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" ) {
ToNormalize[iDf]=i;
}
}
- }
-
+ }
+
if (delimiter == "FLIGHT_CONTROL") bindModel();
eMode = mNone;
break;
}
return string("");
-}
+}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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();
}
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]);
DsbPos[ofMag] = fabs(DsbPos[ofRad]);
DspPos[ofMag] = fabs(DspPos[ofRad]);
DfPos[ofMag] = fabs(DfPos[ofRad]);
-
-}
-
+
+}
+
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGFCS::bind(void)
&FGFCS::GetGearCmd,
&FGFCS::SetGearCmd,
true);
-
+
PropertyManager->Tie("fcs/left-aileron-pos-rad", this,ofRad,
&FGFCS::GetDaLPos,
&FGFCS::SetDaLPos,
&FGFCS::GetDaLPos,
&FGFCS::SetDaLPos,
true);
-
+
PropertyManager->Tie("fcs/right-aileron-pos-rad", this,ofRad,
&FGFCS::GetDaRPos,
&FGFCS::SetDaRPos,
&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,
&FGFCS::GetDrPos,
&FGFCS::SetDrPos,
true);
-
+
PropertyManager->Tie("fcs/flap-pos-deg", this,ofRad,
&FGFCS::GetDfPos,
&FGFCS::SetDfPos,
&FGFCS::GetDfPos,
&FGFCS::SetDfPos,
true);
-
+
PropertyManager->Tie("fcs/speedbrake-pos-rad", this,ofRad,
&FGFCS::GetDsbPos,
&FGFCS::SetDsbPos,
&FGFCS::GetDsbPos,
&FGFCS::SetDsbPos,
true);
-
+
PropertyManager->Tie("fcs/spoiler-pos-rad", this,ofRad,
&FGFCS::GetDspPos,
&FGFCS::SetDspPos,
&FGFCS::GetDspPos,
&FGFCS::SetDspPos,
true);
-
+
PropertyManager->Tie("gear/gear-pos-norm", this,
&FGFCS::GetGearPos,
&FGFCS::SetGearPos,
{
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)
unbind( (FGPropertyManager*)node->getChild(i) );
} else if( node->getChild(i)->isTied() ) {
node->getChild(i)->untie();
- }
- }
+ }
+ }
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
- 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;
- }
- }
-}
-}
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+ Source: FGForce.cpp\r
+ Author: Tony Peden\r
+ Date started: 6/10/00\r
+\r
+ ------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------\r
+\r
+ This program is free software; you can redistribute it and/or modify it under\r
+ the terms of the GNU General Public License as published by the Free Software\r
+ Foundation; either version 2 of the License, or (at your option) any later\r
+ version.\r
+\r
+ This program is distributed in the hope that it will be useful, but WITHOUT\r
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS\r
+ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more\r
+ details.\r
+\r
+ You should have received a copy of the GNU General Public License along with\r
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple\r
+ Place - Suite 330, Boston, MA 02111-1307, USA.\r
+\r
+ Further information about the GNU General Public License can also be found on\r
+ the world wide web at http://www.gnu.org.\r
+\r
+\r
+ HISTORY\r
+--------------------------------------------------------------------------------\r
+6/10/00 TP Created\r
+\r
+\r
+FUNCTIONAL DESCRIPTION\r
+--------------------------------------------------------------------------------\r
+\r
+The purpose of this class is to provide storage for computed forces and\r
+encapsulate all the functionality associated with transforming those\r
+forces from their native coord system to the body system. This includes\r
+computing the moments due to the difference between the point of application\r
+and the cg.\r
+\r
+*/\r
+\r
+#include "FGFDMExec.h"\r
+#include "FGAircraft.h"\r
+#include "FGTranslation.h"\r
+#include "FGMassBalance.h"\r
+#include "FGState.h"\r
+#include "FGForce.h"\r
+\r
+namespace JSBSim {\r
+\r
+static const char *IdSrc = "$Id$";\r
+static const char *IdHdr = ID_FORCE;\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGForce::FGForce(FGFDMExec *FDMExec) :\r
+ ttype(tNone),\r
+ fdmex(FDMExec)\r
+{\r
+ mT(1,1) = 1; //identity matrix\r
+ mT(2,2) = 1;\r
+ mT(3,3) = 1;\r
+ vSense.InitMatrix(1);\r
+ Debug(0);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGForce::~FGForce()\r
+{\r
+ Debug(1);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGColumnVector3& FGForce::GetBodyForces(void)\r
+{\r
+ vFb = Transform()*(vFn.multElementWise(vSense));\r
+\r
+ // Find the distance from this vector's acting location to the cg; this\r
+ // needs to be done like this to convert from structural to body coords.\r
+ // CG and RP values are in inches\r
+\r
+ vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn);\r
+\r
+ vM = vMn + vDXYZ*vFb;\r
+\r
+ return vFb;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33 FGForce::Transform(void)\r
+{\r
+ switch(ttype) {\r
+ case tWindBody:\r
+ return fdmex->GetState()->GetTs2b();\r
+ case tLocalBody:\r
+ return fdmex->GetState()->GetTl2b();\r
+ case tCustom:\r
+ case tNone:\r
+ return mT;\r
+ default:\r
+ cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;\r
+ exit(1);\r
+ }\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)\r
+{\r
+ if (ttype == tCustom) {\r
+ double cp,sp,cr,sr,cy,sy;\r
+\r
+ cp=cos(bpitch); sp=sin(bpitch);\r
+ cr=cos(broll); sr=sin(broll);\r
+ cy=cos(byaw); sy=sin(byaw);\r
+\r
+ mT(1,1)=cp*cy;\r
+ mT(1,2)=cp*sy;\r
+ mT(1,3)=-1*sp;\r
+\r
+ mT(2,1)=sr*sp*cy-cr*sy;\r
+ mT(2,2)=sr*sp*sy+cr*cy;\r
+ mT(2,3)=sr*cp;\r
+\r
+ mT(3,1)=cr*sp*cy+sr*sy;\r
+ mT(3,2)=cr*sp*sy-sr*cy;\r
+ mT(3,3)=cr*cp;\r
+ }\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+// The bitmasked value choices are as follows:\r
+// unset: In this case (the default) JSBSim would only print\r
+// out the normally expected messages, essentially echoing\r
+// the config files as they are read. If the environment\r
+// variable is not set, debug_lvl is set to 1 internally\r
+// 0: This requests JSBSim not to output any messages\r
+// whatsoever.\r
+// 1: This value explicity requests the normal JSBSim\r
+// startup messages\r
+// 2: This value asks for a message to be printed out when\r
+// a class is instantiated\r
+// 4: When this value is set, a message is displayed when a\r
+// FGModel object executes its Run() method\r
+// 8: When this value is set, various runtime state variables\r
+// are printed out periodically\r
+// 16: When set various parameters are sanity checked and\r
+// a message is printed out when they go out of bounds\r
+\r
+void FGForce::Debug(int from)\r
+{\r
+ if (debug_lvl <= 0) return;\r
+\r
+ if (debug_lvl & 1) { // Standard console startup message output\r
+ if (from == 0) { // Constructor\r
+\r
+ }\r
+ }\r
+ if (debug_lvl & 2 ) { // Instantiation/Destruction notification\r
+ if (from == 0) cout << "Instantiated: FGForce" << endl;\r
+ if (from == 1) cout << "Destroyed: FGForce" << endl;\r
+ }\r
+ if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects\r
+ }\r
+ if (debug_lvl & 8 ) { // Runtime state variables\r
+ }\r
+ if (debug_lvl & 16) { // Sanity checking\r
+ }\r
+ if (debug_lvl & 64) {\r
+ if (from == 0) { // Constructor\r
+ cout << IdSrc << endl;\r
+ cout << IdHdr << endl;\r
+ }\r
+ }\r
+}\r
+}\r
#include "FGJSBBase.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
-#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
#include "FGModel.h"
#include "FGConfigFile.h"
#include "FGLGear.h"
-#include "FGInertial.h"
-#include "FGMatrix33.h"
+#include "FGColumnVector3.h"
#define ID_GROUNDREACTIONS "$Id$"
#include "FGInertial.h"
#include "FGPosition.h"
+#include "FGState.h"
#include "FGMassBalance.h"
namespace JSBSim {
vCoriolis.InitMatrix();
vCentrifugal.InitMatrix();
vGravity.InitMatrix();
-
+
bind();
Debug(0);
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;
#include "FGModel.h"
#include "FGConfigFile.h"
-#include "FGMatrix33.h"
#include "FGColumnVector3.h"
-#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
/*******************************************************************************
-
+
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"
} else {
cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
}
-
+
Debug(0);
}
beta=tt;
sbeta=sin(beta); cbeta=cos(beta);
getTheta();
-
+
}
//******************************************************************************
return v;
else {
return vt*sbeta - vw;
- }
+ }
}
//******************************************************************************
double FGInitialCondition::GetWBodyFpsIC(void) {
if( lastSpeedSet == setvg )
return w;
- else
+ else
return vt*salpha*cbeta -ww;
}
//******************************************************************************
// 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);
}
void FGInitialCondition::SetWindDirDegIC(double dir) {
wdir=dir*degtorad;
lastWindSet=setwmd;
- calcWindUVW();
+ calcWindUVW();
if(lastSpeedSet == setvg)
SetVgroundFpsIC(vg);
}
//******************************************************************************
void FGInitialCondition::calcWindUVW(void) {
-
+
switch(lastWindSet) {
case setwmd:
wnorth=wmag*cos(wdir);
break;
case setwned:
break;
- }
+ }
uw=wnorth*ctheta*cpsi +
weast*ctheta*spsi -
wdown*stheta;
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; */
}
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();
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;
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;
//******************************************************************************
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;
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();
<< " 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); }
}
fdmex->RunIC();
-
+
return true;
}
/* 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"); */
#include "FGFDMExec.h"
#include "FGJSBBase.h"
-#include "FGAtmosphere.h"
-#include "FGMatrix33.h"
#include "FGColumnVector3.h"
-#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
- 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
-
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+ Module: FGJSBBase.cpp\r
+ Author: Jon S. Berndt\r
+ Date started: 07/01/01\r
+ Purpose: Encapsulates the JSBBase object\r
+\r
+ ------------- Copyright (C) 2001 Jon S. Berndt (jsb@hal-pc.org) -------------\r
+\r
+ This program is free software; you can redistribute it and/or modify it under\r
+ the terms of the GNU General Public License as published by the Free Software\r
+ Foundation; either version 2 of the License, or (at your option) any later\r
+ version.\r
+\r
+ This program is distributed in the hope that it will be useful, but WITHOUT\r
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS\r
+ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more\r
+ details.\r
+\r
+ You should have received a copy of the GNU General Public License along with\r
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple\r
+ Place - Suite 330, Boston, MA 02111-1307, USA.\r
+\r
+ Further information about the GNU General Public License can also be found on\r
+ the world wide web at http://www.gnu.org.\r
+\r
+FUNCTIONAL DESCRIPTION\r
+--------------------------------------------------------------------------------\r
+\r
+HISTORY\r
+--------------------------------------------------------------------------------\r
+07/01/01 JSB Created\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+INCLUDES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+#include "FGJSBBase.h"\r
+\r
+namespace JSBSim {\r
+\r
+static const char *IdSrc = "$Id$";\r
+static const char *IdHdr = ID_JSBBASE;\r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+CLASS IMPLEMENTATION\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+char FGJSBBase::highint[5] = {27, '[', '1', 'm', '\0' };\r
+char FGJSBBase::halfint[5] = {27, '[', '2', 'm', '\0' };\r
+char FGJSBBase::normint[6] = {27, '[', '2', '2', 'm', '\0' };\r
+char FGJSBBase::reset[5] = {27, '[', '0', 'm', '\0' };\r
+char FGJSBBase::underon[5] = {27, '[', '4', 'm', '\0' };\r
+char FGJSBBase::underoff[6] = {27, '[', '2', '4', 'm', '\0' };\r
+char FGJSBBase::fgblue[6] = {27, '[', '3', '4', 'm', '\0' };\r
+char FGJSBBase::fgcyan[6] = {27, '[', '3', '6', 'm', '\0' };\r
+char FGJSBBase::fgred[6] = {27, '[', '3', '1', 'm', '\0' };\r
+char FGJSBBase::fggreen[6] = {27, '[', '3', '2', 'm', '\0' };\r
+char FGJSBBase::fgdef[6] = {27, '[', '3', '9', 'm', '\0' };\r
+\r
+const double FGJSBBase::radtodeg = 57.29578;\r
+const double FGJSBBase::degtorad = 1.745329E-2;\r
+const double FGJSBBase::hptoftlbssec = 550.0;\r
+const double FGJSBBase::psftoinhg = 0.014138;\r
+const double FGJSBBase::fpstokts = 0.592484;\r
+const double FGJSBBase::ktstofps = 1.68781;\r
+const double FGJSBBase::inchtoft = 0.08333333;\r
+const double FGJSBBase::in3tom3 = 1.638706E-5;\r
+double FGJSBBase::Reng = 1716.0;\r
+const double FGJSBBase::SHRatio = 1.40;\r
+\r
+// Note that definition of lbtoslug by the inverse of slugtolb and not\r
+// to a different constant you can also get from some tables will make\r
+// lbtoslug*slugtolb == 1 up to the magnitude of roundoff. So converting from\r
+// slug to lb and back will yield to the original value you started with up\r
+// to the magnitude of roundoff.\r
+// Taken from units gnu commandline tool\r
+const double FGJSBBase::slugtolb = 32.174049;\r
+const double FGJSBBase::lbtoslug = 1.0/slugtolb;\r
+\r
+const string FGJSBBase::needed_cfg_version = "1.61";\r
+const string FGJSBBase::JSBSim_version = "0.9.5";\r
+\r
+std::queue <FGJSBBase::Message*> FGJSBBase::Messages;\r
+FGJSBBase::Message FGJSBBase::localMsg;\r
+unsigned int FGJSBBase::messageId = 0;\r
+unsigned int FGJSBBase::frame = 0;\r
+\r
+short FGJSBBase::debug_lvl = 1;\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGJSBBase::FGJSBBase()\r
+{\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGJSBBase::Message* FGJSBBase::PutMessage(Message* msg)\r
+{\r
+ Messages.push(msg);\r
+ return msg;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGJSBBase::Message* FGJSBBase::PutMessage(string text)\r
+{\r
+ Message *msg = new Message();\r
+ msg->text = text;\r
+ msg->messageId = messageId++;\r
+ msg->subsystem = "FDM";\r
+ msg->type = Message::eText;\r
+ Messages.push(msg);\r
+ return msg;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGJSBBase::Message* FGJSBBase::PutMessage(string text, bool bVal)\r
+{\r
+ Message *msg = new Message();\r
+ msg->text = text;\r
+ msg->messageId = messageId++;\r
+ msg->subsystem = "FDM";\r
+ msg->type = Message::eBool;\r
+ msg->bVal = bVal;\r
+ Messages.push(msg);\r
+ return msg;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGJSBBase::Message* FGJSBBase::PutMessage(string text, int iVal)\r
+{\r
+ Message *msg = new Message();\r
+ msg->text = text;\r
+ msg->messageId = messageId++;\r
+ msg->subsystem = "FDM";\r
+ msg->type = Message::eInteger;\r
+ msg->bVal = (iVal != 0);\r
+ Messages.push(msg);\r
+ return msg;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGJSBBase::Message* FGJSBBase::PutMessage(string text, double dVal)\r
+{\r
+ Message *msg = new Message();\r
+ msg->text = text;\r
+ msg->messageId = messageId++;\r
+ msg->subsystem = "FDM";\r
+ msg->type = Message::eDouble;\r
+ msg->bVal = (dVal != 0.0);\r
+ Messages.push(msg);\r
+ return msg;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGJSBBase::Message* FGJSBBase::ReadMessage(void)\r
+{\r
+ if (!Messages.empty()) return Messages.front();\r
+ else return NULL;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGJSBBase::Message* FGJSBBase::ProcessMessage(void)\r
+{\r
+ if (!Messages.empty())\r
+ localMsg = *(Messages.front());\r
+ else\r
+ return NULL;\r
+ Messages.pop();\r
+ return &localMsg;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGJSBBase::disableHighLighting(void) {\r
+ highint[0]='\0';\r
+ halfint[0]='\0';\r
+ normint[0]='\0';\r
+ reset[0]='\0';\r
+ underon[0]='\0';\r
+ underoff[0]='\0';\r
+ fgblue[0]='\0';\r
+ fgcyan[0]='\0';\r
+ fgred[0]='\0';\r
+ fggreen[0]='\0';\r
+ fgdef[0]='\0';\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+} // namespace JSBSim\r
+\r
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;
};
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
- Module: FGLGear.cpp
- Author: Jon S. Berndt
- Norman H. Princen
- Date started: 11/18/99
- Purpose: Encapsulates the landing gear elements
- Called by: FGAircraft
-
- ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
-
- This program is free software; you can redistribute it and/or modify it under
- the terms of the GNU General Public License as published by the Free Software
- Foundation; either version 2 of the License, or (at your option) any later
- version.
-
- This program is distributed in the hope that it will be useful, but WITHOUT
- ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
- FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
- details.
-
- You should have received a copy of the GNU General Public License along with
- this program; if not, write to the Free Software Foundation, Inc., 59 Temple
- Place - Suite 330, Boston, MA 02111-1307, USA.
-
- Further information about the GNU General Public License can also be found on
- the world wide web at http://www.gnu.org.
-
-FUNCTIONAL DESCRIPTION
---------------------------------------------------------------------------------
-
-HISTORY
---------------------------------------------------------------------------------
-11/18/99 JSB Created
-01/30/01 NHP Extended gear model to properly simulate steering and braking
-
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-INCLUDES
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-
-#include "FGLGear.h"
-//#include <algorithm>
-
-namespace JSBSim {
-
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-DEFINITIONS
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-GLOBAL DATA
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-
-static const char *IdSrc = "$Id$";
-static const char *IdHdr = ID_LGEAR;
-
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-CLASS IMPLEMENTATION
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-
-FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
-{
- string tmp;
-
- *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
- >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
- >> rollingFCoeff >> sSteerType >> sBrakeGroup
- >> maxSteerAngle >> sRetractable;
-
- if (sBrakeGroup == "LEFT" ) eBrakeGrp = bgLeft;
- else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight;
- else if (sBrakeGroup == "CENTER") eBrakeGrp = bgCenter;
- else if (sBrakeGroup == "NOSE" ) eBrakeGrp = bgNose;
- else if (sBrakeGroup == "TAIL" ) eBrakeGrp = bgTail;
- else if (sBrakeGroup == "NONE" ) eBrakeGrp = bgNone;
- else {
- cerr << "Improper braking group specification in config file: "
- << sBrakeGroup << " is undefined." << endl;
- }
-
- if (sSteerType == "STEERABLE") eSteerType = stSteer;
- else if (sSteerType == "FIXED" ) eSteerType = stFixed;
- else if (sSteerType == "CASTERED" ) eSteerType = stCaster;
- else {
- cerr << "Improper steering type specification in config file: "
- << sSteerType << " is undefined." << endl;
- }
-
- if ( sRetractable == "RETRACT" ) {
- isRetractable = true;
- } else {
- isRetractable = false;
- }
-
- GearUp = false;
- GearDown = true;
- Servicable = true;
-
-// Add some AI here to determine if gear is located properly according to its
-// brake group type ??
-
- State = Exec->GetState();
- Aircraft = Exec->GetAircraft();
- Position = Exec->GetPosition();
- Rotation = Exec->GetRotation();
- FCS = Exec->GetFCS();
- MassBalance = Exec->GetMassBalance();
-
- WOW = lastWOW = true; // should the value be initialized to true?
- ReportEnable = true;
- FirstContact = false;
- StartedGroundRun = false;
- TakeoffReported = LandingReported = false;
- LandingDistanceTraveled = TakeoffDistanceTraveled = TakeoffDistanceTraveled50ft = 0.0;
- MaximumStrutForce = MaximumStrutTravel = 0.0;
- SinkRate = GroundSpeed = 0.0;
-
- vWhlBodyVec = MassBalance->StructuralToBody(vXYZ);
-
- vLocalGear = State->GetTb2l() * vWhlBodyVec;
-
- compressLength = 0.0;
- compressSpeed = 0.0;
- brakePct = 0.0;
- maxCompLen = 0.0;
-
- WheelSlip = lastWheelSlip = 0.0;
-
- compressLength = 0.0;
- compressSpeed = 0.0;
- brakePct = 0.0;
- maxCompLen = 0.0;
-
- TirePressureNorm = 1.0;
-
- Debug(0);
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-FGLGear::FGLGear(const FGLGear& lgear)
-{
- State = lgear.State;
- Aircraft = lgear.Aircraft;
- Position = lgear.Position;
- Rotation = lgear.Rotation;
- Exec = lgear.Exec;
- FCS = lgear.FCS;
- MassBalance = lgear.MassBalance;
-
- vXYZ = lgear.vXYZ;
- vMoment = lgear.vMoment;
- vWhlBodyVec = lgear.vWhlBodyVec;
- vLocalGear = lgear.vLocalGear;
-
- WOW = lgear.WOW;
- lastWOW = lgear.lastWOW;
- ReportEnable = lgear.ReportEnable;
- FirstContact = lgear.FirstContact;
- StartedGroundRun = lgear.StartedGroundRun;
- LandingDistanceTraveled = lgear.LandingDistanceTraveled;
- TakeoffDistanceTraveled = lgear.TakeoffDistanceTraveled;
- TakeoffDistanceTraveled50ft = lgear.TakeoffDistanceTraveled50ft;
- MaximumStrutForce = lgear.MaximumStrutForce;
- MaximumStrutTravel = lgear.MaximumStrutTravel;
-
- kSpring = lgear.kSpring;
- bDamp = lgear.bDamp;
- compressLength = lgear.compressLength;
- compressSpeed = lgear.compressSpeed;
- staticFCoeff = lgear.staticFCoeff;
- dynamicFCoeff = lgear.dynamicFCoeff;
- rollingFCoeff = lgear.rollingFCoeff;
- brakePct = lgear.brakePct;
- maxCompLen = lgear.maxCompLen;
- SinkRate = lgear.SinkRate;
- GroundSpeed = lgear.GroundSpeed;
- LandingReported = lgear.LandingReported;
- TakeoffReported = lgear.TakeoffReported;
- name = lgear.name;
- sSteerType = lgear.sSteerType;
- sRetractable = lgear.sRetractable;
- eSteerType = lgear.eSteerType;
- sBrakeGroup = lgear.sBrakeGroup;
- eBrakeGrp = lgear.eBrakeGrp;
- maxSteerAngle = lgear.maxSteerAngle;
- isRetractable = lgear.isRetractable;
- GearUp = lgear.GearUp;
- GearDown = lgear.GearDown;
- WheelSlip = lgear.WheelSlip;
- lastWheelSlip = lgear.lastWheelSlip;
- TirePressureNorm = lgear.TirePressureNorm;
- Servicable = lgear.Servicable;
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-FGLGear::~FGLGear()
-{
- Debug(1);
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-FGColumnVector3& FGLGear::Force(void)
-{
- double SteerGain = 0;
- double SinWheel, CosWheel;
- double deltaSlip;
- double deltaT = State->Getdt()*Aircraft->GetRate();
- double maxdeltaSlip = 0.5*deltaT;
-
- vForce.InitMatrix();
- vMoment.InitMatrix();
-
- if (isRetractable) {
- if (FCS->GetGearPos() < 0.01) {
- GearUp = true;
- GearDown = false;
- } else if (FCS->GetGearPos() > 0.99) {
- GearDown = true;
- GearUp = false;
- } else {
- GearUp = false;
- GearDown = false;
- }
- } else {
- GearUp = false;
- GearDown = true;
- }
-
- if (GearDown) {
-
- vWhlBodyVec = MassBalance->StructuralToBody(vXYZ);
-
-// vWhlBodyVec now stores the vector from the cg to this wheel
-
- vLocalGear = State->GetTb2l() * vWhlBodyVec;
-
-// vLocalGear now stores the vector from the cg to the wheel in local coords.
-
- compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();
-
-// The compression length is currently measured in the Z-axis, only, at this time.
-// It should be measured along the strut axis. If the local-frame gear position
-// "hangs down" below the CG greater than the altitude, then the compressLength
-// will be positive - i.e. the gear will have made contact.
-
- if (compressLength > 0.00) {
-
- WOW = true; // Weight-On-Wheels is true
-
-// The next equation should really use the vector to the contact patch of the tire
-// including the strut compression and not vWhlBodyVec. Will fix this later.
-// As it stands, now, the following equation takes the aircraft body-frame
-// rotational rate and calculates the cross-product with the vector from the CG
-// to the wheel, thus producing the instantaneous velocity vector of the tire
-// in Body coords. The frame is also converted to local coordinates. When the
-// aircraft local-frame velocity is added to this quantity, the total velocity of
-// the wheel in local frame is then known. Subsequently, the compression speed
-// (used for calculating damping force) is found by taking the Z-component of the
-// wheel velocity.
-
- vWhlVelVec = State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
-
- vWhlVelVec += Position->GetVel();
-
- compressSpeed = vWhlVelVec(eZ);
-
-// If this is the first time the wheel has made contact, remember some values
-// for later printout.
-
- if (!FirstContact) {
- FirstContact = true;
- SinkRate = compressSpeed;
- GroundSpeed = Position->GetVel().Magnitude();
- TakeoffReported = false;
- }
-
-// If the takeoff run is starting, initialize.
-
- if ((Position->GetVel().Magnitude() > 0.1) &&
- (FCS->GetBrake(bgLeft) == 0) &&
- (FCS->GetBrake(bgRight) == 0) &&
- (FCS->GetThrottlePos(0) == 1) && !StartedGroundRun)
- {
- TakeoffDistanceTraveled = 0;
- TakeoffDistanceTraveled50ft = 0;
- StartedGroundRun = true;
- }
-
-// The following needs work regarding friction coefficients and braking and
-// steering The BrakeFCoeff formula assumes that an anti-skid system is used.
-// It also assumes that we won't be turning and braking at the same time.
-// Will fix this later.
-// [JSB] The braking force coefficients include normal rolling coefficient +
-// a percentage of the static friction coefficient based on braking applied.
-
- switch (eBrakeGrp) {
- case bgLeft:
- BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
- staticFCoeff*FCS->GetBrake(bgLeft) );
- break;
- case bgRight:
- BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
- staticFCoeff*FCS->GetBrake(bgRight) );
- break;
- case bgCenter:
- BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
- staticFCoeff*FCS->GetBrake(bgCenter) );
- break;
- case bgNose:
- BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
- staticFCoeff*FCS->GetBrake(bgCenter) );
- break;
- case bgTail:
- BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
- staticFCoeff*FCS->GetBrake(bgCenter) );
- break;
- case bgNone:
- BrakeFCoeff = rollingFCoeff;
- break;
- default:
- cerr << "Improper brake group membership detected for this gear." << endl;
- break;
- }
-
- switch (eSteerType) {
- case stSteer:
- SteerAngle = -maxSteerAngle * FCS->GetDrCmd() * 0.01745;
- break;
- case stFixed:
- SteerAngle = 0.0;
- break;
- case stCaster:
-// Note to Jon: This is not correct for castering gear. I'll fix it later.
- SteerAngle = 0.0;
- break;
- default:
- cerr << "Improper steering type membership detected for this gear." << endl;
- break;
- }
-
-// Transform the wheel velocities from the local axis system to the wheel axis system.
-// For now, steering angle is assumed to happen in the Local Z axis,
-// not the strut axis as it should be. Will fix this later.
-
- SinWheel = sin(Rotation->Getpsi() + SteerAngle);
- CosWheel = cos(Rotation->Getpsi() + SteerAngle);
- RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel;
- SideWhlVel = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel;
-
-// Calculate tire slip angle.
-
- if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
- WheelSlip = 0.0;
- } else if (fabs(RollingWhlVel) < 1.0) {
- WheelSlip = 0.05*radtodeg*atan2(SideWhlVel, RollingWhlVel) + 0.95*WheelSlip;
- } else {
- WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
- }
-/*
- if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
- WheelSlip = 0.0;
- } else if (RollingWhlVel < 1.0) {
- WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
- deltaSlip = WheelSlip - lastWheelSlip;
- if (fabs(deltaSlip) > maxdeltaSlip) {
- if (WheelSlip > lastWheelSlip) {
- WheelSlip = lastWheelSlip + maxdeltaSlip;
- } else if (WheelSlip < lastWheelSlip) {
- WheelSlip = lastWheelSlip - maxdeltaSlip;
- }
- }
- } else {
- WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
- }
-
- if ((WheelSlip < 0.0 && lastWheelSlip > 0.0) ||
- (WheelSlip > 0.0 && lastWheelSlip < 0.0))
- {
- WheelSlip = 0.0;
- }
-*/
- lastWheelSlip = WheelSlip;
-
-// Compute the sideforce coefficients using similar assumptions to LaRCSim for now.
-// Allow a maximum of 10 degrees tire slip angle before wheel slides. At that point,
-// transition from static to dynamic friction. There are more complicated formulations
-// of this that avoid the discrete jump. Will fix this later.
-
- if (fabs(WheelSlip) <= 20.0) {
- FCoeff = staticFCoeff*WheelSlip/20.0;
- } else if (fabs(WheelSlip) <= 40.0) {
-// FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
- FCoeff = (dynamicFCoeff*(fabs(WheelSlip) - 20.0)/20.0 +
- staticFCoeff*(40.0 - fabs(WheelSlip))/20.0)*fabs(WheelSlip)/WheelSlip;
- } else {
- FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
- }
-
-// Compute the vertical force on the wheel using square-law damping (per comment
-// in paper AIAA-2000-4303 - see header prologue comments). We might consider
-// allowing for both square and linear damping force calculation. Also need to
-// possibly give a "rebound damping factor" that differs from the compression
-// case.
-
- vLocalForce(eZ) = min(-compressLength * kSpring
- - compressSpeed * bDamp, (double)0.0);
-
- MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ)));
- MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
-
-// Compute the forces in the wheel ground plane.
-
- RollingForce = 0;
- if (fabs(RollingWhlVel) > 1E-3) {
- RollingForce = (1.0 - TirePressureNorm) * 30
- + vLocalForce(eZ) * BrakeFCoeff
- * fabs(RollingWhlVel)/RollingWhlVel;
- }
- SideForce = vLocalForce(eZ) * FCoeff;
-
-// Transform these forces back to the local reference frame.
-
- vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel;
- vLocalForce(eY) = SideForce*CosWheel + RollingForce*SinWheel;
-
-// Note to Jon: At this point the forces will be too big when the airplane is
-// stopped or rolling to a stop. We need to make sure that the gear forces just
-// balance out the non-gear forces when the airplane is stopped. That way the
-// airplane won't start to accelerate until the non-gear/ forces are larger than
-// the gear forces. I think that the proper fix should go into FGAircraft::FMGear.
-// This routine would only compute the local strut forces and return them to
-// FMGear. All of the gear forces would get adjusted in FMGear using the total
-// non-gear forces. Then the gear moments would be calculated. If strange things
-// start happening to the airplane during testing as it rolls to a stop, then we
-// need to implement this change. I ran out of time to do it now but have the
-// equations.
-
-// Transform the forces back to the body frame and compute the moment.
-
- vForce = State->GetTl2b() * vLocalForce;
- vMoment = vWhlBodyVec * vForce;
-
- } else { // Gear is NOT compressed
-
- WOW = false;
-
- if (Position->GetDistanceAGL() > 200.0) {
- FirstContact = false;
- StartedGroundRun = false;
- LandingReported = false;
- LandingDistanceTraveled = 0.0;
- MaximumStrutForce = MaximumStrutTravel = 0.0;
- }
-
- compressLength = 0.0; // reset compressLength to zero for data output validity
- }
-
- if (FirstContact) LandingDistanceTraveled += Position->GetVground()*deltaT;
-
- if (StartedGroundRun) {
- TakeoffDistanceTraveled50ft += Position->GetVground()*deltaT;
- if (WOW) TakeoffDistanceTraveled += Position->GetVground()*deltaT;
- }
-
- if (ReportEnable && Position->GetVground() <= 0.05 && !LandingReported) {
- if (debug_lvl > 0) Report(erLand);
- }
-
- if (ReportEnable && !TakeoffReported &&
- (vLocalGear(eZ) - Position->GetDistanceAGL()) < -50.0)
- {
- if (debug_lvl > 0) Report(erTakeoff);
- }
-
- if (lastWOW != WOW) {
- PutMessage("GEAR_CONTACT: " + name, WOW);
- }
-
- lastWOW = WOW;
-
-// Crash detection logic (really out-of-bounds detection)
-
- if (compressLength > 500.0 ||
- vForce.Magnitude() > 100000000.0 ||
- vMoment.Magnitude() > 5000000000.0 ||
- SinkRate > 1.4666*30)
- {
- PutMessage("Crash Detected: Simulation FREEZE.");
- Exec->Freeze();
- }
- }
- return vForce;
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-void FGLGear::Report(ReportType repType)
-{
- switch(repType) {
- case erLand:
- cout << endl << "Touchdown report for " << name << endl;
- cout << " Sink rate at contact: " << SinkRate << " fps, "
- << SinkRate*0.3048 << " mps" << endl;
- cout << " Contact ground speed: " << GroundSpeed*.5925 << " knots, "
- << GroundSpeed*0.3048 << " mps" << endl;
- cout << " Maximum contact force: " << MaximumStrutForce << " lbs, "
- << MaximumStrutForce*4.448 << " Newtons" << endl;
- cout << " Maximum strut travel: " << MaximumStrutTravel*12.0 << " inches, "
- << MaximumStrutTravel*30.48 << " cm" << endl;
- cout << " Distance traveled: " << LandingDistanceTraveled << " ft, "
- << LandingDistanceTraveled*0.3048 << " meters" << endl;
- LandingReported = true;
- break;
- case erTakeoff:
- cout << endl << "Takeoff report for " << name << endl;
- cout << " Distance traveled: " << TakeoffDistanceTraveled
- << " ft, " << TakeoffDistanceTraveled*0.3048 << " meters" << endl;
- cout << " Distance traveled (over 50'): " << TakeoffDistanceTraveled50ft
- << " ft, " << TakeoffDistanceTraveled50ft*0.3048 << " meters" << endl;
- TakeoffReported = true;
- break;
- }
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-// The bitmasked value choices are as follows:
-// unset: In this case (the default) JSBSim would only print
-// out the normally expected messages, essentially echoing
-// the config files as they are read. If the environment
-// variable is not set, debug_lvl is set to 1 internally
-// 0: This requests JSBSim not to output any messages
-// whatsoever.
-// 1: This value explicity requests the normal JSBSim
-// startup messages
-// 2: This value asks for a message to be printed out when
-// a class is instantiated
-// 4: When this value is set, a message is displayed when a
-// FGModel object executes its Run() method
-// 8: When this value is set, various runtime state variables
-// are printed out periodically
-// 16: When set various parameters are sanity checked and
-// a message is printed out when they go out of bounds
-
-void FGLGear::Debug(int from)
-{
- if (debug_lvl <= 0) return;
-
- if (debug_lvl & 1) { // Standard console startup message output
- if (from == 0) { // Constructor
- cout << " Name: " << name << endl;
- cout << " Location: " << vXYZ << endl;
- cout << " Spring Constant: " << kSpring << endl;
- cout << " Damping Constant: " << bDamp << endl;
- cout << " Dynamic Friction: " << dynamicFCoeff << endl;
- cout << " Static Friction: " << staticFCoeff << endl;
- cout << " Rolling Friction: " << rollingFCoeff << endl;
- cout << " Steering Type: " << sSteerType << endl;
- cout << " Grouping: " << sBrakeGroup << endl;
- cout << " Max Steer Angle: " << maxSteerAngle << endl;
- cout << " Retractable: " << sRetractable << endl;
- }
- }
- if (debug_lvl & 2 ) { // Instantiation/Destruction notification
- if (from == 0) cout << "Instantiated: FGLGear" << endl;
- if (from == 1) cout << "Destroyed: FGLGear" << endl;
- }
- if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
- }
- if (debug_lvl & 8 ) { // Runtime state variables
- }
- if (debug_lvl & 16) { // Sanity checking
- }
- if (debug_lvl & 64) {
- if (from == 0) { // Constructor
- cout << IdSrc << endl;
- cout << IdHdr << endl;
- }
- }
-}
-
-} // namespace JSBSim
-
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+ Module: FGLGear.cpp\r
+ Author: Jon S. Berndt\r
+ Norman H. Princen\r
+ Date started: 11/18/99\r
+ Purpose: Encapsulates the landing gear elements\r
+ Called by: FGAircraft\r
+\r
+ ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------\r
+\r
+ This program is free software; you can redistribute it and/or modify it under\r
+ the terms of the GNU General Public License as published by the Free Software\r
+ Foundation; either version 2 of the License, or (at your option) any later\r
+ version.\r
+\r
+ This program is distributed in the hope that it will be useful, but WITHOUT\r
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS\r
+ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more\r
+ details.\r
+\r
+ You should have received a copy of the GNU General Public License along with\r
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple\r
+ Place - Suite 330, Boston, MA 02111-1307, USA.\r
+\r
+ Further information about the GNU General Public License can also be found on\r
+ the world wide web at http://www.gnu.org.\r
+\r
+FUNCTIONAL DESCRIPTION\r
+--------------------------------------------------------------------------------\r
+\r
+HISTORY\r
+--------------------------------------------------------------------------------\r
+11/18/99 JSB Created\r
+01/30/01 NHP Extended gear model to properly simulate steering and braking\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+INCLUDES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+#include "FGLGear.h"\r
+\r
+namespace JSBSim {\r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+DEFINITIONS\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+GLOBAL DATA\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+static const char *IdSrc = "$Id$";\r
+static const char *IdHdr = ID_LGEAR;\r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+CLASS IMPLEMENTATION\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)\r
+{\r
+ string tmp;\r
+ \r
+ *AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)\r
+ >> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff\r
+ >> rollingFCoeff >> sSteerType >> sBrakeGroup \r
+ >> maxSteerAngle >> sRetractable;\r
+\r
+ if (sBrakeGroup == "LEFT" ) eBrakeGrp = bgLeft;\r
+ else if (sBrakeGroup == "RIGHT" ) eBrakeGrp = bgRight;\r
+ else if (sBrakeGroup == "CENTER") eBrakeGrp = bgCenter;\r
+ else if (sBrakeGroup == "NOSE" ) eBrakeGrp = bgNose;\r
+ else if (sBrakeGroup == "TAIL" ) eBrakeGrp = bgTail;\r
+ else if (sBrakeGroup == "NONE" ) eBrakeGrp = bgNone;\r
+ else {\r
+ cerr << "Improper braking group specification in config file: "\r
+ << sBrakeGroup << " is undefined." << endl;\r
+ }\r
+\r
+ if (sSteerType == "STEERABLE") eSteerType = stSteer;\r
+ else if (sSteerType == "FIXED" ) eSteerType = stFixed;\r
+ else if (sSteerType == "CASTERED" ) eSteerType = stCaster;\r
+ else {\r
+ cerr << "Improper steering type specification in config file: "\r
+ << sSteerType << " is undefined." << endl;\r
+ }\r
+ \r
+ if ( sRetractable == "RETRACT" ) {\r
+ isRetractable = true;\r
+ } else {\r
+ isRetractable = false;\r
+ } \r
+ \r
+ GearUp = false;\r
+ GearDown = true;\r
+ Servicable = true;\r
+\r
+// Add some AI here to determine if gear is located properly according to its\r
+// brake group type ??\r
+\r
+ State = Exec->GetState();\r
+ Aircraft = Exec->GetAircraft();\r
+ Position = Exec->GetPosition();\r
+ Rotation = Exec->GetRotation();\r
+ FCS = Exec->GetFCS();\r
+ MassBalance = Exec->GetMassBalance();\r
+\r
+ WOW = lastWOW = true; // should the value be initialized to true?\r
+ ReportEnable = true;\r
+ FirstContact = false;\r
+ StartedGroundRun = false;\r
+ TakeoffReported = LandingReported = false;\r
+ LandingDistanceTraveled = TakeoffDistanceTraveled = TakeoffDistanceTraveled50ft = 0.0;\r
+ MaximumStrutForce = MaximumStrutTravel = 0.0;\r
+ SideForce = RollingForce = 0.0;\r
+ SinkRate = GroundSpeed = 0.0;\r
+\r
+ vWhlBodyVec = MassBalance->StructuralToBody(vXYZ);\r
+ \r
+ vLocalGear = State->GetTb2l() * vWhlBodyVec;\r
+\r
+ compressLength = 0.0;\r
+ compressSpeed = 0.0;\r
+ brakePct = 0.0;\r
+ maxCompLen = 0.0;\r
+\r
+ WheelSlip = lastWheelSlip = 0.0;\r
+\r
+ compressLength = 0.0;\r
+ compressSpeed = 0.0;\r
+ brakePct = 0.0;\r
+ maxCompLen = 0.0;\r
+\r
+ TirePressureNorm = 1.0;\r
+\r
+ Debug(0);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGLGear::FGLGear(const FGLGear& lgear)\r
+{\r
+ State = lgear.State;\r
+ Aircraft = lgear.Aircraft;\r
+ Position = lgear.Position;\r
+ Rotation = lgear.Rotation;\r
+ Exec = lgear.Exec;\r
+ FCS = lgear.FCS;\r
+ MassBalance = lgear.MassBalance;\r
+\r
+ vXYZ = lgear.vXYZ;\r
+ vMoment = lgear.vMoment;\r
+ vWhlBodyVec = lgear.vWhlBodyVec;\r
+ vLocalGear = lgear.vLocalGear;\r
+\r
+ WOW = lgear.WOW;\r
+ lastWOW = lgear.lastWOW;\r
+ ReportEnable = lgear.ReportEnable;\r
+ FirstContact = lgear.FirstContact;\r
+ StartedGroundRun = lgear.StartedGroundRun;\r
+ LandingDistanceTraveled = lgear.LandingDistanceTraveled;\r
+ TakeoffDistanceTraveled = lgear.TakeoffDistanceTraveled;\r
+ TakeoffDistanceTraveled50ft = lgear.TakeoffDistanceTraveled50ft;\r
+ MaximumStrutForce = lgear.MaximumStrutForce;\r
+ MaximumStrutTravel = lgear.MaximumStrutTravel;\r
+ SideForce = lgear.SideForce;\r
+ RollingForce = lgear.RollingForce;\r
+\r
+ kSpring = lgear.kSpring;\r
+ bDamp = lgear.bDamp;\r
+ compressLength = lgear.compressLength;\r
+ compressSpeed = lgear.compressSpeed;\r
+ staticFCoeff = lgear.staticFCoeff;\r
+ dynamicFCoeff = lgear.dynamicFCoeff;\r
+ rollingFCoeff = lgear.rollingFCoeff;\r
+ brakePct = lgear.brakePct;\r
+ maxCompLen = lgear.maxCompLen;\r
+ SinkRate = lgear.SinkRate;\r
+ GroundSpeed = lgear.GroundSpeed;\r
+ LandingReported = lgear.LandingReported;\r
+ TakeoffReported = lgear.TakeoffReported;\r
+ name = lgear.name;\r
+ sSteerType = lgear.sSteerType;\r
+ sRetractable = lgear.sRetractable;\r
+ eSteerType = lgear.eSteerType;\r
+ sBrakeGroup = lgear.sBrakeGroup;\r
+ eBrakeGrp = lgear.eBrakeGrp;\r
+ maxSteerAngle = lgear.maxSteerAngle;\r
+ isRetractable = lgear.isRetractable;\r
+ GearUp = lgear.GearUp;\r
+ GearDown = lgear.GearDown;\r
+ WheelSlip = lgear.WheelSlip;\r
+ lastWheelSlip = lgear.lastWheelSlip;\r
+ TirePressureNorm = lgear.TirePressureNorm;\r
+ Servicable = lgear.Servicable;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGLGear::~FGLGear()\r
+{\r
+ Debug(1);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGColumnVector3& FGLGear::Force(void)\r
+{\r
+ double SteerGain = 0;\r
+ double SinWheel, CosWheel;\r
+ double deltaSlip;\r
+ double deltaT = State->Getdt()*Aircraft->GetRate();\r
+\r
+ vForce.InitMatrix();\r
+ vMoment.InitMatrix();\r
+\r
+ if (isRetractable) {\r
+ if (FCS->GetGearPos() < 0.01) {\r
+ GearUp = true;\r
+ GearDown = false;\r
+ } else if (FCS->GetGearPos() > 0.99) {\r
+ GearDown = true;\r
+ GearUp = false;\r
+ } else {\r
+ GearUp = false;\r
+ GearDown = false;\r
+ }\r
+ } else {\r
+ GearUp = false;\r
+ GearDown = true;\r
+ } \r
+ \r
+ if (GearDown) {\r
+\r
+ vWhlBodyVec = MassBalance->StructuralToBody(vXYZ);\r
+\r
+// vWhlBodyVec now stores the vector from the cg to this wheel\r
+\r
+ vLocalGear = State->GetTb2l() * vWhlBodyVec;\r
+\r
+// vLocalGear now stores the vector from the cg to the wheel in local coords.\r
+\r
+ compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();\r
+\r
+// The compression length is currently measured in the Z-axis, only, at this time.\r
+// It should be measured along the strut axis. If the local-frame gear position\r
+// "hangs down" below the CG greater than the altitude, then the compressLength\r
+// will be positive - i.e. the gear will have made contact.\r
+\r
+ if (compressLength > 0.00) {\r
+\r
+ WOW = true; // Weight-On-Wheels is true\r
+\r
+// The next equation should really use the vector to the contact patch of the tire\r
+// including the strut compression and not vWhlBodyVec. Will fix this later.\r
+// As it stands, now, the following equation takes the aircraft body-frame\r
+// rotational rate and calculates the cross-product with the vector from the CG\r
+// to the wheel, thus producing the instantaneous velocity vector of the tire\r
+// in Body coords. The frame is also converted to local coordinates. When the\r
+// aircraft local-frame velocity is added to this quantity, the total velocity of\r
+// the wheel in local frame is then known. Subsequently, the compression speed\r
+// (used for calculating damping force) is found by taking the Z-component of the\r
+// wheel velocity.\r
+\r
+ vWhlVelVec = State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);\r
+ vWhlVelVec += Position->GetVel();\r
+ compressSpeed = vWhlVelVec(eZ);\r
+\r
+// If this is the first time the wheel has made contact, remember some values\r
+// for later printout.\r
+\r
+ if (!FirstContact) {\r
+ FirstContact = true;\r
+ SinkRate = compressSpeed;\r
+ GroundSpeed = Position->GetVel().Magnitude();\r
+ TakeoffReported = false;\r
+ }\r
+\r
+// If the takeoff run is starting, initialize.\r
+\r
+ if ((Position->GetVel().Magnitude() > 0.1) &&\r
+ (FCS->GetBrake(bgLeft) == 0) &&\r
+ (FCS->GetBrake(bgRight) == 0) &&\r
+ (FCS->GetThrottlePos(0) == 1) && !StartedGroundRun)\r
+ {\r
+ TakeoffDistanceTraveled = 0;\r
+ TakeoffDistanceTraveled50ft = 0;\r
+ StartedGroundRun = true;\r
+ }\r
+\r
+// The following needs work regarding friction coefficients and braking and\r
+// steering The BrakeFCoeff formula assumes that an anti-skid system is used.\r
+// It also assumes that we won't be turning and braking at the same time.\r
+// Will fix this later.\r
+// [JSB] The braking force coefficients include normal rolling coefficient +\r
+// a percentage of the static friction coefficient based on braking applied.\r
+\r
+ switch (eBrakeGrp) {\r
+ case bgLeft:\r
+ BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +\r
+ staticFCoeff*FCS->GetBrake(bgLeft) );\r
+ break;\r
+ case bgRight:\r
+ BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +\r
+ staticFCoeff*FCS->GetBrake(bgRight) );\r
+ break;\r
+ case bgCenter:\r
+ BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +\r
+ staticFCoeff*FCS->GetBrake(bgCenter) );\r
+ break;\r
+ case bgNose:\r
+ BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +\r
+ staticFCoeff*FCS->GetBrake(bgCenter) );\r
+ break;\r
+ case bgTail:\r
+ BrakeFCoeff = ( rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +\r
+ staticFCoeff*FCS->GetBrake(bgCenter) );\r
+ break;\r
+ case bgNone:\r
+ BrakeFCoeff = rollingFCoeff;\r
+ break;\r
+ default:\r
+ cerr << "Improper brake group membership detected for this gear." << endl;\r
+ break;\r
+ }\r
+\r
+ switch (eSteerType) {\r
+ case stSteer:\r
+ SteerAngle = -maxSteerAngle * FCS->GetDrCmd() * 0.01745; \r
+ break;\r
+ case stFixed:\r
+ SteerAngle = 0.0;\r
+ break;\r
+ case stCaster:\r
+// Note to Jon: This is not correct for castering gear. I'll fix it later.\r
+ SteerAngle = 0.0;\r
+ break;\r
+ default:\r
+ cerr << "Improper steering type membership detected for this gear." << endl;\r
+ break;\r
+ }\r
+\r
+// Transform the wheel velocities from the local axis system to the wheel axis system.\r
+// For now, steering angle is assumed to happen in the Local Z axis,\r
+// not the strut axis as it should be. Will fix this later.\r
+\r
+ SinWheel = sin(Rotation->Getpsi() + SteerAngle);\r
+ CosWheel = cos(Rotation->Getpsi() + SteerAngle);\r
+ RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel;\r
+ SideWhlVel = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel;\r
+\r
+// Calculate tire slip angle.\r
+\r
+ if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {\r
+ WheelSlip = 0.0;\r
+ } else if (fabs(RollingWhlVel) < 1.0) {\r
+ WheelSlip = 0.05*radtodeg*atan2(SideWhlVel, RollingWhlVel) + 0.95*WheelSlip;\r
+ } else {\r
+ WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);\r
+ }\r
+/*\r
+ double maxdeltaSlip = 0.5*deltaT;\r
+\r
+ if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {\r
+ WheelSlip = 0.0;\r
+ } else if (RollingWhlVel < 1.0) {\r
+ WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);\r
+ deltaSlip = WheelSlip - lastWheelSlip;\r
+ if (fabs(deltaSlip) > maxdeltaSlip) {\r
+ if (WheelSlip > lastWheelSlip) {\r
+ WheelSlip = lastWheelSlip + maxdeltaSlip;\r
+ } else if (WheelSlip < lastWheelSlip) {\r
+ WheelSlip = lastWheelSlip - maxdeltaSlip;\r
+ }\r
+ }\r
+ } else {\r
+ WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);\r
+ }\r
+\r
+ if ((WheelSlip < 0.0 && lastWheelSlip > 0.0) ||\r
+ (WheelSlip > 0.0 && lastWheelSlip < 0.0))\r
+ {\r
+ WheelSlip = 0.0;\r
+ }\r
+*/ \r
+ lastWheelSlip = WheelSlip;\r
+\r
+// Compute the sideforce coefficients using similar assumptions to LaRCSim for now.\r
+// Allow a maximum of 10 degrees tire slip angle before wheel slides. At that point,\r
+// transition from static to dynamic friction. There are more complicated formulations\r
+// of this that avoid the discrete jump. Will fix this later.\r
+\r
+ if (fabs(WheelSlip) <= 20.0) {\r
+ FCoeff = staticFCoeff*WheelSlip/20.0;\r
+ } else if (fabs(WheelSlip) <= 40.0) {\r
+// FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;\r
+ FCoeff = (dynamicFCoeff*(fabs(WheelSlip) - 20.0)/20.0 + \r
+ staticFCoeff*(40.0 - fabs(WheelSlip))/20.0)*fabs(WheelSlip)/WheelSlip;\r
+ } else {\r
+ FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;\r
+ }\r
+\r
+// Compute the vertical force on the wheel using square-law damping (per comment\r
+// in paper AIAA-2000-4303 - see header prologue comments). We might consider\r
+// allowing for both square and linear damping force calculation. Also need to\r
+// possibly give a "rebound damping factor" that differs from the compression\r
+// case.\r
+\r
+ vLocalForce(eZ) = min(-compressLength * kSpring\r
+ - compressSpeed * bDamp, (double)0.0);\r
+\r
+ MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ)));\r
+ MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));\r
+\r
+// Compute the forces in the wheel ground plane.\r
+\r
+ RollingForce = 0;\r
+ if (fabs(RollingWhlVel) > 1E-3) {\r
+ RollingForce = (1.0 - TirePressureNorm) * 30\r
+ + vLocalForce(eZ) * BrakeFCoeff\r
+ * fabs(RollingWhlVel)/RollingWhlVel;\r
+ }\r
+ SideForce = vLocalForce(eZ) * FCoeff;\r
+\r
+// Transform these forces back to the local reference frame.\r
+\r
+ vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel;\r
+ vLocalForce(eY) = SideForce*CosWheel + RollingForce*SinWheel;\r
+\r
+// Note to Jon: At this point the forces will be too big when the airplane is\r
+// stopped or rolling to a stop. We need to make sure that the gear forces just\r
+// balance out the non-gear forces when the airplane is stopped. That way the\r
+// airplane won't start to accelerate until the non-gear/ forces are larger than\r
+// the gear forces. I think that the proper fix should go into FGAircraft::FMGear.\r
+// This routine would only compute the local strut forces and return them to\r
+// FMGear. All of the gear forces would get adjusted in FMGear using the total\r
+// non-gear forces. Then the gear moments would be calculated. If strange things\r
+// start happening to the airplane during testing as it rolls to a stop, then we\r
+// need to implement this change. I ran out of time to do it now but have the\r
+// equations.\r
+\r
+// Transform the forces back to the body frame and compute the moment.\r
+\r
+ vForce = State->GetTl2b() * vLocalForce;\r
+ vMoment = vWhlBodyVec * vForce;\r
+\r
+ } else { // Gear is NOT compressed\r
+\r
+ WOW = false;\r
+\r
+ if (Position->GetDistanceAGL() > 200.0) {\r
+ FirstContact = false;\r
+ StartedGroundRun = false;\r
+ LandingReported = false;\r
+ LandingDistanceTraveled = 0.0;\r
+ MaximumStrutForce = MaximumStrutTravel = 0.0;\r
+ }\r
+\r
+ compressLength = 0.0; // reset compressLength to zero for data output validity\r
+ }\r
+\r
+ if (FirstContact) LandingDistanceTraveled += Position->GetVground()*deltaT;\r
+ \r
+ if (StartedGroundRun) {\r
+ TakeoffDistanceTraveled50ft += Position->GetVground()*deltaT;\r
+ if (WOW) TakeoffDistanceTraveled += Position->GetVground()*deltaT;\r
+ }\r
+\r
+ if (ReportEnable && Position->GetVground() <= 0.05 && !LandingReported) {\r
+ if (debug_lvl > 0) Report(erLand);\r
+ }\r
+\r
+ if (ReportEnable && !TakeoffReported &&\r
+ (vLocalGear(eZ) - Position->GetDistanceAGL()) < -50.0)\r
+ {\r
+ if (debug_lvl > 0) Report(erTakeoff);\r
+ }\r
+\r
+ if (lastWOW != WOW) {\r
+ PutMessage("GEAR_CONTACT: " + name, WOW);\r
+ }\r
+\r
+ lastWOW = WOW;\r
+\r
+// Crash detection logic (really out-of-bounds detection)\r
+\r
+ if (compressLength > 500.0 ||\r
+ vForce.Magnitude() > 100000000.0 ||\r
+ vMoment.Magnitude() > 5000000000.0 ||\r
+ SinkRate > 1.4666*30)\r
+ {\r
+ PutMessage("Crash Detected: Simulation FREEZE.");\r
+ Exec->Freeze();\r
+ }\r
+ } \r
+ return vForce; \r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGLGear::Report(ReportType repType)\r
+{\r
+ switch(repType) {\r
+ case erLand:\r
+ cout << endl << "Touchdown report for " << name << endl;\r
+ cout << " Sink rate at contact: " << SinkRate << " fps, "\r
+ << SinkRate*0.3048 << " mps" << endl;\r
+ cout << " Contact ground speed: " << GroundSpeed*.5925 << " knots, "\r
+ << GroundSpeed*0.3048 << " mps" << endl;\r
+ cout << " Maximum contact force: " << MaximumStrutForce << " lbs, "\r
+ << MaximumStrutForce*4.448 << " Newtons" << endl;\r
+ cout << " Maximum strut travel: " << MaximumStrutTravel*12.0 << " inches, "\r
+ << MaximumStrutTravel*30.48 << " cm" << endl;\r
+ cout << " Distance traveled: " << LandingDistanceTraveled << " ft, "\r
+ << LandingDistanceTraveled*0.3048 << " meters" << endl;\r
+ LandingReported = true;\r
+ break;\r
+ case erTakeoff:\r
+ cout << endl << "Takeoff report for " << name << endl;\r
+ cout << " Distance traveled: " << TakeoffDistanceTraveled\r
+ << " ft, " << TakeoffDistanceTraveled*0.3048 << " meters" << endl;\r
+ cout << " Distance traveled (over 50'): " << TakeoffDistanceTraveled50ft\r
+ << " ft, " << TakeoffDistanceTraveled50ft*0.3048 << " meters" << endl;\r
+ TakeoffReported = true;\r
+ break;\r
+ }\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+// The bitmasked value choices are as follows:\r
+// unset: In this case (the default) JSBSim would only print\r
+// out the normally expected messages, essentially echoing\r
+// the config files as they are read. If the environment\r
+// variable is not set, debug_lvl is set to 1 internally\r
+// 0: This requests JSBSim not to output any messages\r
+// whatsoever.\r
+// 1: This value explicity requests the normal JSBSim\r
+// startup messages\r
+// 2: This value asks for a message to be printed out when\r
+// a class is instantiated\r
+// 4: When this value is set, a message is displayed when a\r
+// FGModel object executes its Run() method\r
+// 8: When this value is set, various runtime state variables\r
+// are printed out periodically\r
+// 16: When set various parameters are sanity checked and\r
+// a message is printed out when they go out of bounds\r
+\r
+void FGLGear::Debug(int from)\r
+{\r
+ if (debug_lvl <= 0) return;\r
+\r
+ if (debug_lvl & 1) { // Standard console startup message output\r
+ if (from == 0) { // Constructor\r
+ cout << " Name: " << name << endl;\r
+ cout << " Location: " << vXYZ << endl;\r
+ cout << " Spring Constant: " << kSpring << endl;\r
+ cout << " Damping Constant: " << bDamp << endl;\r
+ cout << " Dynamic Friction: " << dynamicFCoeff << endl;\r
+ cout << " Static Friction: " << staticFCoeff << endl;\r
+ cout << " Rolling Friction: " << rollingFCoeff << endl;\r
+ cout << " Steering Type: " << sSteerType << endl;\r
+ cout << " Grouping: " << sBrakeGroup << endl;\r
+ cout << " Max Steer Angle: " << maxSteerAngle << endl;\r
+ cout << " Retractable: " << sRetractable << endl;\r
+ }\r
+ }\r
+ if (debug_lvl & 2 ) { // Instantiation/Destruction notification\r
+ if (from == 0) cout << "Instantiated: FGLGear" << endl;\r
+ if (from == 1) cout << "Destroyed: FGLGear" << endl;\r
+ }\r
+ if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects\r
+ }\r
+ if (debug_lvl & 8 ) { // Runtime state variables\r
+ }\r
+ if (debug_lvl & 16) { // Sanity checking\r
+ }\r
+ if (debug_lvl & 64) {\r
+ if (from == 0) { // Constructor\r
+ cout << IdSrc << endl;\r
+ cout << IdHdr << endl;\r
+ }\r
+ }\r
+}\r
+\r
+} // namespace JSBSim\r
+\r
# 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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGMassBalance.h"
+#include "FGPropulsion.h"
#include "FGPropertyManager.h"
namespace JSBSim {
{
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);
bool FGMassBalance::Run(void)
{
+ double denom, k1, k2, k3, k4, k5, k6;
+ double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
+
if (!FGModel::Run()) {
Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetPointMassWeight();
- Mass = Weight / Inertial->gravity();
+ Mass = lbtoslug*Weight;
-// Calculate new CG here.
+// Calculate new CG
vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg
+ GetPointMassMoment() ) / Weight;
-// Calculate new moments of inertia here
-
- Ixx = baseIxx + Propulsion->GetTanksIxx(vXYZcg) + GetPMIxx();
- Iyy = baseIyy + Propulsion->GetTanksIyy(vXYZcg) + GetPMIyy();
- Izz = baseIzz + Propulsion->GetTanksIzz(vXYZcg) + GetPMIzz();
- Ixy = baseIxy + Propulsion->GetTanksIxy(vXYZcg) + GetPMIxy();
- Ixz = baseIxz + Propulsion->GetTanksIxz(vXYZcg) + GetPMIxz();
+// Calculate new total moments of inertia
+
+ // At first it is the base configuration inertia matrix ...
+ mJ = baseJ;
+ // ... with the additional term originating from the parallel axis theorem.
+ mJ += GetPointmassInertia( lbtoslug * EmptyWeight, vbaseXYZcg );
+ // Then add the contributions from the additional pointmasses.
+ mJ += CalculatePMInertias();
+ mJ += Propulsion->CalculateTankInertias();
+
+ Ixx = mJ(1,1);
+ Iyy = mJ(2,2);
+ Izz = mJ(3,3);
+ Ixy = -mJ(1,2);
+ Ixz = -mJ(1,3);
+ Iyz = -mJ(2,3);
+
+// Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
+
+ k1 = (Iyy*Izz - Iyz*Iyz);
+ k2 = (Iyz*Ixz + Ixy*Izz);
+ k3 = (Ixy*Iyz + Iyy*Ixz);
+
+ denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 );
+ k1 = k1*denom;
+ k2 = k2*denom;
+ k3 = k3*denom;
+ k4 = (Izz*Ixx - Ixz*Ixz)*denom;
+ k5 = (Ixy*Ixz + Iyz*Ixx)*denom;
+ k6 = (Ixx*Iyy - Ixy*Ixy)*denom;
+
+ mJinv.InitMatrix( k1, k2, k3,
+ k2, k4, k5,
+ k3, k5, k6 );
Debug(2);
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);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-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;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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,
{
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");
}
}
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
}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGModel.h"
-#include "FGPropulsion.h"
+#include "FGColumnVector3.h"
+#include "FGMatrix33.h"
#include <vector>
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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);
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);
};
}
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-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;
- }
- }
-}
-}
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+Module: FGMatrix33.cpp\r
+Author: Tony Peden, Jon Berndt, Mathias Frolich\r
+Date started: 1998\r
+Purpose: FGMatrix33 class\r
+Called by: Various\r
+\r
+FUNCTIONAL DESCRIPTION\r
+--------------------------------------------------------------------------------\r
+\r
+HISTORY\r
+--------------------------------------------------------------------------------\r
+??/??/?? TP Created\r
+03/16/2000 JSB Added exception throwing\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+INCLUDES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+#include "FGMatrix33.h"\r
+#include "FGColumnVector3.h"\r
+\r
+namespace JSBSim {\r
+\r
+static const char *IdSrc = "$Id$";\r
+static const char *IdHdr = ID_MATRIX33;\r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+CLASS IMPLEMENTATION\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33::FGMatrix33(void)\r
+{\r
+ data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =\r
+ data[6] = data[7] = data[8] = 0.0;\r
+\r
+ Debug(0);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+ostream& operator<<(ostream& os, const FGMatrix33& M)\r
+{\r
+ for (unsigned int i=1; i<=M.Rows(); i++) {\r
+ for (unsigned int j=1; j<=M.Cols(); j++) {\r
+ if (i == M.Rows() && j == M.Cols())\r
+ os << M(i,j);\r
+ else\r
+ os << M(i,j) << ", ";\r
+ }\r
+ }\r
+ return os;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+istream& operator>>(istream& is, FGMatrix33& M)\r
+{\r
+ for (unsigned int i=1; i<=M.Rows(); i++) {\r
+ for (unsigned int j=1; j<=M.Cols(); j++) {\r
+ is >> M(i,j);\r
+ }\r
+ }\r
+ return is;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+double FGMatrix33::Determinant(void) const {\r
+ return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)\r
+ + Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)\r
+ - Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33 FGMatrix33::Inverse(void) const {\r
+ // Compute the inverse of a general matrix using Cramers rule.\r
+ // I guess googling for cramers rule gives tons of references\r
+ // for this. :)\r
+ double rdet = 1.0/Determinant();\r
+\r
+ double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));\r
+ double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));\r
+ double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));\r
+ double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));\r
+ double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));\r
+ double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));\r
+ double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));\r
+ double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));\r
+ double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));\r
+\r
+ return FGMatrix33( i11, i12, i13,\r
+ i21, i22, i23,\r
+ i31, i32, i33 );\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGMatrix33::InitMatrix(void)\r
+{\r
+ data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =\r
+ data[6] = data[7] = data[8] = 0.0;\r
+}\r
+\r
+// *****************************************************************************\r
+// binary operators ************************************************************\r
+// *****************************************************************************\r
+\r
+FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const\r
+{\r
+ return FGMatrix33( Entry(1,1) - M(1,1),\r
+ Entry(1,2) - M(1,2),\r
+ Entry(1,3) - M(1,3),\r
+ Entry(2,1) - M(2,1),\r
+ Entry(2,2) - M(2,2),\r
+ Entry(2,3) - M(2,3),\r
+ Entry(3,1) - M(3,1),\r
+ Entry(3,2) - M(3,2),\r
+ Entry(3,3) - M(3,3) );\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)\r
+{\r
+ data[0] -= M.data[0];\r
+ data[1] -= M.data[1];\r
+ data[2] -= M.data[2];\r
+ data[3] -= M.data[3];\r
+ data[4] -= M.data[4];\r
+ data[5] -= M.data[5];\r
+ data[6] -= M.data[6];\r
+ data[7] -= M.data[7];\r
+ data[8] -= M.data[8];\r
+\r
+ return *this;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const\r
+{\r
+ return FGMatrix33( Entry(1,1) + M(1,1),\r
+ Entry(1,2) + M(1,2),\r
+ Entry(1,3) + M(1,3),\r
+ Entry(2,1) + M(2,1),\r
+ Entry(2,2) + M(2,2),\r
+ Entry(2,3) + M(2,3),\r
+ Entry(3,1) + M(3,1),\r
+ Entry(3,2) + M(3,2),\r
+ Entry(3,3) + M(3,3) );\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)\r
+{\r
+ Entry(1,1) += M(1,1);\r
+ Entry(1,2) += M(1,2);\r
+ Entry(1,3) += M(1,3);\r
+ Entry(2,1) += M(2,1);\r
+ Entry(2,2) += M(2,2);\r
+ Entry(2,3) += M(2,3);\r
+ Entry(3,1) += M(3,1);\r
+ Entry(3,2) += M(3,2);\r
+ Entry(3,3) += M(3,3);\r
+\r
+ return *this;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33 FGMatrix33::operator*(const double scalar) const\r
+{\r
+ return FGMatrix33( scalar * Entry(1,1),\r
+ scalar * Entry(1,2),\r
+ scalar * Entry(1,3),\r
+ scalar * Entry(2,1),\r
+ scalar * Entry(2,2),\r
+ scalar * Entry(2,3),\r
+ scalar * Entry(3,1),\r
+ scalar * Entry(3,2),\r
+ scalar * Entry(3,3) );\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33 operator*(double scalar, FGMatrix33 &M)\r
+{\r
+ return FGMatrix33( scalar * M(1,1),\r
+ scalar * M(1,2),\r
+ scalar * M(1,3),\r
+ scalar * M(2,1),\r
+ scalar * M(2,2),\r
+ scalar * M(2,3),\r
+ scalar * M(3,1),\r
+ scalar * M(3,2),\r
+ scalar * M(3,3) );\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33& FGMatrix33::operator*=(const double scalar)\r
+{\r
+ Entry(1,1) *= scalar;\r
+ Entry(1,2) *= scalar;\r
+ Entry(1,3) *= scalar;\r
+ Entry(2,1) *= scalar;\r
+ Entry(2,2) *= scalar;\r
+ Entry(2,3) *= scalar;\r
+ Entry(3,1) *= scalar;\r
+ Entry(3,2) *= scalar;\r
+ Entry(3,3) *= scalar;\r
+\r
+ return *this;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const\r
+{\r
+ // FIXME: Make compiler friendlier\r
+ FGMatrix33 Product;\r
+\r
+ Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);\r
+ Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);\r
+ Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);\r
+ Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);\r
+ Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);\r
+ Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);\r
+ Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);\r
+ Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);\r
+ Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);\r
+\r
+ return Product;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)\r
+{\r
+ // FIXME: Make compiler friendlier\r
+ double a,b,c;\r
+\r
+ a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);\r
+ Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);\r
+ Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);\r
+ Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);\r
+\r
+ a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);\r
+ Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);\r
+ Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);\r
+ Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);\r
+\r
+ a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);\r
+ Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);\r
+ Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);\r
+ Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);\r
+\r
+ return *this;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33 FGMatrix33::operator/(const double scalar) const\r
+{\r
+ FGMatrix33 Quot;\r
+\r
+ if ( scalar != 0 ) {\r
+ double tmp = 1.0/scalar;\r
+ Quot(1,1) = Entry(1,1) * tmp;\r
+ Quot(1,2) = Entry(1,2) * tmp;\r
+ Quot(1,3) = Entry(1,3) * tmp;\r
+ Quot(2,1) = Entry(2,1) * tmp;\r
+ Quot(2,2) = Entry(2,2) * tmp;\r
+ Quot(2,3) = Entry(2,3) * tmp;\r
+ Quot(3,1) = Entry(3,1) * tmp;\r
+ Quot(3,2) = Entry(3,2) * tmp;\r
+ Quot(3,3) = Entry(3,3) * tmp;\r
+ } else {\r
+ MatrixException mE;\r
+ mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";\r
+ throw mE;\r
+ }\r
+ return Quot;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGMatrix33& FGMatrix33::operator/=(const double scalar)\r
+{\r
+ if ( scalar != 0 ) {\r
+ double tmp = 1.0/scalar;\r
+ Entry(1,1) *= tmp;\r
+ Entry(1,2) *= tmp;\r
+ Entry(1,3) *= tmp;\r
+ Entry(2,1) *= tmp;\r
+ Entry(2,2) *= tmp;\r
+ Entry(2,3) *= tmp;\r
+ Entry(3,1) *= tmp;\r
+ Entry(3,2) *= tmp;\r
+ Entry(3,3) *= tmp;\r
+ } else {\r
+ MatrixException mE;\r
+ mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";\r
+ throw mE;\r
+ }\r
+ return *this;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGMatrix33::T(void)\r
+{\r
+ for (unsigned int i=1; i<=3; i++) {\r
+ for (unsigned int j=i+1; j<=3; j++) {\r
+ double tmp = Entry(i,j);\r
+ Entry(i,j) = Entry(j,i);\r
+ Entry(j,i) = tmp;\r
+ }\r
+ }\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {\r
+ double tmp1 = v(1)*Entry(1,1);\r
+ double tmp2 = v(1)*Entry(2,1);\r
+ double tmp3 = v(1)*Entry(3,1);\r
+\r
+ tmp1 += v(2)*Entry(1,2);\r
+ tmp2 += v(2)*Entry(2,2);\r
+ tmp3 += v(2)*Entry(3,2);\r
+\r
+ tmp1 += v(3)*Entry(1,3);\r
+ tmp2 += v(3)*Entry(2,3);\r
+ tmp3 += v(3)*Entry(3,3);\r
+\r
+ return FGColumnVector3( tmp1, tmp2, tmp3 );\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+// The bitmasked value choices are as follows:\r
+// unset: In this case (the default) JSBSim would only print\r
+// out the normally expected messages, essentially echoing\r
+// the config files as they are read. If the environment\r
+// variable is not set, debug_lvl is set to 1 internally\r
+// 0: This requests JSBSim not to output any messages\r
+// whatsoever.\r
+// 1: This value explicity requests the normal JSBSim\r
+// startup messages\r
+// 2: This value asks for a message to be printed out when\r
+// a class is instantiated\r
+// 4: When this value is set, a message is displayed when a\r
+// FGModel object executes its Run() method\r
+// 8: When this value is set, various runtime state variables\r
+// are printed out periodically\r
+// 16: When set various parameters are sanity checked and\r
+// a message is printed out when they go out of bounds\r
+\r
+void FGMatrix33::Debug(int from)\r
+{\r
+ if (debug_lvl <= 0) return;\r
+\r
+ if (debug_lvl & 1) { // Standard console startup message output\r
+ if (from == 0) { // Constructor\r
+\r
+ }\r
+ }\r
+ if (debug_lvl & 2 ) { // Instantiation/Destruction notification\r
+ if (from == 0) cout << "Instantiated: FGMatrix33" << endl;\r
+ if (from == 1) cout << "Destroyed: FGMatrix33" << endl;\r
+ }\r
+ if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects\r
+ }\r
+ if (debug_lvl & 8 ) { // Runtime state variables\r
+ }\r
+ if (debug_lvl & 16) { // Sanity checking\r
+ }\r
+ if (debug_lvl & 64) {\r
+ if (from == 0) { // Constructor\r
+ cout << IdSrc << endl;\r
+ cout << IdHdr << endl;\r
+ }\r
+ }\r
+}\r
+}\r
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-/** Handles matrix math operations.
+ /** Handles matrix math operations.
+ @author Tony Peden, Jon Berndt, Mathias Froelich
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
class FGMatrix33 : public FGJSBBase
{
public:
+
+ enum {
+ eRows = 3,
+ eColumns = 3
+ };
+
+ /** Default initializer.
+
+ Create a zero matrix.
+ */
FGMatrix33(void);
- FGMatrix33(int r, int c);
- FGMatrix33(const FGMatrix33& A);
- ~FGMatrix33(void);
- FGMatrix33& operator=(const FGMatrix33& A);
- inline double operator()(unsigned int row, unsigned int col) const {return data[row][col];}
- inline double& operator()(unsigned int row, unsigned int col) {return data[row][col];}
+ /** Copy constructor.
+
+ @param M Matrix which is used for initialization.
+
+ Create copy of the matrix given in the argument.
+ */
+ FGMatrix33(const FGMatrix33& M) {
+ Entry(1,1) = M.Entry(1,1);
+ Entry(2,1) = M.Entry(2,1);
+ Entry(3,1) = M.Entry(3,1);
+ Entry(1,2) = M.Entry(1,2);
+ Entry(2,2) = M.Entry(2,2);
+ Entry(3,2) = M.Entry(3,2);
+ Entry(1,3) = M.Entry(1,3);
+ Entry(2,3) = M.Entry(2,3);
+ Entry(3,3) = M.Entry(3,3);
+
+ Debug(0);
+ }
+
+ /** Initialization by given values.
+
+ @param m11 value of the 1,1 Matrix element.
+ @param m12 value of the 1,2 Matrix element.
+ @param m13 value of the 1,3 Matrix element.
+ @param m21 value of the 2,1 Matrix element.
+ @param m22 value of the 2,2 Matrix element.
+ @param m23 value of the 2,3 Matrix element.
+ @param m31 value of the 3,1 Matrix element.
+ @param m32 value of the 3,2 Matrix element.
+ @param m33 value of the 3,3 Matrix element.
+
+ Create a matrix from the doubles given in the arguments.
+ */
+ FGMatrix33(double m11, double m12, double m13,
+ double m21, double m22, double m23,
+ double m31, double m32, double m33) {
+ Entry(1,1) = m11;
+ Entry(2,1) = m21;
+ Entry(3,1) = m31;
+ Entry(1,2) = m12;
+ Entry(2,2) = m22;
+ Entry(3,2) = m32;
+ Entry(1,3) = m13;
+ Entry(2,3) = m23;
+ Entry(3,3) = m33;
+
+ Debug(0);
+ }
+
+ /** Destructor.
+ */
+ ~FGMatrix33(void) { Debug(1); }
+
+ /** Read access the entries of the matrix.
+ @param row Row index.
+ @param col Column index.
+
+ @return the value of the matrix entry at the given row and
+ column indices. Indices are counted starting with 1.
+ */
+ double operator()(unsigned int row, unsigned int col) const {
+ return Entry(row, col);
+ }
+
+ /** Write access the entries of the matrix.
+ Note that the indices given in the arguments are unchecked.
+
+ @param row Row index.
+ @param col Column index.
+
+ @return a reference to the matrix entry at the given row and
+ column indices. Indices are counted starting with 1.
+ */
+ double& operator()(unsigned int row, unsigned int col) {
+ return Entry(row, col);
+ }
+
+ /** Read access the entries of the matrix.
+ This function is just a shortcut for the @ref double&
+ operator()(unsigned int row, unsigned int col) function. It is
+ used internally to access the elements in a more convenient way.
+
+ Note that the indices given in the arguments are unchecked.
+
+ @param row Row index.
+ @param col Column index.
+
+ @return the value of the matrix entry at the given row and
+ column indices. Indices are counted starting with 1.
+ */
+ double Entry(unsigned int row, unsigned int col) const {
+ return data[(col-1)*eRows+row-1];
+ }
+
+ /** Write access the entries of the matrix.
+ This function is just a shortcut for the @ref double&
+ operator()(unsigned int row, unsigned int col) function. It is
+ used internally to access the elements in a more convenient way.
- FGColumnVector3 operator*(const FGColumnVector3& Col);
+ Note that the indices given in the arguments are unchecked.
- inline unsigned int Rows(void) const { return 3; }
- inline unsigned int Cols(void) const { return 3; }
+ @param row Row index.
+ @param col Column index.
+ @return a reference to the matrix entry at the given row and
+ column indices. Indices are counted starting with 1.
+ */
+ double& Entry(unsigned int row, unsigned int col) {
+ return data[(col-1)*eRows+row-1];
+ }
+
+ /** Number of rows in the matrix.
+ @return the number of rows in the matrix.
+ */
+ unsigned int Rows(void) const { return eRows; }
+
+ /** Number of cloumns in the matrix.
+ @return the number of columns in the matrix.
+ */
+ unsigned int Cols(void) const { return eColumns; }
+
+ /** Transposed matrix.
+ This function only returns the transpose of this matrix. This matrix itself
+ remains unchanged.
+ @return the transposed matrix.
+ */
+ FGMatrix33 Transposed(void) const {
+ return FGMatrix33( Entry(1,1), Entry(2,1), Entry(3,1),
+ Entry(1,2), Entry(2,2), Entry(3,2),
+ Entry(1,3), Entry(2,3), Entry(3,3) );
+ }
+
+ /** Transposes this matrix.
+ This function only transposes this matrix. Nothing is returned.
+ */
void T(void);
+
+/** Initialize the matrix.
+ This function initializes a matrix to all 0.0.
+ */
void InitMatrix(void);
- void InitMatrix(double value);
-
- //friend FGMatrix33 operator*(double scalar,FGMatrix33& A);
- FGMatrix33 operator-(const FGMatrix33& B);
- FGMatrix33 operator+(const FGMatrix33& B);
- FGMatrix33 operator*(const FGMatrix33& B);
- FGMatrix33 operator*(const double scalar);
- FGMatrix33 operator/(const double scalar);
- FGMatrix33& operator<<(const double ff);
+/** Initialize the matrix.
+ This function initializes a matrix to user specified values.
+ */
+ void InitMatrix(double m11, double m12, double m13,
+ double m21, double m22, double m23,
+ double m31, double m32, double m33) {
+ Entry(1,1) = m11;
+ Entry(2,1) = m21;
+ Entry(3,1) = m31;
+ Entry(1,2) = m12;
+ Entry(2,2) = m22;
+ Entry(3,2) = m32;
+ Entry(1,3) = m13;
+ Entry(2,3) = m23;
+ Entry(3,3) = m33;
+ }
+
+ /** Determinant of the matrix.
+ @return the determinant of the matrix.
+ */
+ double Determinant(void) const;
+
+ /** Return if the matrix is invertible.
+ Checks and returns if the matrix is nonsingular and thus
+ invertible. This is done by simply computing the determinant and
+ check if it is zero. Note that this test does not cover any
+ instabilities caused by nearly singular matirces using finite
+ arithmetics. It only checks exact singularity.
+ */
+ bool Invertible(void) const { return 0.0 != Determinant(); }
+
+ /** Return the inverse of the matrix.
+ Computes and returns if the inverse of the matrix. It is computed
+ by Cramers Rule. Also there are no checks performed if the matrix
+ is invertible. If you are not sure that it really is check this
+ with the @ref Invertible() call before.
+ */
+ FGMatrix33 Inverse(void) const;
+
+ /** Assignment operator.
+
+ @param A source matrix.
+
+ Copy the content of the matrix given in the argument into *this.
+ */
+ FGMatrix33& operator=(const FGMatrix33& A) {
+ data[0] = A.data[0];
+ data[1] = A.data[1];
+ data[2] = A.data[2];
+ data[3] = A.data[3];
+ data[4] = A.data[4];
+ data[5] = A.data[5];
+ data[6] = A.data[6];
+ data[7] = A.data[7];
+ data[8] = A.data[8];
+ return *this;
+ }
+
+ /** Matrix vector multiplication.
+
+ @param v vector to multiply with.
+ @return matric vector product.
+
+ Compute and return the product of the current matrix with the
+ vector given in the argument.
+ */
+ FGColumnVector3 operator*(const FGColumnVector3& v) const;
+
+ /** Matrix subtraction.
+
+ @param B matrix to add to.
+ @return difference of the matrices.
+
+ Compute and return the sum of the current matrix and the matrix
+ B given in the argument.
+ */
+ FGMatrix33 operator-(const FGMatrix33& B) const;
+
+ /** Matrix addition.
+
+ @param B matrix to add to.
+ @return sum of the matrices.
+
+ Compute and return the sum of the current matrix and the matrix
+ B given in the argument.
+ */
+ FGMatrix33 operator+(const FGMatrix33& B) const;
+
+ /** Matrix product.
+
+ @param B matrix to add to.
+ @return product of the matrices.
+
+ Compute and return the product of the current matrix and the matrix
+ B given in the argument.
+ */
+ FGMatrix33 operator*(const FGMatrix33& B) const;
+
+ /** Multiply the matrix with a scalar.
+
+ @param scalar scalar factor to multiply with.
+ @return scaled matrix.
+
+ Compute and return the product of the current matrix with the
+ scalar value scalar given in the argument.
+ */
+ FGMatrix33 operator*(const double scalar) const;
+
+ /** Multiply the matrix with 1.0/scalar.
+
+ @param scalar scalar factor to divide through.
+ @return scaled matrix.
+
+ Compute and return the product of the current matrix with the
+ scalar value 1.0/scalar, where scalar is given in the argument.
+ */
+ FGMatrix33 operator/(const double scalar) const;
+
+ /** In place matrix subtraction.
+
+ @param B matrix to subtract.
+ @return reference to the current matrix.
+
+ Compute the diffence from the current matrix and the matrix B
+ given in the argument.
+ */
+ FGMatrix33& operator-=(const FGMatrix33 &B);
+
+ /** In place matrix addition.
+
+ @param B matrix to add.
+ @return reference to the current matrix.
+
+ Compute the sum of the current matrix and the matrix B
+ given in the argument.
+ */
+ FGMatrix33& operator+=(const FGMatrix33 &B);
- friend ostream& operator<<(ostream& os, const FGMatrix33& M);
- friend istream& operator>>(istream& is, FGMatrix33& M);
+ /** In place matrix multiplication.
- void operator-=(const FGMatrix33 &B);
- void operator+=(const FGMatrix33 &B);
- void operator*=(const FGMatrix33 &B);
- void operator*=(const double scalar);
- void operator/=(const double scalar);
+ @param B matrix to multiply with.
+ @return reference to the current matrix.
-protected:
- double data[4][4];
+ Compute the product of the current matrix and the matrix B
+ given in the argument.
+ */
+ FGMatrix33& operator*=(const FGMatrix33 &B);
+
+ /** In place matrix scale.
+
+ @param scalar scalar value to multiply with.
+ @return reference to the current matrix.
+
+ Compute the product of the current matrix and the scalar value scalar
+ given in the argument.
+ */
+ FGMatrix33& operator*=(const double scalar);
+
+ /** In place matrix scale.
+
+ @param scalar scalar value to divide through.
+ @return reference to the current matrix.
+
+ Compute the product of the current matrix and the scalar value
+ 1.0/scalar, where scalar is given in the argument.
+ */
+ FGMatrix33& operator/=(const double scalar);
private:
- void TransposeSquare(void);
- unsigned int rowCtr, colCtr;
+ double data[eRows*eColumns];
+
void Debug(int from);
};
+
+/** Scalar multiplication.
+
+ @param scalar scalar value to multiply with.
+ @param A Matrix to multiply.
+
+ Multiply the Matrix with a scalar value.
+*/
+inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) {
+ // use already defined operation.
+ return A*scalar;
}
+
+/** Write matrix to a stream.
+
+ @param os Stream to write to.
+ @param M Matrix to write.
+
+ Write the matrix to a stream.
+*/
+ostream& operator<<(ostream& os, const FGMatrix33& M);
+
+/** Read matrix from a stream.
+
+ @param os Stream to read from.
+ @param M Matrix to initialize with the values from the stream.
+
+ Read matrix from a stream.
+*/
+istream& operator>>(istream& is, FGMatrix33& M);
+
+} // namespace JSBSim
+
#endif
#endif
#include "FGNozzle.h"
+#include "FGAtmosphere.h"
namespace JSBSim {
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
- Module: FGOutput.cpp
- Author: Jon Berndt
- Date started: 12/02/98
- Purpose: Manage output of sim parameters to file or stdout
- Called by: FGSimExec
-
- ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
-
- This program is free software; you can redistribute it and/or modify it under
- the terms of the GNU General Public License as published by the Free Software
- Foundation; either version 2 of the License, or (at your option) any later
- version.
-
- This program is distributed in the hope that it will be useful, but WITHOUT
- ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
- FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
- details.
-
- You should have received a copy of the GNU General Public License along with
- this program; if not, write to the Free Software Foundation, Inc., 59 Temple
- Place - Suite 330, Boston, MA 02111-1307, USA.
-
- Further information about the GNU General Public License can also be found on
- the world wide web at http://www.gnu.org.
-
-FUNCTIONAL DESCRIPTION
---------------------------------------------------------------------------------
-This is the place where you create output routines to dump data for perusal
-later.
-
-HISTORY
---------------------------------------------------------------------------------
-12/02/98 JSB Created
-
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-INCLUDES
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-
-#include "FGOutput.h"
-#include "FGState.h"
-#include "FGFDMExec.h"
-#include "FGAtmosphere.h"
-#include "FGFCS.h"
-#include "FGAerodynamics.h"
-#include "FGGroundReactions.h"
-#include "FGAircraft.h"
-#include "FGMassBalance.h"
-#include "FGTranslation.h"
-#include "FGRotation.h"
-#include "FGPosition.h"
-#include "FGAuxiliary.h"
-
-namespace JSBSim {
-
-static const char *IdSrc = "$Id$";
-static const char *IdHdr = ID_OUTPUT;
-
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-CLASS IMPLEMENTATION
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-
-FGOutput::FGOutput(FGFDMExec* fdmex) : FGModel(fdmex)
-{
- Name = "FGOutput";
- sFirstPass = dFirstPass = true;
- socket = 0;
- Type = otNone;
- Filename = "";
- SubSystems = 0;
- enabled = true;
- outputInFileName = "";
-
- Debug(0);
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-FGOutput::~FGOutput()
-{
- if (socket) delete socket;
- for (int i=0; i<OutputProperties.size(); i++) delete OutputProperties[i];
-
- Debug(1);
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-bool FGOutput::Run(void)
-{
- if (enabled) {
- if (!FGModel::Run()) {
-
- if (Type == otSocket) {
- SocketOutput();
- } else if (Type == otCSV) {
- DelimitedOutput(Filename);
- } else if (Type == otTerminal) {
- // Not done yet
- } else if (Type == otNone) {
- // Do nothing
- } else {
- // Not a valid type of output
- }
- return false;
- } else {
- return true;
- }
- }
- return false;
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-void FGOutput::SetType(string type)
-{
- if (type == "CSV") {
- Type = otCSV;
- } else if (type == "TABULAR") {
- Type = otTab;
- } else if (type == "SOCKET") {
- Type = otSocket;
- } else if (type == "TERMINAL") {
- Type = otTerminal;
- } else if (type != string("NONE")) {
- Type = otUnknown;
- cerr << "Unknown type of output specified in config file" << endl;
- }
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-void FGOutput::DelimitedOutput(string fname)
-{
- streambuf* buffer;
-
- if (fname == "COUT" || fname == "cout") {
- buffer = cout.rdbuf();
- } else {
- datafile.open(fname.c_str());
- buffer = datafile.rdbuf();
- }
-
- ostream outstream(buffer);
-
- if (dFirstPass) {
- outstream << "Time";
- if (SubSystems & ssSimulation) {
- // Nothing here, yet
- }
- if (SubSystems & ssAerosurfaces) {
- outstream << ", ";
- outstream << "Aileron Cmd, ";
- outstream << "Elevator Cmd, ";
- outstream << "Rudder Cmd, ";
- outstream << "Flap Cmd, ";
- outstream << "Left Aileron Pos, ";
- outstream << "Right Aileron Pos, ";
- outstream << "Elevator Pos, ";
- outstream << "Rudder Pos, ";
- outstream << "Flap Pos";
- }
- if (SubSystems & ssRates) {
- outstream << ", ";
- outstream << "P, Q, R, ";
- outstream << "Pdot, Qdot, Rdot";
- }
- if (SubSystems & ssVelocities) {
- outstream << ", ";
- outstream << "QBar, ";
- outstream << "Vtotal, ";
- outstream << "UBody, VBody, WBody, ";
- outstream << "UAero, VAero, WAero, ";
- outstream << "Vn, Ve, Vd";
- }
- if (SubSystems & ssForces) {
- outstream << ", ";
- outstream << "Drag, Side, Lift, ";
- outstream << "L/D, ";
- outstream << "Xforce, Yforce, Zforce, ";
- outstream << "xGravity, yGravity, zGravity, ";
- outstream << "xCoriolis, yCoriolis, zCoriolis, ";
- outstream << "xCentrifugal, yCentrifugal, zCentrifugal";
- }
- if (SubSystems & ssMoments) {
- outstream << ", ";
- outstream << "L, M, N";
- }
- if (SubSystems & ssAtmosphere) {
- outstream << ", ";
- outstream << "Rho, ";
- outstream << "NWind, EWind, DWind";
- }
- if (SubSystems & ssMassProps) {
- outstream << ", ";
- outstream << "Ixx, ";
- outstream << "Iyy, ";
- outstream << "Izz, ";
- outstream << "Ixz, ";
- outstream << "Mass, ";
- outstream << "Xcg, Ycg, Zcg";
- }
- if (SubSystems & ssPosition) {
- outstream << ", ";
- outstream << "Altitude, ";
- outstream << "Phi, Tht, Psi, ";
- outstream << "Alpha, ";
- outstream << "Beta, ";
- outstream << "Latitude, ";
- outstream << "Longitude, ";
- outstream << "Distance AGL, ";
- outstream << "Runway Radius";
- }
- if (SubSystems & ssCoefficients) {
- outstream << ", ";
- outstream << Aerodynamics->GetCoefficientStrings();
- }
- if (SubSystems & ssFCS) {
- outstream << ", ";
- outstream << FCS->GetComponentStrings();
- }
- if (SubSystems & ssGroundReactions) {
- outstream << ", ";
- outstream << GroundReactions->GetGroundReactionStrings();
- }
- if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) {
- outstream << ", ";
- outstream << Propulsion->GetPropulsionStrings();
- }
- if (OutputProperties.size() > 0) {
- for (int i=0;i<OutputProperties.size();i++) {
- outstream << ", " << OutputProperties[i]->GetName();
- }
- }
-
- outstream << endl;
- dFirstPass = false;
- }
-
- outstream << State->Getsim_time();
- if (SubSystems & ssSimulation) {
- }
- if (SubSystems & ssAerosurfaces) {
- outstream << ", ";
- outstream << FCS->GetDaCmd() << ", ";
- outstream << FCS->GetDeCmd() << ", ";
- outstream << FCS->GetDrCmd() << ", ";
- outstream << FCS->GetDfCmd() << ", ";
- outstream << FCS->GetDaLPos() << ", ";
- outstream << FCS->GetDaRPos() << ", ";
- outstream << FCS->GetDePos() << ", ";
- outstream << FCS->GetDrPos() << ", ";
- outstream << FCS->GetDfPos();
- }
- if (SubSystems & ssRates) {
- outstream << ", ";
- outstream << Rotation->GetPQR() << ", ";
- outstream << Rotation->GetPQRdot();
- }
- if (SubSystems & ssVelocities) {
- outstream << ", ";
- outstream << Translation->Getqbar() << ", ";
- outstream << Translation->GetVt() << ", ";
- outstream << Translation->GetUVW() << ", ";
- outstream << Translation->GetAeroUVW() << ", ";
- outstream << Position->GetVel();
- }
- if (SubSystems & ssForces) {
- outstream << ", ";
- outstream << Aerodynamics->GetvFs() << ", ";
- outstream << Aerodynamics->GetLoD() << ", ";
- outstream << Aircraft->GetForces() << ", ";
- outstream << Inertial->GetGravity() << ", ";
- outstream << Inertial->GetCoriolis() << ", ";
- outstream << Inertial->GetCentrifugal();
- }
- if (SubSystems & ssMoments) {
- outstream << ", ";
- outstream << Aircraft->GetMoments();
- }
- if (SubSystems & ssAtmosphere) {
- outstream << ", ";
- outstream << Atmosphere->GetDensity() << ", ";
- outstream << Atmosphere->GetWindNED();
- }
- if (SubSystems & ssMassProps) {
- outstream << ", ";
- outstream << MassBalance->GetIxx() << ", ";
- outstream << MassBalance->GetIyy() << ", ";
- outstream << MassBalance->GetIzz() << ", ";
- outstream << MassBalance->GetIxz() << ", ";
- outstream << MassBalance->GetMass() << ", ";
- outstream << MassBalance->GetXYZcg();
- }
- if (SubSystems & ssPosition) {
- outstream << ", ";
- outstream << Position->Geth() << ", ";
- outstream << Rotation->GetEuler() << ", ";
- outstream << Translation->Getalpha() << ", ";
- outstream << Translation->Getbeta() << ", ";
- outstream << Position->GetLatitude() << ", ";
- outstream << Position->GetLongitude() << ", ";
- outstream << Position->GetDistanceAGL() << ", ";
- outstream << Position->GetRunwayRadius();
- }
- if (SubSystems & ssCoefficients) {
- outstream << ", ";
- outstream << Aerodynamics->GetCoefficientValues();
- }
- if (SubSystems & ssFCS) {
- outstream << ", ";
- outstream << FCS->GetComponentValues();
- }
- if (SubSystems & ssGroundReactions) {
- outstream << ", ";
- outstream << GroundReactions->GetGroundReactionValues();
- }
- if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) {
- outstream << ", ";
- outstream << Propulsion->GetPropulsionValues();
- }
-
- for (int i=0;i<OutputProperties.size();i++) {
- outstream << ", " << OutputProperties[i]->getDoubleValue();
- }
-
- outstream << endl;
- outstream.flush();
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-void FGOutput::SocketOutput(void)
-{
- string asciiData;
-
- if (socket == NULL) return;
- if (!socket->GetConnectStatus()) return;
-
- socket->Clear();
- if (sFirstPass) {
- socket->Append("<LABELS>");
- socket->Append("Time");
- socket->Append("Altitude");
- socket->Append("Phi");
- socket->Append("Tht");
- socket->Append("Psi");
- socket->Append("Rho");
- socket->Append("Vtotal");
- socket->Append("UBody");
- socket->Append("VBody");
- socket->Append("WBody");
- socket->Append("UAero");
- socket->Append("VAero");
- socket->Append("WAero");
- socket->Append("Vn");
- socket->Append("Ve");
- socket->Append("Vd");
- socket->Append("Udot");
- socket->Append("Vdot");
- socket->Append("Wdot");
- socket->Append("P");
- socket->Append("Q");
- socket->Append("R");
- socket->Append("PDot");
- socket->Append("QDot");
- socket->Append("RDot");
- socket->Append("Fx");
- socket->Append("Fy");
- socket->Append("Fz");
- socket->Append("Latitude");
- socket->Append("Longitude");
- socket->Append("QBar");
- socket->Append("Alpha");
- socket->Append("L");
- socket->Append("M");
- socket->Append("N");
- socket->Append("Throttle Position");
- socket->Append("Left Aileron Position");
- socket->Append("Right Aileron Position");
- socket->Append("Elevator Position");
- socket->Append("Rudder Position");
- sFirstPass = false;
- socket->Send();
- }
-
- socket->Clear();
- socket->Append(State->Getsim_time());
- socket->Append(Position->Geth());
- socket->Append(Rotation->Getphi());
- socket->Append(Rotation->Gettht());
- socket->Append(Rotation->Getpsi());
- socket->Append(Atmosphere->GetDensity());
- socket->Append(Translation->GetVt());
- socket->Append(Translation->GetUVW(eU));
- socket->Append(Translation->GetUVW(eV));
- socket->Append(Translation->GetUVW(eW));
- socket->Append(Translation->GetAeroUVW(eU));
- socket->Append(Translation->GetAeroUVW(eV));
- socket->Append(Translation->GetAeroUVW(eW));
- socket->Append(Position->GetVn());
- socket->Append(Position->GetVe());
- socket->Append(Position->GetVd());
- socket->Append(Translation->GetUVWdot(eU));
- socket->Append(Translation->GetUVWdot(eV));
- socket->Append(Translation->GetUVWdot(eW));
- socket->Append(Rotation->GetPQR(eP));
- socket->Append(Rotation->GetPQR(eQ));
- socket->Append(Rotation->GetPQR(eR));
- socket->Append(Rotation->GetPQRdot(eP));
- socket->Append(Rotation->GetPQRdot(eQ));
- socket->Append(Rotation->GetPQRdot(eR));
- socket->Append(Aircraft->GetForces(eX));
- socket->Append(Aircraft->GetForces(eY));
- socket->Append(Aircraft->GetForces(eZ));
- socket->Append(Position->GetLatitude());
- socket->Append(Position->GetLongitude());
- socket->Append(Translation->Getqbar());
- socket->Append(Translation->Getalpha());
- socket->Append(Aircraft->GetMoments(eL));
- socket->Append(Aircraft->GetMoments(eM));
- socket->Append(Aircraft->GetMoments(eN));
- socket->Append(FCS->GetThrottlePos(0));
- socket->Append(FCS->GetDaLPos());
- socket->Append(FCS->GetDaRPos());
- socket->Append(FCS->GetDePos());
- socket->Append(FCS->GetDrPos());
- socket->Send();
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-void FGOutput::SocketStatusOutput(string out_str)
-{
- string asciiData;
-
- if (socket == NULL) return;
-
- socket->Clear();
- asciiData = string("<STATUS>") + out_str;
- socket->Append(asciiData.c_str());
- socket->Send();
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-bool FGOutput::Load(FGConfigFile* AC_cfg)
-{
- string token="", parameter="", separator="";
- string name="", fname="";
- int OutRate = 0;
- FGConfigFile* Output_cfg;
- string property;
-
-# ifndef macintosh
- separator = "/";
-# else
- separator = ";";
-# endif
-
- name = AC_cfg->GetValue("NAME");
- fname = AC_cfg->GetValue("FILE");
- token = AC_cfg->GetValue("TYPE");
- Output->SetType(token);
-
-#if defined( FG_WITH_JSBSIM_SOCKET ) || !defined( FGFS )
- if (token == "SOCKET") {
- socket = new FGfdmSocket("localhost",1138);
- }
-#endif
-
- if (!fname.empty()) {
- outputInFileName = FDMExec->GetAircraftPath() + separator
- + FDMExec->GetModelName() + separator + fname + ".xml";
- Output_cfg = new FGConfigFile(outputInFileName);
- if (!Output_cfg->IsOpen()) {
- cerr << "Could not open file: " << outputInFileName << endl;
- return false;
- }
- } else {
- Output_cfg = AC_cfg;
- }
- Output->SetFilename(name);
-
- while ((token = Output_cfg->GetValue()) != string("/OUTPUT")) {
- *Output_cfg >> parameter;
- if (parameter == "RATE_IN_HZ") {
- *Output_cfg >> OutRate;
- }
- if (parameter == "SIMULATION") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssSimulation;
- }
- if (parameter == "AEROSURFACES") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssAerosurfaces;
- }
- if (parameter == "RATES") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssRates;
- }
- if (parameter == "VELOCITIES") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssVelocities;
- }
- if (parameter == "FORCES") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssForces;
- }
- if (parameter == "MOMENTS") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssMoments;
- }
- if (parameter == "ATMOSPHERE") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssAtmosphere;
- }
- if (parameter == "MASSPROPS") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssMassProps;
- }
- if (parameter == "POSITION") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssPosition;
- }
- if (parameter == "COEFFICIENTS") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssCoefficients;
- }
- if (parameter == "GROUND_REACTIONS") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssGroundReactions;
- }
- if (parameter == "FCS") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssFCS;
- }
- if (parameter == "PROPULSION") {
- *Output_cfg >> parameter;
- if (parameter == "ON") SubSystems += ssPropulsion;
- }
- if (parameter == "PROPERTY") {
- *Output_cfg >> property;
- OutputProperties.push_back(PropertyManager->GetNode(property));
- }
-
- if (parameter == "EOF") break;
- }
-
- OutRate = OutRate>120?120:(OutRate<0?0:OutRate);
- rate = (int)(0.5 + 1.0/(State->Getdt()*OutRate));
-
- Debug(2);
-
- return true;
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-// The bitmasked value choices are as follows:
-// unset: In this case (the default) JSBSim would only print
-// out the normally expected messages, essentially echoing
-// the config files as they are read. If the environment
-// variable is not set, debug_lvl is set to 1 internally
-// 0: This requests JSBSim not to output any messages
-// whatsoever.
-// 1: This value explicity requests the normal JSBSim
-// startup messages
-// 2: This value asks for a message to be printed out when
-// a class is instantiated
-// 4: When this value is set, a message is displayed when a
-// FGModel object executes its Run() method
-// 8: When this value is set, various runtime state variables
-// are printed out periodically
-// 16: When set various parameters are sanity checked and
-// a message is printed out when they go out of bounds
-
-void FGOutput::Debug(int from)
-{
- string scratch="";
-
- if (debug_lvl <= 0) return;
-
- if (debug_lvl & 1) { // Standard console startup message output
- if (from == 0) { // Constructor
-
- }
- if (from == 2) {
- if (outputInFileName.empty())
- cout << " " << "Output parameters read inline" << endl;
- else
- cout << " Output parameters read from file: " << outputInFileName << endl;
-
- if (Filename == "cout" || Filename == "COUT") {
- scratch = " Log output goes to screen console";
- } else if (!Filename.empty()) {
- scratch = " Log output goes to file: " + Filename;
- }
- switch (Type) {
- case otCSV:
- cout << scratch << " in CSV format output at rate " << 120/rate << " Hz" << endl;
- break;
- case otNone:
- cout << " No log output" << endl;
- break;
- }
-
- if (SubSystems & ssSimulation) cout << " Simulation parameters logged" << endl;
- if (SubSystems & ssAerosurfaces) cout << " Aerosurface parameters logged" << endl;
- if (SubSystems & ssRates) cout << " Rate parameters logged" << endl;
- if (SubSystems & ssVelocities) cout << " Velocity parameters logged" << endl;
- if (SubSystems & ssForces) cout << " Force parameters logged" << endl;
- if (SubSystems & ssMoments) cout << " Moments parameters logged" << endl;
- if (SubSystems & ssAtmosphere) cout << " Atmosphere parameters logged" << endl;
- if (SubSystems & ssMassProps) cout << " Mass parameters logged" << endl;
- if (SubSystems & ssCoefficients) cout << " Coefficient parameters logged" << endl;
- if (SubSystems & ssPosition) cout << " Position parameters logged" << endl;
- if (SubSystems & ssGroundReactions) cout << " Ground parameters logged" << endl;
- if (SubSystems & ssFCS) cout << " FCS parameters logged" << endl;
- if (SubSystems & ssPropulsion) cout << " Propulsion parameters logged" << endl;
- }
- }
- if (debug_lvl & 2 ) { // Instantiation/Destruction notification
- if (from == 0) cout << "Instantiated: FGOutput" << endl;
- if (from == 1) cout << "Destroyed: FGOutput" << endl;
- }
- if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
- }
- if (debug_lvl & 8 ) { // Runtime state variables
- }
- if (debug_lvl & 16) { // Sanity checking
- }
- if (debug_lvl & 64) {
- if (from == 0) { // Constructor
- cout << IdSrc << endl;
- cout << IdHdr << endl;
- }
- }
-}
-}
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+ Module: FGOutput.cpp\r
+ Author: Jon Berndt\r
+ Date started: 12/02/98\r
+ Purpose: Manage output of sim parameters to file or stdout\r
+ Called by: FGSimExec\r
+\r
+ ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------\r
+\r
+ This program is free software; you can redistribute it and/or modify it under\r
+ the terms of the GNU General Public License as published by the Free Software\r
+ Foundation; either version 2 of the License, or (at your option) any later\r
+ version.\r
+\r
+ This program is distributed in the hope that it will be useful, but WITHOUT\r
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS\r
+ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more\r
+ details.\r
+\r
+ You should have received a copy of the GNU General Public License along with\r
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple\r
+ Place - Suite 330, Boston, MA 02111-1307, USA.\r
+\r
+ Further information about the GNU General Public License can also be found on\r
+ the world wide web at http://www.gnu.org.\r
+\r
+FUNCTIONAL DESCRIPTION\r
+--------------------------------------------------------------------------------\r
+This is the place where you create output routines to dump data for perusal\r
+later.\r
+\r
+HISTORY\r
+--------------------------------------------------------------------------------\r
+12/02/98 JSB Created\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+INCLUDES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+#include "FGOutput.h"\r
+#include "FGState.h"\r
+#include "FGFDMExec.h"\r
+#include "FGAtmosphere.h"\r
+#include "FGFCS.h"\r
+#include "FGAerodynamics.h"\r
+#include "FGGroundReactions.h"\r
+#include "FGAircraft.h"\r
+#include "FGMassBalance.h"\r
+#include "FGTranslation.h"\r
+#include "FGRotation.h"\r
+#include "FGPosition.h"\r
+#include "FGAuxiliary.h"\r
+#include "FGInertial.h"\r
+\r
+namespace JSBSim {\r
+\r
+static const char *IdSrc = "$Id$";\r
+static const char *IdHdr = ID_OUTPUT;\r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+CLASS IMPLEMENTATION\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+FGOutput::FGOutput(FGFDMExec* fdmex) : FGModel(fdmex)\r
+{\r
+ Name = "FGOutput";\r
+ sFirstPass = dFirstPass = true;\r
+ socket = 0;\r
+ Type = otNone;\r
+ Filename = "";\r
+ SubSystems = 0;\r
+ enabled = true;\r
+ outputInFileName = "";\r
+\r
+ Debug(0);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGOutput::~FGOutput()\r
+{\r
+ if (socket) delete socket;\r
+ for (unsigned int i=0; i<OutputProperties.size(); i++) delete OutputProperties[i];\r
+\r
+ Debug(1);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+bool FGOutput::Run(void)\r
+{\r
+ if (enabled) {\r
+ if (!FGModel::Run()) {\r
+\r
+ if (Type == otSocket) {\r
+ SocketOutput();\r
+ } else if (Type == otCSV) {\r
+ DelimitedOutput(Filename);\r
+ } else if (Type == otTerminal) {\r
+ // Not done yet\r
+ } else if (Type == otNone) {\r
+ // Do nothing\r
+ } else {\r
+ // Not a valid type of output\r
+ }\r
+ return false;\r
+ } else {\r
+ return true;\r
+ }\r
+ }\r
+ return false;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGOutput::SetType(string type)\r
+{\r
+ if (type == "CSV") {\r
+ Type = otCSV;\r
+ } else if (type == "TABULAR") {\r
+ Type = otTab;\r
+ } else if (type == "SOCKET") {\r
+ Type = otSocket;\r
+ } else if (type == "TERMINAL") {\r
+ Type = otTerminal;\r
+ } else if (type != string("NONE")) {\r
+ Type = otUnknown;\r
+ cerr << "Unknown type of output specified in config file" << endl;\r
+ }\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGOutput::DelimitedOutput(string fname)\r
+{\r
+ streambuf* buffer;\r
+ string scratch = "";\r
+\r
+ if (fname == "COUT" || fname == "cout") {\r
+ buffer = cout.rdbuf();\r
+ } else {\r
+ datafile.open(fname.c_str());\r
+ buffer = datafile.rdbuf();\r
+ }\r
+\r
+ ostream outstream(buffer);\r
+\r
+ if (dFirstPass) {\r
+ outstream << "Time";\r
+ if (SubSystems & ssSimulation) {\r
+ // Nothing here, yet\r
+ }\r
+ if (SubSystems & ssAerosurfaces) {\r
+ outstream << ", ";\r
+ outstream << "Aileron Cmd, ";\r
+ outstream << "Elevator Cmd, ";\r
+ outstream << "Rudder Cmd, ";\r
+ outstream << "Flap Cmd, ";\r
+ outstream << "Left Aileron Pos, ";\r
+ outstream << "Right Aileron Pos, ";\r
+ outstream << "Elevator Pos, ";\r
+ outstream << "Rudder Pos, ";\r
+ outstream << "Flap Pos";\r
+ }\r
+ if (SubSystems & ssRates) {\r
+ outstream << ", ";\r
+ outstream << "P, Q, R, ";\r
+ outstream << "Pdot, Qdot, Rdot";\r
+ }\r
+ if (SubSystems & ssVelocities) {\r
+ outstream << ", ";\r
+ outstream << "QBar, ";\r
+ outstream << "Vtotal, ";\r
+ outstream << "UBody, VBody, WBody, ";\r
+ outstream << "UAero, VAero, WAero, ";\r
+ outstream << "Vn, Ve, Vd";\r
+ }\r
+ if (SubSystems & ssForces) {\r
+ outstream << ", ";\r
+ outstream << "Drag, Side, Lift, ";\r
+ outstream << "L/D, ";\r
+ outstream << "Xforce, Yforce, Zforce, ";\r
+ outstream << "xGravity, yGravity, zGravity, ";\r
+ outstream << "xCoriolis, yCoriolis, zCoriolis, ";\r
+ outstream << "xCentrifugal, yCentrifugal, zCentrifugal";\r
+ }\r
+ if (SubSystems & ssMoments) {\r
+ outstream << ", ";\r
+ outstream << "L, M, N";\r
+ }\r
+ if (SubSystems & ssAtmosphere) {\r
+ outstream << ", ";\r
+ outstream << "Rho, ";\r
+ outstream << "NWind, EWind, DWind";\r
+ }\r
+ if (SubSystems & ssMassProps) {\r
+ outstream << ", ";\r
+ outstream << "Ixx, ";\r
+ outstream << "Ixy, ";\r
+ outstream << "Ixz, ";\r
+ outstream << "Iyx, ";\r
+ outstream << "Iyy, ";\r
+ outstream << "Iyz, ";\r
+ outstream << "Izx, ";\r
+ outstream << "Izy, ";\r
+ outstream << "Izz, ";\r
+ outstream << "Mass, ";\r
+ outstream << "Xcg, Ycg, Zcg";\r
+ }\r
+ if (SubSystems & ssPosition) {\r
+ outstream << ", ";\r
+ outstream << "Altitude, ";\r
+ outstream << "Phi, Tht, Psi, ";\r
+ outstream << "Alpha, ";\r
+ outstream << "Beta, ";\r
+ outstream << "Latitude, ";\r
+ outstream << "Longitude, ";\r
+ outstream << "Distance AGL, ";\r
+ outstream << "Runway Radius";\r
+ }\r
+ if (SubSystems & ssCoefficients) {\r
+ scratch = Aerodynamics->GetCoefficientStrings();\r
+ if (scratch.length() != 0) outstream << ", " << scratch;\r
+ }\r
+ if (SubSystems & ssFCS) {\r
+ scratch = FCS->GetComponentStrings();\r
+ if (scratch.length() != 0) outstream << ", " << scratch;\r
+ }\r
+ if (SubSystems & ssGroundReactions) {\r
+ outstream << ", ";\r
+ outstream << GroundReactions->GetGroundReactionStrings();\r
+ }\r
+ if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) {\r
+ outstream << ", ";\r
+ outstream << Propulsion->GetPropulsionStrings();\r
+ }\r
+ if (OutputProperties.size() > 0) {\r
+ for (unsigned int i=0;i<OutputProperties.size();i++) {\r
+ outstream << ", " << OutputProperties[i]->GetName();\r
+ }\r
+ }\r
+\r
+ outstream << endl;\r
+ dFirstPass = false;\r
+ }\r
+\r
+ outstream << State->Getsim_time();\r
+ if (SubSystems & ssSimulation) {\r
+ }\r
+ if (SubSystems & ssAerosurfaces) {\r
+ outstream << ", ";\r
+ outstream << FCS->GetDaCmd() << ", ";\r
+ outstream << FCS->GetDeCmd() << ", ";\r
+ outstream << FCS->GetDrCmd() << ", ";\r
+ outstream << FCS->GetDfCmd() << ", ";\r
+ outstream << FCS->GetDaLPos() << ", ";\r
+ outstream << FCS->GetDaRPos() << ", ";\r
+ outstream << FCS->GetDePos() << ", ";\r
+ outstream << FCS->GetDrPos() << ", ";\r
+ outstream << FCS->GetDfPos();\r
+ }\r
+ if (SubSystems & ssRates) {\r
+ outstream << ", ";\r
+ outstream << Rotation->GetPQR() << ", ";\r
+ outstream << Rotation->GetPQRdot();\r
+ }\r
+ if (SubSystems & ssVelocities) {\r
+ outstream << ", ";\r
+ outstream << Translation->Getqbar() << ", ";\r
+ outstream << Translation->GetVt() << ", ";\r
+ outstream << Translation->GetUVW() << ", ";\r
+ outstream << Translation->GetAeroUVW() << ", ";\r
+ outstream << Position->GetVel();\r
+ }\r
+ if (SubSystems & ssForces) {\r
+ outstream << ", ";\r
+ outstream << Aerodynamics->GetvFs() << ", ";\r
+ outstream << Aerodynamics->GetLoD() << ", ";\r
+ outstream << Aircraft->GetForces() << ", ";\r
+ outstream << Inertial->GetGravity() << ", ";\r
+ outstream << Inertial->GetCoriolis() << ", ";\r
+ outstream << Inertial->GetCentrifugal();\r
+ }\r
+ if (SubSystems & ssMoments) {\r
+ outstream << ", ";\r
+ outstream << Aircraft->GetMoments();\r
+ }\r
+ if (SubSystems & ssAtmosphere) {\r
+ outstream << ", ";\r
+ outstream << Atmosphere->GetDensity() << ", ";\r
+ outstream << Atmosphere->GetWindNED();\r
+ }\r
+ if (SubSystems & ssMassProps) {\r
+ outstream << ", ";\r
+ outstream << MassBalance->GetJ() << ", ";\r
+ outstream << MassBalance->GetMass() << ", ";\r
+ outstream << MassBalance->GetXYZcg();\r
+ }\r
+ if (SubSystems & ssPosition) {\r
+ outstream << ", ";\r
+ outstream << Position->Geth() << ", ";\r
+ outstream << Rotation->GetEuler() << ", ";\r
+ outstream << Translation->Getalpha() << ", ";\r
+ outstream << Translation->Getbeta() << ", ";\r
+ outstream << Position->GetLatitude() << ", ";\r
+ outstream << Position->GetLongitude() << ", ";\r
+ outstream << Position->GetDistanceAGL() << ", ";\r
+ outstream << Position->GetRunwayRadius();\r
+ }\r
+ if (SubSystems & ssCoefficients) {\r
+ scratch = Aerodynamics->GetCoefficientValues();\r
+ if (scratch.length() != 0) outstream << ", " << scratch;\r
+ }\r
+ if (SubSystems & ssFCS) {\r
+ scratch = FCS->GetComponentValues();\r
+ if (scratch.length() != 0) outstream << ", " << scratch;\r
+ }\r
+ if (SubSystems & ssGroundReactions) {\r
+ outstream << ", ";\r
+ outstream << GroundReactions->GetGroundReactionValues();\r
+ }\r
+ if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) {\r
+ outstream << ", ";\r
+ outstream << Propulsion->GetPropulsionValues();\r
+ }\r
+\r
+ for (unsigned int i=0;i<OutputProperties.size();i++) {\r
+ outstream << ", " << OutputProperties[i]->getDoubleValue();\r
+ }\r
+\r
+ outstream << endl;\r
+ outstream.flush();\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGOutput::SocketOutput(void)\r
+{\r
+ string asciiData;\r
+\r
+ if (socket == NULL) return;\r
+ if (!socket->GetConnectStatus()) return;\r
+\r
+ socket->Clear();\r
+ if (sFirstPass) {\r
+ socket->Append("<LABELS>");\r
+ socket->Append("Time");\r
+ socket->Append("Altitude");\r
+ socket->Append("Phi");\r
+ socket->Append("Tht");\r
+ socket->Append("Psi");\r
+ socket->Append("Rho");\r
+ socket->Append("Vtotal");\r
+ socket->Append("UBody");\r
+ socket->Append("VBody");\r
+ socket->Append("WBody");\r
+ socket->Append("UAero");\r
+ socket->Append("VAero");\r
+ socket->Append("WAero");\r
+ socket->Append("Vn");\r
+ socket->Append("Ve");\r
+ socket->Append("Vd");\r
+ socket->Append("Udot");\r
+ socket->Append("Vdot");\r
+ socket->Append("Wdot");\r
+ socket->Append("P");\r
+ socket->Append("Q");\r
+ socket->Append("R");\r
+ socket->Append("PDot");\r
+ socket->Append("QDot");\r
+ socket->Append("RDot");\r
+ socket->Append("Fx");\r
+ socket->Append("Fy");\r
+ socket->Append("Fz");\r
+ socket->Append("Latitude");\r
+ socket->Append("Longitude");\r
+ socket->Append("QBar");\r
+ socket->Append("Alpha");\r
+ socket->Append("L");\r
+ socket->Append("M");\r
+ socket->Append("N");\r
+ socket->Append("Throttle Position");\r
+ socket->Append("Left Aileron Position");\r
+ socket->Append("Right Aileron Position");\r
+ socket->Append("Elevator Position");\r
+ socket->Append("Rudder Position");\r
+ sFirstPass = false;\r
+ socket->Send();\r
+ }\r
+\r
+ socket->Clear();\r
+ socket->Append(State->Getsim_time());\r
+ socket->Append(Position->Geth());\r
+ socket->Append(Rotation->Getphi());\r
+ socket->Append(Rotation->Gettht());\r
+ socket->Append(Rotation->Getpsi());\r
+ socket->Append(Atmosphere->GetDensity());\r
+ socket->Append(Translation->GetVt());\r
+ socket->Append(Translation->GetUVW(eU));\r
+ socket->Append(Translation->GetUVW(eV));\r
+ socket->Append(Translation->GetUVW(eW));\r
+ socket->Append(Translation->GetAeroUVW(eU));\r
+ socket->Append(Translation->GetAeroUVW(eV));\r
+ socket->Append(Translation->GetAeroUVW(eW));\r
+ socket->Append(Position->GetVn());\r
+ socket->Append(Position->GetVe());\r
+ socket->Append(Position->GetVd());\r
+ socket->Append(Translation->GetUVWdot(eU));\r
+ socket->Append(Translation->GetUVWdot(eV));\r
+ socket->Append(Translation->GetUVWdot(eW));\r
+ socket->Append(Rotation->GetPQR(eP));\r
+ socket->Append(Rotation->GetPQR(eQ));\r
+ socket->Append(Rotation->GetPQR(eR));\r
+ socket->Append(Rotation->GetPQRdot(eP));\r
+ socket->Append(Rotation->GetPQRdot(eQ));\r
+ socket->Append(Rotation->GetPQRdot(eR));\r
+ socket->Append(Aircraft->GetForces(eX));\r
+ socket->Append(Aircraft->GetForces(eY));\r
+ socket->Append(Aircraft->GetForces(eZ));\r
+ socket->Append(Position->GetLatitude());\r
+ socket->Append(Position->GetLongitude());\r
+ socket->Append(Translation->Getqbar());\r
+ socket->Append(Translation->Getalpha());\r
+ socket->Append(Aircraft->GetMoments(eL));\r
+ socket->Append(Aircraft->GetMoments(eM));\r
+ socket->Append(Aircraft->GetMoments(eN));\r
+ socket->Append(FCS->GetThrottlePos(0));\r
+ socket->Append(FCS->GetDaLPos());\r
+ socket->Append(FCS->GetDaRPos());\r
+ socket->Append(FCS->GetDePos());\r
+ socket->Append(FCS->GetDrPos());\r
+ socket->Send();\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGOutput::SocketStatusOutput(string out_str)\r
+{\r
+ string asciiData;\r
+\r
+ if (socket == NULL) return;\r
+\r
+ socket->Clear();\r
+ asciiData = string("<STATUS>") + out_str;\r
+ socket->Append(asciiData.c_str());\r
+ socket->Send();\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+bool FGOutput::Load(FGConfigFile* AC_cfg)\r
+{\r
+ string token="", parameter="", separator="";\r
+ string name="", fname="";\r
+ int OutRate = 0;\r
+ FGConfigFile* Output_cfg;\r
+ string property;\r
+\r
+# ifndef macintosh\r
+ separator = "/";\r
+# else\r
+ separator = ";";\r
+# endif\r
+\r
+ name = AC_cfg->GetValue("NAME");\r
+ fname = AC_cfg->GetValue("FILE");\r
+ token = AC_cfg->GetValue("TYPE");\r
+ Output->SetType(token);\r
+\r
+#if defined( FG_WITH_JSBSIM_SOCKET ) || !defined( FGFS )\r
+ if (token == "SOCKET") {\r
+ socket = new FGfdmSocket("localhost",1138);\r
+ }\r
+#endif\r
+\r
+ if (!fname.empty()) {\r
+ outputInFileName = FDMExec->GetAircraftPath() + separator\r
+ + FDMExec->GetModelName() + separator + fname + ".xml";\r
+ Output_cfg = new FGConfigFile(outputInFileName);\r
+ if (!Output_cfg->IsOpen()) {\r
+ cerr << "Could not open file: " << outputInFileName << endl;\r
+ return false;\r
+ }\r
+ } else {\r
+ Output_cfg = AC_cfg;\r
+ }\r
+ Output->SetFilename(name);\r
+\r
+ while ((token = Output_cfg->GetValue()) != string("/OUTPUT")) {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "RATE_IN_HZ") {\r
+ *Output_cfg >> OutRate;\r
+ }\r
+ if (parameter == "SIMULATION") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssSimulation;\r
+ }\r
+ if (parameter == "AEROSURFACES") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssAerosurfaces;\r
+ }\r
+ if (parameter == "RATES") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssRates;\r
+ }\r
+ if (parameter == "VELOCITIES") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssVelocities;\r
+ }\r
+ if (parameter == "FORCES") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssForces;\r
+ }\r
+ if (parameter == "MOMENTS") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssMoments;\r
+ }\r
+ if (parameter == "ATMOSPHERE") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssAtmosphere;\r
+ }\r
+ if (parameter == "MASSPROPS") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssMassProps;\r
+ }\r
+ if (parameter == "POSITION") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssPosition;\r
+ }\r
+ if (parameter == "COEFFICIENTS") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssCoefficients;\r
+ }\r
+ if (parameter == "GROUND_REACTIONS") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssGroundReactions;\r
+ }\r
+ if (parameter == "FCS") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssFCS;\r
+ }\r
+ if (parameter == "PROPULSION") {\r
+ *Output_cfg >> parameter;\r
+ if (parameter == "ON") SubSystems += ssPropulsion;\r
+ }\r
+ if (parameter == "PROPERTY") {\r
+ *Output_cfg >> property;\r
+ OutputProperties.push_back(PropertyManager->GetNode(property));\r
+ }\r
+\r
+ if (parameter == "EOF") break;\r
+ }\r
+\r
+ OutRate = OutRate>120?120:(OutRate<0?0:OutRate);\r
+ rate = (int)(0.5 + 1.0/(State->Getdt()*OutRate));\r
+\r
+ Debug(2);\r
+\r
+ return true;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+// The bitmasked value choices are as follows:\r
+// unset: In this case (the default) JSBSim would only print\r
+// out the normally expected messages, essentially echoing\r
+// the config files as they are read. If the environment\r
+// variable is not set, debug_lvl is set to 1 internally\r
+// 0: This requests JSBSim not to output any messages\r
+// whatsoever.\r
+// 1: This value explicity requests the normal JSBSim\r
+// startup messages\r
+// 2: This value asks for a message to be printed out when\r
+// a class is instantiated\r
+// 4: When this value is set, a message is displayed when a\r
+// FGModel object executes its Run() method\r
+// 8: When this value is set, various runtime state variables\r
+// are printed out periodically\r
+// 16: When set various parameters are sanity checked and\r
+// a message is printed out when they go out of bounds\r
+\r
+void FGOutput::Debug(int from)\r
+{\r
+ string scratch="";\r
+\r
+ if (debug_lvl <= 0) return;\r
+\r
+ if (debug_lvl & 1) { // Standard console startup message output\r
+ if (from == 0) { // Constructor\r
+\r
+ }\r
+ if (from == 2) {\r
+ if (outputInFileName.empty())\r
+ cout << " " << "Output parameters read inline" << endl;\r
+ else\r
+ cout << " Output parameters read from file: " << outputInFileName << endl;\r
+\r
+ if (Filename == "cout" || Filename == "COUT") {\r
+ scratch = " Log output goes to screen console";\r
+ } else if (!Filename.empty()) {\r
+ scratch = " Log output goes to file: " + Filename;\r
+ }\r
+ switch (Type) {\r
+ case otCSV:\r
+ cout << scratch << " in CSV format output at rate " << 120/rate << " Hz" << endl;\r
+ break;\r
+ case otNone:\r
+ cout << " No log output" << endl;\r
+ break;\r
+ }\r
+\r
+ if (SubSystems & ssSimulation) cout << " Simulation parameters logged" << endl;\r
+ if (SubSystems & ssAerosurfaces) cout << " Aerosurface parameters logged" << endl;\r
+ if (SubSystems & ssRates) cout << " Rate parameters logged" << endl;\r
+ if (SubSystems & ssVelocities) cout << " Velocity parameters logged" << endl;\r
+ if (SubSystems & ssForces) cout << " Force parameters logged" << endl;\r
+ if (SubSystems & ssMoments) cout << " Moments parameters logged" << endl;\r
+ if (SubSystems & ssAtmosphere) cout << " Atmosphere parameters logged" << endl;\r
+ if (SubSystems & ssMassProps) cout << " Mass parameters logged" << endl;\r
+ if (SubSystems & ssCoefficients) cout << " Coefficient parameters logged" << endl;\r
+ if (SubSystems & ssPosition) cout << " Position parameters logged" << endl;\r
+ if (SubSystems & ssGroundReactions) cout << " Ground parameters logged" << endl;\r
+ if (SubSystems & ssFCS) cout << " FCS parameters logged" << endl;\r
+ if (SubSystems & ssPropulsion) cout << " Propulsion parameters logged" << endl;\r
+ }\r
+ }\r
+ if (debug_lvl & 2 ) { // Instantiation/Destruction notification\r
+ if (from == 0) cout << "Instantiated: FGOutput" << endl;\r
+ if (from == 1) cout << "Destroyed: FGOutput" << endl;\r
+ }\r
+ if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects\r
+ }\r
+ if (debug_lvl & 8 ) { // Runtime state variables\r
+ }\r
+ if (debug_lvl & 16) { // Sanity checking\r
+ }\r
+ if (debug_lvl & 64) {\r
+ if (from == 0) { // Constructor\r
+ cout << IdSrc << endl;\r
+ cout << IdHdr << endl;\r
+ }\r
+ }\r
+}\r
+}\r
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
- 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;
- }
- }
-}
-}
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+ Module: FGPosition.cpp\r
+ Author: Jon S. Berndt\r
+ Date started: 01/05/99\r
+ Purpose: Integrate the EOM to determine instantaneous position\r
+ Called by: FGFDMExec\r
+\r
+ ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------\r
+\r
+ This program is free software; you can redistribute it and/or modify it under\r
+ the terms of the GNU General Public License as published by the Free Software\r
+ Foundation; either version 2 of the License, or (at your option) any later\r
+ version.\r
+\r
+ This program is distributed in the hope that it will be useful, but WITHOUT\r
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS\r
+ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more\r
+ details.\r
+\r
+ You should have received a copy of the GNU General Public License along with\r
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple\r
+ Place - Suite 330, Boston, MA 02111-1307, USA.\r
+\r
+ Further information about the GNU General Public License can also be found on\r
+ the world wide web at http://www.gnu.org.\r
+\r
+FUNCTIONAL DESCRIPTION\r
+--------------------------------------------------------------------------------\r
+This class encapsulates the integration of rates and accelerations to get the\r
+current position of the aircraft.\r
+\r
+HISTORY\r
+--------------------------------------------------------------------------------\r
+01/05/99 JSB Created\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+COMMENTS, REFERENCES, and NOTES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+[1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling\r
+ Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate\r
+ School, January 1994\r
+[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",\r
+ JSC 12960, July 1977\r
+[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at\r
+ NASA-Ames", NASA CR-2497, January 1975\r
+[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",\r
+ Wiley & Sons, 1979 ISBN 0-471-03032-5\r
+[5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,\r
+ 1982 ISBN 0-471-08936-2\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+INCLUDES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+#ifdef FGFS\r
+# include <simgear/compiler.h>\r
+# ifdef SG_HAVE_STD_INCLUDES\r
+# include <cmath>\r
+# include <iomanip>\r
+# else\r
+# include <math.h>\r
+# include <iomanip.h>\r
+# endif\r
+#else\r
+# if defined(sgi) && !defined(__GNUC__)\r
+# include <math.h>\r
+# if (_COMPILER_VERSION < 740)\r
+# include <iomanip.h>\r
+# else\r
+# include <iomanip>\r
+# endif\r
+# else\r
+# include <cmath>\r
+# include <iomanip>\r
+# endif\r
+#endif\r
+\r
+#include "FGPosition.h"\r
+#include "FGState.h"\r
+#include "FGFDMExec.h"\r
+#include "FGAircraft.h"\r
+#include "FGMassBalance.h"\r
+#include "FGTranslation.h"\r
+#include "FGRotation.h"\r
+#include "FGInertial.h"\r
+#include "FGPropertyManager.h"\r
+\r
+namespace JSBSim {\r
+\r
+static const char *IdSrc = "$Id$";\r
+static const char *IdHdr = ID_POSITION;\r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+CLASS IMPLEMENTATION\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex)\r
+{\r
+ Name = "FGPosition";\r
+ LongitudeDot = LatitudeDot = RadiusDot = 0.0;\r
+\r
+ for (int i=0;i<4;i++) {\r
+ LatitudeDot_prev[i] = 0.0;\r
+ LongitudeDot_prev[i] = 0.0;\r
+ RadiusDot_prev[i] = 0.0;\r
+ }\r
+\r
+ vVRPoffset.InitMatrix();\r
+\r
+ Longitude = Latitude = 0.0;\r
+ LongitudeVRP = LatitudeVRP = 0.0;\r
+ gamma = Vt = Vground = 0.0;\r
+ hoverbmac = hoverbcg = 0.0;\r
+ psigt = 0.0;\r
+ bind();\r
+ Debug(0);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGPosition::~FGPosition(void)\r
+{\r
+ unbind();\r
+ Debug(1);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+bool FGPosition::InitModel(void)\r
+{\r
+ FGModel::InitModel();\r
+\r
+ h = 3.0; // Est. height of aircraft cg off runway\r
+ SeaLevelRadius = Inertial->RefRadius(); // For initialization ONLY\r
+ Radius = SeaLevelRadius + h;\r
+ RunwayRadius = SeaLevelRadius;\r
+ DistanceAGL = Radius - RunwayRadius; // Geocentric\r
+ vRunwayNormal(3) = -1.0; // Initialized for standalone mode\r
+ b = 1;\r
+ return true;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+/*\r
+Purpose: Called on a schedule to perform Positioning algorithms\r
+Notes: [TP] Make sure that -Vt <= hdot <= Vt, which, of course, should always\r
+ be the case\r
+ [JB] Run in standalone mode, SeaLevelRadius will be reference radius.\r
+ In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.\r
+*/\r
+\r
+bool FGPosition::Run(void)\r
+{\r
+ double cosLat;\r
+ double hdot_Vt;\r
+\r
+ if (!FGModel::Run()) {\r
+ GetState();\r
+\r
+ Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) );\r
+\r
+ if (vVel(eNorth) == 0) psigt = 0;\r
+ else psigt = atan2(vVel(eEast), vVel(eNorth));\r
+\r
+ if (psigt < 0.0) psigt += 2*M_PI;\r
+\r
+ Radius = h + SeaLevelRadius;\r
+\r
+ cosLat = cos(Latitude);\r
+ if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat);\r
+ LatitudeDot = vVel(eNorth) / Radius;\r
+ RadiusDot = -vVel(eDown);\r
+\r
+ Longitude += State->Integrate(FGState::TRAPZ, dt*rate, LongitudeDot, LongitudeDot_prev);\r
+ Latitude += State->Integrate(FGState::TRAPZ, dt*rate, LatitudeDot, LatitudeDot_prev);\r
+ Radius += State->Integrate(FGState::TRAPZ, dt*rate, RadiusDot, RadiusDot_prev);\r
+\r
+ h = Radius - SeaLevelRadius; // Geocentric\r
+\r
+ vVRPoffset = State->GetTb2l() * MassBalance->StructuralToBody(Aircraft->GetXYZvrp());\r
+\r
+ // vVRP - the vector to the Visual Reference Point - now contains the\r
+ // offset from the CG to the VRP, in units of feet, in the Local coordinate\r
+ // frame, where X points north, Y points East, and Z points down. This needs\r
+ // to be converted to Lat/Lon/Alt, now.\r
+\r
+ if (cosLat != 0)\r
+ LongitudeVRP = vVRPoffset(eEast) / (Radius * cosLat) + Longitude;\r
+\r
+ LatitudeVRP = vVRPoffset(eNorth) / Radius + Latitude;\r
+ hVRP = h - vVRPoffset(eDown);\r
+/*\r
+cout << "Lat/Lon/Alt : " << Latitude << " / " << Longitude << " / " << h << endl;\r
+cout << "Lat/Lon/Alt VRP: " << LatitudeVRP << " / " << LongitudeVRP << " / " << hVRP << endl << endl;\r
+*/\r
+ DistanceAGL = Radius - RunwayRadius; // Geocentric\r
+\r
+ hoverbcg = DistanceAGL/b;\r
+\r
+ vMac = State->GetTb2l()*MassBalance->StructuralToBody(Aircraft->GetXYZrp());\r
+ hoverbmac = (DistanceAGL + vMac(3)) / b;\r
+\r
+ if (Vt > 0) {\r
+ hdot_Vt = RadiusDot/Vt;\r
+ if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt);\r
+ } else {\r
+ gamma = 0.0;\r
+ }\r
+\r
+ return false;\r
+\r
+ } else {\r
+ return true;\r
+ }\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGPosition::GetState(void)\r
+{\r
+ dt = State->Getdt();\r
+\r
+ Vt = Translation->GetVt();\r
+ vVel = State->GetTb2l() * Translation->GetUVW();\r
+ vVelDot = State->GetTb2l() * Translation->GetUVWdot();\r
+\r
+ b = Aircraft->GetWingSpan();\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGPosition::Seth(double tt)\r
+{\r
+ h = tt;\r
+ Radius = h + SeaLevelRadius;\r
+ DistanceAGL = Radius - RunwayRadius; // Geocentric\r
+ hoverbcg = DistanceAGL/b;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGPosition::SetDistanceAGL(double tt)\r
+{\r
+ DistanceAGL=tt;\r
+ Radius = RunwayRadius + DistanceAGL;\r
+ h = Radius - SeaLevelRadius;\r
+ hoverbcg = DistanceAGL/b;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGPosition::bind(void)\r
+{\r
+ PropertyManager->Tie("velocities/v-north-fps", this,\r
+ &FGPosition::GetVn);\r
+ PropertyManager->Tie("velocities/v-east-fps", this,\r
+ &FGPosition::GetVe);\r
+ PropertyManager->Tie("velocities/v-down-fps", this,\r
+ &FGPosition::GetVd);\r
+ PropertyManager->Tie("velocities/vg-fps", this,\r
+ &FGPosition::GetVground);\r
+ PropertyManager->Tie("flight-path/psi-gt-rad", this,\r
+ &FGPosition::GetGroundTrack);\r
+ PropertyManager->Tie("position/h-sl-ft", this,\r
+ &FGPosition::Geth,\r
+ &FGPosition::Seth,\r
+ true);\r
+ PropertyManager->Tie("velocities/h-dot-fps", this,\r
+ &FGPosition::Gethdot);\r
+ PropertyManager->Tie("position/lat-gc-rad", this,\r
+ &FGPosition::GetLatitude,\r
+ &FGPosition::SetLatitude);\r
+ PropertyManager->Tie("position/lat-dot-gc-rad", this,\r
+ &FGPosition::GetLatitudeDot);\r
+ PropertyManager->Tie("position/long-gc-rad", this,\r
+ &FGPosition::GetLongitude,\r
+ &FGPosition::SetLongitude,\r
+ true);\r
+ PropertyManager->Tie("position/long-dot-gc-rad", this,\r
+ &FGPosition::GetLongitudeDot);\r
+ PropertyManager->Tie("metrics/runway-radius", this,\r
+ &FGPosition::GetRunwayRadius,\r
+ &FGPosition::SetRunwayRadius);\r
+ PropertyManager->Tie("position/h-agl-ft", this,\r
+ &FGPosition::GetDistanceAGL,\r
+ &FGPosition::SetDistanceAGL);\r
+ PropertyManager->Tie("position/radius-to-vehicle-ft", this,\r
+ &FGPosition::GetRadius);\r
+ PropertyManager->Tie("flight-path/gamma-rad", this,\r
+ &FGPosition::GetGamma,\r
+ &FGPosition::SetGamma);\r
+ PropertyManager->Tie("aero/h_b-cg-ft", this,\r
+ &FGPosition::GetHOverBCG);\r
+ PropertyManager->Tie("aero/h_b-mac-ft", this,\r
+ &FGPosition::GetHOverBMAC);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGPosition::unbind(void)\r
+{\r
+ PropertyManager->Untie("velocities/v-north-fps");\r
+ PropertyManager->Untie("velocities/v-east-fps");\r
+ PropertyManager->Untie("velocities/v-down-fps");\r
+ PropertyManager->Untie("velocities/vg-fps");\r
+ PropertyManager->Untie("flight-path/psi-gt-rad");\r
+ PropertyManager->Untie("position/h-sl-ft");\r
+ PropertyManager->Untie("velocities/h-dot-fps");\r
+ PropertyManager->Untie("position/lat-gc-rad");\r
+ PropertyManager->Untie("position/lat-dot-gc-rad");\r
+ PropertyManager->Untie("position/long-gc-rad");\r
+ PropertyManager->Untie("position/long-dot-gc-rad");\r
+ PropertyManager->Untie("metrics/runway-radius");\r
+ PropertyManager->Untie("position/h-agl-ft");\r
+ PropertyManager->Untie("position/radius-to-vehicle-ft");\r
+ PropertyManager->Untie("flight-path/gamma-rad");\r
+ PropertyManager->Untie("aero/h_b-cg-ft");\r
+ PropertyManager->Untie("aero/h_b-mac-ft");\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+// The bitmasked value choices are as follows:\r
+// unset: In this case (the default) JSBSim would only print\r
+// out the normally expected messages, essentially echoing\r
+// the config files as they are read. If the environment\r
+// variable is not set, debug_lvl is set to 1 internally\r
+// 0: This requests JSBSim not to output any messages\r
+// whatsoever.\r
+// 1: This value explicity requests the normal JSBSim\r
+// startup messages\r
+// 2: This value asks for a message to be printed out when\r
+// a class is instantiated\r
+// 4: When this value is set, a message is displayed when a\r
+// FGModel object executes its Run() method\r
+// 8: When this value is set, various runtime state variables\r
+// are printed out periodically\r
+// 16: When set various parameters are sanity checked and\r
+// a message is printed out when they go out of bounds\r
+\r
+void FGPosition::Debug(int from)\r
+{\r
+ if (debug_lvl <= 0) return;\r
+\r
+ if (debug_lvl & 1) { // Standard console startup message output\r
+ if (from == 0) { // Constructor\r
+\r
+ }\r
+ }\r
+ if (debug_lvl & 2 ) { // Instantiation/Destruction notification\r
+ if (from == 0) cout << "Instantiated: FGPosition" << endl;\r
+ if (from == 1) cout << "Destroyed: FGPosition" << endl;\r
+ }\r
+ if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects\r
+ }\r
+ if (debug_lvl & 8 ) { // Runtime state variables\r
+ }\r
+ if (debug_lvl & 16) { // Sanity checking\r
+ }\r
+ if (debug_lvl & 64) {\r
+ if (from == 0) { // Constructor\r
+ cout << IdSrc << endl;\r
+ cout << IdHdr << endl;\r
+ }\r
+ }\r
+}\r
+}\r
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGModel.h"
-#include "FGMatrix33.h"
#include "FGColumnVector3.h"
-#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGPropeller.h"
+#include "FGTranslation.h"
+#include "FGRotation.h"
#include "FGFCS.h"
+#include "FGAtmosphere.h"
namespace JSBSim {
#include "FGThruster.h"
#include "FGTable.h"
-#include "FGTranslation.h"
-#include "FGRotation.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
+
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.
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#include <simgear/props/props.hxx>
#include "FGPropertyManager.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-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);
-}
+}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
string FGPropertyManager::GetName( void ) {
return string( getName() );
-}
+}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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)
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);
}
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);
}
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);
}
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
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
+
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include <string>
+#ifdef FGFS
#include <simgear/props/props.hxx>
+#else
+#include "simgear/props/props.hxx"
+#endif
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
/** Class wrapper for property handling.
@author David Megginson, Tony Peden
*/
-
+
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/// 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.
*
* @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);
/**
* 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.
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); */
* @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)
{
* 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)
{
* 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.
*/
"Failed to tie property " << name << " to object methods" << endl;
}
}
-
+
/**
* Tie a property to a pair of indexed object methods.
*
* 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)
"Failed to tie property " << name << " to indexed object methods" << endl;
}
}
-};
+};
}
#endif // FGPROPERTYMANAGER_H
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#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__)
numOxiTanks = numFuelTanks = 0;
dt = 0.0;
ActiveEngine = -1; // -1: ALL, 0: Engine 1, 1: Engine 2 ...
+ tankJ.InitMatrix();
bind();
vForces += Thrusters[i]->GetBodyForces(); // sum body frame forces
vMoments += Thrusters[i]->GetMoments(); // sum body frame moments
}
+
return false;
} else {
return true;
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")) {
AC_cfg->GetNextConfigLine();
}
+ CalculateTankInertias();
if (!ThrottleAdded) FCS->AddThrottle(); // need to have at least one throttle
return true;
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;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-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;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#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
/** 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);
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;
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;
double dt;
FGColumnVector3 vForces;
FGColumnVector3 vMoments;
- FGColumnVector3 vXYZtank;
+ FGColumnVector3 vTankXYZ;
+ FGColumnVector3 vXYZtank_arm;
+ FGMatrix33 tankJ;
void Debug(int from);
};
}
#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"
vPQRdot_prev[3].InitMatrix();
bind();
-
+
Debug(0);
}
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);
{
dt = State->Getdt();
vMoments = Aircraft->GetMoments();
-
- Ixx = MassBalance->GetIxx();
- Iyy = MassBalance->GetIyy();
- Izz = MassBalance->GetIzz();
- Ixz = MassBalance->GetIxz();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endif
#include "FGModel.h"
-#include "FGMatrix33.h"
#include "FGColumnVector3.h"
-#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
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;}
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);
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);
void FGSimTurbine::SetDefaults(void)
{
Name = "Not defined";
+ N1 = N2 = 0.0;
Type = etSimTurbine;
MilThrust = 10000.0;
MaxThrust = 10000.0;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
+
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
{
FDMExec = fdex;
- a = 1000.0;
sim_time = 0.0;
dt = 1.0/120.0;
for(int i=0;i<4;i++) vQdot_prev[i].InitMatrix();
bind();
-
+
Debug(0);
}
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
double U, V, W, h;
double latitude, longitude;
double wnorth,weast, wdown;
-
+
latitude = FGIC->GetLatitudeRadIC();
longitude = FGIC->GetLongitudeRadIC();
h = FGIC->GetAltitudeFtIC();
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);
}
{
float alpha,beta;
float ca, cb, sa, sb;
-
+
alpha = Translation->Getalpha();
beta = Translation->Getbeta();
-
+
ca = cos(alpha);
sa = sin(alpha);
cb = cos(beta);
{
#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
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 );
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
-}
+}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
+
void FGState::unbind(void)
{
PropertyManager->Untie("sim-time-sec");
*/
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.
/// 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.
void unbind();
private:
- double a; // speed of sound
double sim_time, dt;
double saved_dt;
FGColumnVector4 vQtrn;
FGColumnVector4 vQdot_prev[4];
FGColumnVector4 vQdot;
- FGColumnVector3 vUVW;
FGColumnVector3 vLocalVelNED;
FGColumnVector3 vLocalEuler;
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;
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) {
} else {
Contents = 0;
PctFull = 0;
- }
+ }
Debug(0);
}
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;
}
}
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#include "FGConfigFile.h"
#include "FGJSBBase.h"
+#include "FGConfigFile.h"
+#include "FGColumnVector3.h"
#ifdef FGFS
# include <simgear/compiler.h>
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; }
private:
TankType Type;
string type;
- double X, Y, Z;
+ FGColumnVector3 vXYZ;
double Capacity;
double Radius;
double PctFull;
{
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);
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)
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);
#endif
#include "FGModel.h"
-#include "FGMatrix33.h"
#include "FGColumnVector3.h"
-#include "FGColumnVector4.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
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);
#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
void FGTrim::setupPullup() {
float g,q,cgamma;
- FGColumnVector3 vPQR;
g=fdmex->GetInertial()->gravity();
cgamma=cos(fgic->GetFlightPathAngleRadIC());
cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
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();
#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"
/******************************************************************************/
-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
}
fdmex = new FGFDMExec( (FGPropertyManager*)globals->get_props() );
-
+
State = fdmex->GetState();
Atmosphere = fdmex->GetAtmosphere();
FCS = fdmex->GetFCS();
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" );
result = fdmex->LoadModel( aircraft_path.str(),
engine_path.str(),
fgGetString("/sim/aero") );
-
+
if (result) {
SG_LOG( SG_FLIGHT, SG_INFO, " loaded aero.");
} else {
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() );
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());
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);
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);
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);
void FGJSBsim::init()
{
double tmp;
-
+
SG_LOG( SG_FLIGHT, SG_INFO, "Starting and initializing JSBsim" );
// Explicitly call the superclass's
Atmosphere->UseInternal();
}
#endif
-
+
fgic->SetVnorthFpsIC( wind_from_north->getDoubleValue() );
fgic->SetVeastFpsIC( wind_from_east->getDoubleValue() );
fgic->SetVdownFpsIC( wind_from_down->getDoubleValue() );
//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();
<< 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: "
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() );
}
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();
}
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.
_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(
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) );
_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) );
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() );
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() );
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:
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;
}
// 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 );
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);
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);
{
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);
{
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);
{
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);
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);
//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;
}
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)
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)
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() );
- }
+ }
}
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
- 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
-
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+ Module: FGCondition.cpp\r
+ Author: Jon S. Berndt\r
+ Date started: 1/2/2003\r
+\r
+ -------------- Copyright (C) 2003 Jon S. Berndt (jsb@hal-pc.org) --------------\r
+\r
+ This program is free software; you can redistribute it and/or modify it under\r
+ the terms of the GNU General Public License as published by the Free Software\r
+ Foundation; either version 2 of the License, or (at your option) any later\r
+ version.\r
+\r
+ This program is distributed in the hope that it will be useful, but WITHOUT\r
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS\r
+ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more\r
+ details.\r
+\r
+ You should have received a copy of the GNU General Public License along with\r
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple\r
+ Place - Suite 330, Boston, MA 02111-1307, USA.\r
+\r
+ Further information about the GNU General Public License can also be found on\r
+ the world wide web at http://www.gnu.org.\r
+\r
+HISTORY\r
+--------------------------------------------------------------------------------\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+COMMENTS, REFERENCES, and NOTES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+INCLUDES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+#include "FGCondition.h"\r
+\r
+namespace JSBSim {\r
+\r
+static const char *IdSrc = "$Id$";\r
+static const char *IdHdr = ID_CONDITION;\r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+CLASS IMPLEMENTATION\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+string FGCondition::indent = " ";\r
+\r
+\r
+FGCondition::FGCondition(FGConfigFile* AC_cfg, FGPropertyManager* PropertyManager) :\r
+ PropertyManager(PropertyManager)\r
+{\r
+ string property1, property2;\r
+\r
+ mComparison["EQ"] = eEQ;\r
+ mComparison["NE"] = eNE;\r
+ mComparison["GT"] = eGT;\r
+ mComparison["GE"] = eGE;\r
+ mComparison["LT"] = eLT;\r
+ mComparison["LE"] = eLE;\r
+ mComparison["=="] = eEQ;\r
+ mComparison["!="] = eNE;\r
+ mComparison[">"] = eGT;\r
+ mComparison[">="] = eGE;\r
+ mComparison["<"] = eLT;\r
+ mComparison["<="] = eLE;\r
+\r
+ TestParam1 = TestParam2 = 0L;\r
+ TestValue = 0.0;\r
+ Comparison = ecUndef;\r
+ Logic = elUndef;\r
+ conditions.clear();\r
+\r
+ if (AC_cfg->GetValue("CONDITION_GROUP").empty()) { // define a condition\r
+\r
+ *AC_cfg >> property1 >> conditional >> property2;\r
+ TestParam1 = PropertyManager->GetNode(property1, true);\r
+ Comparison = mComparison[conditional];\r
+\r
+ if (property2.find_first_not_of("-.0123456789eE") == string::npos) {\r
+ TestValue = atof(property2.c_str());\r
+ } else {\r
+ TestParam2 = PropertyManager->GetNode(property2, true);\r
+ }\r
+\r
+ isGroup = false;\r
+\r
+ } else { // define a condition group\r
+\r
+ if (AC_cfg->GetValue("LOGIC") == "OR") Logic = eOR;\r
+ else if (AC_cfg->GetValue("LOGIC") == "AND") Logic = eAND;\r
+\r
+ AC_cfg->GetNextConfigLine();\r
+ while (AC_cfg->GetValue() != string("/CONDITION_GROUP")) {\r
+ conditions.push_back(FGCondition(AC_cfg, PropertyManager));\r
+ }\r
+ isGroup = true;\r
+ AC_cfg->GetNextConfigLine();\r
+ }\r
+\r
+ Debug(0);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGCondition::~FGCondition(void)\r
+{\r
+ Debug(1);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+bool FGCondition::Evaluate(void )\r
+{\r
+ vector <FGCondition>::iterator iConditions;\r
+ bool pass = false;\r
+ double compareValue;\r
+\r
+ if (Logic == eAND) {\r
+\r
+ iConditions = conditions.begin();\r
+ pass = true;\r
+ while (iConditions < conditions.end()) {\r
+ if (!iConditions->Evaluate()) pass = false;\r
+ *iConditions++;\r
+ }\r
+\r
+ } else if (Logic == eOR) {\r
+\r
+ pass = false;\r
+ while (iConditions < conditions.end()) {\r
+ if (iConditions->Evaluate()) pass = true;\r
+ *iConditions++;\r
+ }\r
+\r
+ } else {\r
+\r
+ if (TestParam2 != 0L) compareValue = TestParam2->getDoubleValue();\r
+ else compareValue = TestValue;\r
+\r
+ switch (Comparison) {\r
+ case ecUndef:\r
+ cerr << "Undefined comparison operator." << endl;\r
+ break;\r
+ case eEQ:\r
+ pass = TestParam1->getDoubleValue() == compareValue;\r
+ break;\r
+ case eNE:\r
+ pass = TestParam1->getDoubleValue() != compareValue;\r
+ break;\r
+ case eGT:\r
+ pass = TestParam1->getDoubleValue() > compareValue;\r
+ break;\r
+ case eGE:\r
+ pass = TestParam1->getDoubleValue() >= compareValue;\r
+ break;\r
+ case eLT:\r
+ pass = TestParam1->getDoubleValue() < compareValue;\r
+ break;\r
+ case eLE:\r
+ pass = TestParam1->getDoubleValue() <= compareValue;\r
+ break;\r
+ default:\r
+ cerr << "Unknown comparison operator." << endl;\r
+ }\r
+ }\r
+\r
+ return pass;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+void FGCondition::PrintCondition(void )\r
+{\r
+ vector <FGCondition>::iterator iConditions;\r
+ string scratch;\r
+\r
+ if (isGroup) {\r
+ switch(Logic) {\r
+ case (elUndef):\r
+ scratch = " UNSET";\r
+ cerr << "unset logic for test condition" << endl;\r
+ break;\r
+ case (eAND):\r
+ scratch = " if all of the following are true";\r
+ break;\r
+ case (eOR):\r
+ scratch = " if any of the following are true:";\r
+ break;\r
+ default:\r
+ scratch = " UNKNOWN";\r
+ cerr << "Unknown logic for test condition" << endl;\r
+ }\r
+\r
+ iConditions = conditions.begin();\r
+ cout << scratch << endl;\r
+ while (iConditions < conditions.end()) {\r
+ iConditions->PrintCondition();\r
+ *iConditions++;\r
+ }\r
+ } else {\r
+ if (TestParam2 != 0L)\r
+ cout << TestParam1->GetName() << " " << conditional << " " << TestParam2->GetName();\r
+ else\r
+ cout << TestParam1->GetName() << " " << conditional << " " << TestValue;\r
+ }\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+// The bitmasked value choices are as follows:\r
+// unset: In this case (the default) JSBSim would only print\r
+// out the normally expected messages, essentially echoing\r
+// the config files as they are read. If the environment\r
+// variable is not set, debug_lvl is set to 1 internally\r
+// 0: This requests JSBSim not to output any messages\r
+// whatsoever.\r
+// 1: This value explicity requests the normal JSBSim\r
+// startup messages\r
+// 2: This value asks for a message to be printed out when\r
+// a class is instantiated\r
+// 4: When this value is set, a message is displayed when a\r
+// FGModel object executes its Run() method\r
+// 8: When this value is set, various runtime state variables\r
+// are printed out periodically\r
+// 16: When set various parameters are sanity checked and\r
+// a message is printed out when they go out of bounds\r
+\r
+void FGCondition::Debug(int from)\r
+{\r
+ if (debug_lvl <= 0) return;\r
+\r
+ if (debug_lvl & 1) { // Standard console startup message output\r
+ if (from == 0) { // Constructor\r
+\r
+ }\r
+ }\r
+ if (debug_lvl & 2 ) { // Instantiation/Destruction notification\r
+ if (from == 0) cout << "Instantiated: FGCondition" << endl;\r
+ if (from == 1) cout << "Destroyed: FGCondition" << endl;\r
+ }\r
+ if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects\r
+ }\r
+ if (debug_lvl & 8 ) { // Runtime state variables\r
+ }\r
+ if (debug_lvl & 16) { // Sanity checking\r
+ }\r
+ if (debug_lvl & 64) {\r
+ if (from == 0) { // Constructor\r
+ cout << IdSrc << endl;\r
+ cout << IdHdr << endl;\r
+ }\r
+ }\r
+}\r
+\r
+} //namespace JSBSim\r
+\r
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
+
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 {
Detents.clear();
TransitionTimes.clear();
-
+
OutputPct=0;
- InTransit=0;
+ DoScale = true;
Type = AC_cfg->GetValue("TYPE");
Name = AC_cfg->GetValue("NAME");
} 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;
IsOutput = true;
*AC_cfg >> sOutputIdx;
- OutputNode = PropertyManager->GetNode(sOutputIdx);
+ OutputNode = PropertyManager->GetNode(sOutputIdx, true);
+ } else if (token == "NOSCALE") {
+
+ DoScale = false;
}
}
FGFCSComponent::bind();
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-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;
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
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);
};
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
- 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
-
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+ Module: FGSwitch.cpp\r
+ Author: Jon S. Berndt\r
+ Date started: 4/2000\r
+\r
+ ------------- Copyright (C) 2000 -------------\r
+\r
+ This program is free software; you can redistribute it and/or modify it under\r
+ the terms of the GNU General Public License as published by the Free Software\r
+ Foundation; either version 2 of the License, or (at your option) any later\r
+ version.\r
+\r
+ This program is distributed in the hope that it will be useful, but WITHOUT\r
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS\r
+ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more\r
+ details.\r
+\r
+ You should have received a copy of the GNU General Public License along with\r
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple\r
+ Place - Suite 330, Boston, MA 02111-1307, USA.\r
+\r
+ Further information about the GNU General Public License can also be found on\r
+ the world wide web at http://www.gnu.org.\r
+\r
+FUNCTIONAL DESCRIPTION\r
+--------------------------------------------------------------------------------\r
+\r
+HISTORY\r
+--------------------------------------------------------------------------------\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+COMMENTS, REFERENCES, and NOTES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+The SWITCH component is defined as follows (see the API documentation for more\r
+information):\r
+\r
+<COMPONENT NAME="switch1" TYPE="SWITCH">\r
+ <TEST LOGIC="{AND|OR|DEFAULT}" OUTPUT="{property|value}">\r
+ {property} {conditional} {property|value}\r
+ <CONDITION_GROUP LOGIC="{AND|OR}">\r
+ {property} {conditional} {property|value}\r
+ ...\r
+ </CONDITION_GROUP>\r
+ ...\r
+ </TEST>\r
+ <TEST LOGIC="{AND|OR}" OUTPUT="{property|value}">\r
+ {property} {conditional} {property|value}\r
+ ...\r
+ </TEST>\r
+ ...\r
+</COMPONENT>\r
+\r
+Also, see the header file (FGSwitch.h) for further details.\r
+\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+INCLUDES\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+#include "FGSwitch.h"\r
+\r
+namespace JSBSim {\r
+\r
+static const char *IdSrc = "$Id$";\r
+static const char *IdHdr = ID_SWITCH;\r
+\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+CLASS IMPLEMENTATION\r
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
+\r
+\r
+FGSwitch::FGSwitch(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),\r
+ AC_cfg(AC_cfg)\r
+{\r
+ string token, value;\r
+ struct test *current_test;\r
+ struct FGCondition *current_condition;\r
+\r
+ Type = AC_cfg->GetValue("TYPE");\r
+ Name = AC_cfg->GetValue("NAME");\r
+\r
+ AC_cfg->GetNextConfigLine();\r
+ while ((token = AC_cfg->GetValue()) != string("/COMPONENT")) {\r
+\r
+ // See the above documentation, or the API docs, for information on what\r
+ // the SWITCH component is supposed to look like in the configuration file.\r
+ // Below, the switch component is read in.\r
+\r
+ if (token == "TEST") {\r
+ tests.push_back(test());\r
+ current_test = &tests.back();\r
+\r
+ if (AC_cfg->GetValue("LOGIC") == "OR") {\r
+ current_test->Logic = eOR;\r
+ } else if (AC_cfg->GetValue("LOGIC") == "AND") {\r
+ current_test->Logic = eAND;\r
+ } else if (AC_cfg->GetValue("LOGIC") == "DEFAULT") {\r
+ current_test->Logic = eDefault;\r
+ } else { // error\r
+ cerr << "Unrecognized LOGIC token in switch component: " << Name << endl;\r
+ }\r
+\r
+ value = AC_cfg->GetValue("VALUE");\r
+ if (value.empty()) {\r
+ cerr << "No VALUE supplied for switch component: " << Name << endl;\r
+ } else {\r
+ if (value.find_first_not_of("-.0123456789eE") == string::npos) {\r
+ // if true (and execution falls into this block), "value" is a number.\r
+ current_test->OutputVal = atof(value.c_str());\r
+ } else {\r
+ // "value" must be a property if execution passes to here.\r
+ if (value[0] == '-') {\r
+ current_test->sign = -1.0;\r
+ value.erase(0,1);\r
+ } else {\r
+ current_test->sign = 1.0;\r
+ }\r
+ current_test->OutputProp = PropertyManager->GetNode(value);\r
+ }\r
+ }\r
+\r
+ AC_cfg->GetNextConfigLine();\r
+ while (AC_cfg->GetValue() != string("/TEST")) {\r
+ current_test->conditions.push_back(FGCondition(AC_cfg, PropertyManager));\r
+ }\r
+ }\r
+ AC_cfg->GetNextConfigLine();\r
+ }\r
+\r
+ FGFCSComponent::bind();\r
+\r
+ Debug(0);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+FGSwitch::~FGSwitch()\r
+{\r
+ Debug(1);\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+\r
+bool FGSwitch::Run(void )\r
+{\r
+ vector <test>::iterator iTests = tests.begin();\r
+ vector <FGCondition>::iterator iConditions;\r
+ bool pass = false;\r
+\r
+ FGFCSComponent::Run(); // call the base class for initialization of Input\r
+\r
+ while (iTests < tests.end()) {\r
+ iConditions = iTests->conditions.begin();\r
+\r
+ if (iTests->Logic == eDefault) {\r
+ Output = iTests->GetValue();\r
+ } else if (iTests->Logic == eAND) {\r
+ pass = true;\r
+ while (iConditions < iTests->conditions.end()) {\r
+ if (!iConditions->Evaluate()) pass = false;\r
+ *iConditions++;\r
+ }\r
+ } else if (iTests->Logic == eOR) {\r
+ pass = false;\r
+ while (iConditions < iTests->conditions.end()) {\r
+ if (iConditions->Evaluate()) pass = true;\r
+ *iConditions++;\r
+ }\r
+ } else {\r
+ cerr << "Invalid logic test" << endl;\r
+ }\r
+\r
+ if (pass) {\r
+ Output = iTests->GetValue();\r
+ break;\r
+ }\r
+ *iTests++;\r
+ }\r
+\r
+ return true;\r
+}\r
+\r
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
+// The bitmasked value choices are as follows:\r
+// unset: In this case (the default) JSBSim would only print\r
+// out the normally expected messages, essentially echoing\r
+// the config files as they are read. If the environment\r
+// variable is not set, debug_lvl is set to 1 internally\r
+// 0: This requests JSBSim not to output any messages\r
+// whatsoever.\r
+// 1: This value explicity requests the normal JSBSim\r
+// startup messages\r
+// 2: This value asks for a message to be printed out when\r
+// a class is instantiated\r
+// 4: When this value is set, a message is displayed when a\r
+// FGModel object executes its Run() method\r
+// 8: When this value is set, various runtime state variables\r
+// are printed out periodically\r
+// 16: When set various parameters are sanity checked and\r
+// a message is printed out when they go out of bounds\r
+\r
+void FGSwitch::Debug(int from)\r
+{\r
+ vector <test>::iterator iTests = tests.begin();\r
+ vector <FGCondition>::iterator iConditions;\r
+ string comp, scratch;\r
+ string indent = " ";\r
+ bool first = false;\r
+\r
+ if (debug_lvl <= 0) return;\r
+\r
+ if (debug_lvl & 1) { // Standard console startup message output\r
+ if (from == 0) { // Constructor\r
+ while (iTests < tests.end()) {\r
+\r
+ scratch = " if ";\r
+\r
+ switch(iTests->Logic) {\r
+ case (elUndef):\r
+ comp = " UNSET ";\r
+ cerr << "Unset logic for test condition" << endl;\r
+ break;\r
+ case (eAND):\r
+ comp = " AND ";\r
+ break;\r
+ case (eOR):\r
+ comp=" OR ";\r
+ break;\r
+ case (eDefault):\r
+ scratch = " by default.";\r
+ break;\r
+ default:\r
+ comp = " UNKNOWN ";\r
+ cerr << "Unknown logic for test condition" << endl;\r
+ }\r
+\r
+ if (iTests->OutputProp != 0L)\r
+ if (iTests->sign < 0)\r
+ cout << indent << "Switch VALUE is - " << iTests->OutputProp->GetName() << scratch << endl;\r
+ else\r
+ cout << indent << "Switch VALUE is " << iTests->OutputProp->GetName() << scratch << endl;\r
+ else\r
+ cout << indent << "Switch VALUE is " << iTests->OutputVal << scratch << endl;\r
+\r
+ iConditions = iTests->conditions.begin();\r
+ first = true;\r
+ while (iConditions < iTests->conditions.end()) {\r
+ if (!first) cout << indent << comp << " ";\r
+ else cout << indent << " ";\r
+ first = false;\r
+ iConditions->PrintCondition();\r
+ cout << endl;\r
+ *iConditions++;\r
+ }\r
+ cout << endl;\r
+ *iTests++;\r
+ }\r
+ }\r
+ }\r
+ if (debug_lvl & 2 ) { // Instantiation/Destruction notification\r
+ if (from == 0) cout << "Instantiated: FGSwitch" << endl;\r
+ if (from == 1) cout << "Destroyed: FGSwitch" << endl;\r
+ }\r
+ if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects\r
+ }\r
+ if (debug_lvl & 8 ) { // Runtime state variables\r
+ }\r
+ if (debug_lvl & 16) { // Sanity checking\r
+ }\r
+ if (debug_lvl & 64) {\r
+ if (from == 0) { // Constructor\r
+ cout << IdSrc << endl;\r
+ cout << IdHdr << endl;\r
+ }\r
+ }\r
+}\r
+\r
+} //namespace JSBSim\r
+\r
@author Jon S. Berndt
@version $Id$
*/
-
+
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
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;
};
vector <test> tests;
-
+
void Debug(int from);
};
}
projects / flightgear.git / commitdiff