fly/flightgear
1
0
Fork 0
Code Activity

Merge branch 'next' of git@gitorious.org:fg/flightgear into next

Browse source
This commit is contained in:
Durk Talsma 2010-07-18 15:46:38 +02:00
commit 10366f4f1b
157 changed files with 4592 additions and 1542 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
projects/VC90/FlightGear/FlightGear.vcproj
View file

@ -952,6 +952,14 @@
RelativePath="..\..\..\src\Fdm\JSBSim\math\FGCondition.h"
>
</File>
<File
RelativePath="..\..\..\src\Fdm\JSBSim\math\FGRungeKutta.cpp"
>
</File>
<File
RelativePath="..\..\..\src\Fdm\JSBSim\math\FGRungeKutta.h"
>
</File>
<File
RelativePath="..\..\..\src\Fdm\JSBSim\math\FGFunction.cpp"
>

2
src/Aircraft/replay.cxx
View file

@ -163,7 +163,7 @@ void FGReplay::update( double dt ) {
//FGProps2NetFDM( &f, false );
// sanity check, don't collect data if FDM data isn't good
if (!fgGetBool("/sim/signals/fdm-initialized", false)) {
if (!fgGetBool("/sim/fdm-initialized", false)) {
return;
}

53
src/Environment/environment_ctrl.cxx
View file

@ -183,6 +183,14 @@ FGInterpolateEnvironmentCtrl::read_table (const SGPropertyNode * node, vector<bu
if( sort_required )
sort(table.begin(), table.end(), bucket::lessThan);
// cleanup entries with (almost)same altitude
for( vector<bucket *>::size_type n = 1; n < table.size(); n++ ) {
if( fabs(table[n]->altitude_ft - table[n-1]->altitude_ft ) < 1 ) {
SG_LOG( SG_GENERAL, SG_ALERT, "Removing duplicate altitude entry in environment config for altitude " << table[n]->altitude_ft );
table.erase( table.begin() + n );
}
}
}
void
@ -196,7 +204,7 @@ FGInterpolateEnvironmentCtrl::update (double delta_time_sec)
int length = _boundary_table.size();
if (length > 0) {
// boundary table
// boundary table
double boundary_limit = _boundary_table[length-1]->altitude_ft;
if (boundary_limit >= altitude_agl_ft) {
do_interpolate(_boundary_table, altitude_agl_ft, _environment);
@ -204,16 +212,16 @@ FGInterpolateEnvironmentCtrl::update (double delta_time_sec)
} else if ((boundary_limit + boundary_transition) >= altitude_agl_ft) {
//TODO: this is 500ft above the top altitude of boundary layer
//shouldn't this be +/-250 ft off of the top altitude?
// both tables
// both tables
do_interpolate(_boundary_table, altitude_agl_ft, &env1);
do_interpolate(_aloft_table, altitude_ft, &env2);
double fraction =
(altitude_agl_ft - boundary_limit) / boundary_transition;
double fraction = boundary_transition > SGLimitsd::min() ?
(altitude_agl_ft - boundary_limit) / boundary_transition : 1.0;
interpolate(&env1, &env2, fraction, _environment);
return;
}
}
// aloft table
// aloft table
do_interpolate(_aloft_table, altitude_ft, _environment);
}
@ -224,31 +232,26 @@ FGInterpolateEnvironmentCtrl::do_interpolate (vector<bucket *> &table, double al
if (length == 0)
return;
// Boundary conditions
// Boundary conditions
if ((length == 1) || (table[0]->altitude_ft >= altitude_ft)) {
environment->copy(table[0]->environment);
environment->copy(table[0]->environment); // below bottom of table
return;
} else if (table[length-1]->altitude_ft <= altitude_ft) {
environment->copy(table[length-1]->environment);
environment->copy(table[length-1]->environment); // above top of table
return;
}
// Search the interpolation table
for (int i = 0; i < length - 1; i++) {
if ((i == length - 1) || (table[i]->altitude_ft <= altitude_ft)) {
FGEnvironment * env1 = &(table[i]->environment);
FGEnvironment * env2 = &(table[i+1]->environment);
double fraction;
if (table[i]->altitude_ft == table[i+1]->altitude_ft)
fraction = 1.0;
else
fraction =
((altitude_ft - table[i]->altitude_ft) /
(table[i+1]->altitude_ft - table[i]->altitude_ft));
interpolate(env1, env2, fraction, environment);
return;
}
}
// Search the interpolation table
int layer;
for ( layer = 1; // can't be below bottom layer, handled above
layer < length && table[layer]->altitude_ft <= altitude_ft;
layer++);
FGEnvironment * env1 = &(table[layer-1]->environment);
FGEnvironment * env2 = &(table[layer]->environment);
// two layers of same altitude were sorted out in read_table
double fraction = ((altitude_ft - table[layer-1]->altitude_ft) /
(table[layer]->altitude_ft - table[layer-1]->altitude_ft));
interpolate(env1, env2, fraction, environment);
}
bool
@ -747,7 +750,7 @@ void FGMetarCtrl::set_metar( const char * metar_string )
m = new FGMetar( metar_string );
}
catch( sg_io_exception ) {
fprintf( stderr, "can't get metar: %s\n", metar_string );
SG_LOG( SG_GENERAL, SG_WARN, "Can't get metar: " << metar_string );
metar_valid = false;
return;
}

122
src/FDM/JSBSim/FGFDMExec.cpp
View file

@ -42,7 +42,6 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGFDMExec.h"
#include "FGState.h"
#include "models/FGAtmosphere.h"
#include "models/atmosphere/FGMSIS.h"
#include "models/atmosphere/FGMars.h"
@ -72,7 +71,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGFDMExec.cpp,v 1.78 2010/04/12 12:25:19 jberndt Exp $";
static const char *IdHdr = ID_FDMEXEC;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -111,7 +110,6 @@ FGFDMExec::FGFDMExec(FGPropertyManager* root) : Root(root)
Frame = 0;
Error = 0;
GroundCallback = 0;
State = 0;
Atmosphere = 0;
FCS = 0;
Propulsion = 0;
@ -129,11 +127,17 @@ FGFDMExec::FGFDMExec(FGPropertyManager* root) : Root(root)
Trim = 0;
Script = 0;
RootDir = "";
modelLoaded = false;
IsChild = false;
holding = false;
Terminate = false;
sim_time = 0.0;
dT = 1.0/120.0; // a default timestep size. This is needed for when JSBSim is
// run in standalone mode with no initialization file.
IdFDM = FDMctr; // The main (parent) JSBSim instance is always the "zeroth"
FDMctr++; // instance. "child" instances are loaded last.
@ -169,6 +173,9 @@ FGFDMExec::FGFDMExec(FGPropertyManager* root) : Root(root)
instance->Tie("simulation/do_simple_trim", this, (iPMF)0, &FGFDMExec::DoTrim);
instance->Tie("simulation/reset", this, (iPMF)0, &FGFDMExec::ResetToInitialConditions);
instance->Tie("simulation/terminate", (int *)&Terminate);
instance->Tie("simulation/sim-time-sec", this, &FGFDMExec::GetSimTime);
instance->Tie("simulation/jsbsim-debug", this, &FGFDMExec::GetDebugLevel, &FGFDMExec::SetDebugLevel);
Constructing = false;
}
@ -217,15 +224,11 @@ bool FGFDMExec::Allocate(void)
Auxiliary = new FGAuxiliary(this);
Input = new FGInput(this);
State = new FGState(this); // This must be done here, as the FGState
// class needs valid pointers to the above
// model classes
// Schedule a model. The second arg (the integer) is the pass number. For
// instance, the atmosphere model could get executed every fifth pass it is called.
Schedule(Input, 1);
Schedule(Atmosphere, 30);
Schedule(Atmosphere, 1);
Schedule(FCS, 1);
Schedule(Propulsion, 1);
Schedule(MassBalance, 1);
@ -267,7 +270,6 @@ bool FGFDMExec::DeAllocate(void)
delete Aircraft;
delete Propagate;
delete Auxiliary;
delete State;
delete Script;
for (unsigned i=0; i<Outputs.size(); i++) delete Outputs[i];
@ -280,7 +282,6 @@ bool FGFDMExec::DeAllocate(void)
Error = 0;
State = 0;
Input = 0;
Atmosphere = 0;
FCS = 0;
@ -324,13 +325,13 @@ bool FGFDMExec::Run(void)
}
// returns true if success, false if complete
if (Script != 0 && !State->IntegrationSuspended()) success = Script->RunScript();
if (Script != 0 && !IntegrationSuspended()) success = Script->RunScript();
vector <FGModel*>::iterator it;
for (it = Models.begin(); it != Models.end(); ++it) (*it)->Run();
Frame++;
if (!Holding()) State->IncrTime();
if (!Holding()) IncrTime();
if (Terminate) success = false;
return (success);
@ -341,14 +342,49 @@ bool FGFDMExec::Run(void)
bool FGFDMExec::RunIC(void)
{
State->SuspendIntegration();
State->Initialize(IC);
SuspendIntegration(); // saves the integration rate, dt, then sets it to 0.0.
Initialize(IC);
Run();
State->ResumeIntegration();
ResumeIntegration(); // Restores the integration rate to what it was.
return true;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGFDMExec::Initialize(FGInitialCondition *FGIC)
{
Propagate->SetInitialState( FGIC );
Atmosphere->Run();
Atmosphere->SetWindNED( FGIC->GetWindNFpsIC(),
FGIC->GetWindEFpsIC(),
FGIC->GetWindDFpsIC() );
FGColumnVector3 vAeroUVW;
vAeroUVW = Propagate->GetUVW() + Propagate->GetTl2b()*Atmosphere->GetTotalWindNED();
double alpha, beta;
if (vAeroUVW(eW) != 0.0)
alpha = vAeroUVW(eU)*vAeroUVW(eU) > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0;
else
alpha = 0.0;
if (vAeroUVW(eV) != 0.0)
beta = vAeroUVW(eU)*vAeroUVW(eU)+vAeroUVW(eW)*vAeroUVW(eW) > 0.0 ? atan2(vAeroUVW(eV), (fabs(vAeroUVW(eU))/vAeroUVW(eU))*sqrt(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW))) : 0.0;
else
beta = 0.0;
Auxiliary->SetAB(alpha, beta);
double Vt = vAeroUVW.Magnitude();
Auxiliary->SetVt(Vt);
Auxiliary->SetMach(Vt/Atmosphere->GetSoundSpeed());
double qbar = 0.5*Vt*Vt*Atmosphere->GetDensity();
Auxiliary->Setqbar(qbar);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
// A private, internal function call for Tie-ing to a property, so it needs an
@ -388,20 +424,6 @@ void FGFDMExec::SetGroundCallback(FGGroundCallback* p)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGFDMExec::GetSimTime(void)
{
return (State->Getsim_time());
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGFDMExec::GetDeltaT(void)
{
return (State->Getdt());
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
vector <string> FGFDMExec::EnumerateFDMs(void)
{
vector <string> FDMList;
@ -417,12 +439,12 @@ vector <string> FGFDMExec::EnumerateFDMs(void)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGFDMExec::LoadScript(string script)
bool FGFDMExec::LoadScript(string script, double deltaT)
{
bool result;
Script = new FGScript(this);
result = Script->LoadScript(script);
result = Script->LoadScript(RootDir + script, deltaT);
return result;
}
@ -432,9 +454,9 @@ bool FGFDMExec::LoadScript(string script)
bool FGFDMExec::LoadModel(string AircraftPath, string EnginePath, string SystemsPath,
string model, bool addModelToPath)
{
FGFDMExec::AircraftPath = AircraftPath;
FGFDMExec::EnginePath = EnginePath;
FGFDMExec::SystemsPath = SystemsPath;
FGFDMExec::AircraftPath = RootDir + AircraftPath;
FGFDMExec::EnginePath = RootDir + EnginePath;
FGFDMExec::SystemsPath = RootDir + SystemsPath;
return LoadModel(model, addModelToPath);
}
@ -445,7 +467,6 @@ bool FGFDMExec::LoadModel(string model, bool addModelToPath)
{
string token;
string aircraftCfgFileName;
string separator = "/";
Element* element = 0L;
bool result = false; // initialize result to false, indicating input file not yet read
@ -458,8 +479,8 @@ bool FGFDMExec::LoadModel(string model, bool addModelToPath)
}
FullAircraftPath = AircraftPath;
if (addModelToPath) FullAircraftPath += separator + model;
aircraftCfgFileName = FullAircraftPath + separator + model + ".xml";
if (addModelToPath) FullAircraftPath += "/" + model;
aircraftCfgFileName = FullAircraftPath + "/" + model + ".xml";
if (modelLoaded) {
DeAllocate();
@ -644,6 +665,9 @@ bool FGFDMExec::LoadModel(string model, bool addModelToPath)
modelLoaded = true;
MassBalance->Run(); // Update all mass properties for the report.
MassBalance->GetMassPropertiesReport();
if (debug_lvl > 0) {
cout << endl << fgblue << highint
<< "End of vehicle configuration loading." << endl
@ -893,15 +917,17 @@ bool FGFDMExec::SetOutputDirectives(string fname)
bool result;
FGOutput* Output = new FGOutput(this);
Output->SetDirectivesFile(fname);
Output->SetDirectivesFile(RootDir + fname);
Output->InitModel();
Schedule(Output, 1);
Schedule(Output, 1);
result = Output->Load(0);
Outputs.push_back(Output);
typedef int (FGOutput::*iOPMF)(void) const;
string outputProp = CreateIndexedPropertyName("simulation/output",Outputs.size()-1);
instance->Tie(outputProp+"/log_rate_hz", Output, (iOPMF)0, &FGOutput::SetRate);
if (result) {
Outputs.push_back(Output);
typedef int (FGOutput::*iOPMF)(void) const;
string outputProp = CreateIndexedPropertyName("simulation/output",Outputs.size()-1);
instance->Tie(outputProp+"/log_rate_hz", Output, (iOPMF)0, &FGOutput::SetRate);
}
return result;
}
@ -918,11 +944,11 @@ void FGFDMExec::DoTrim(int mode)
cerr << endl << "Illegal trimming mode!" << endl << endl;
return;
}
saved_time = State->Getsim_time();
saved_time = sim_time;
FGTrim trim(this, (JSBSim::TrimMode)mode);
if ( !trim.DoTrim() ) cerr << endl << "Trim Failed" << endl << endl;
trim.Report();
State->Setsim_time(saved_time);
sim_time = saved_time;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -936,7 +962,7 @@ void FGFDMExec::DoTrimAnalysis(int mode)
cerr << endl << "Illegal trimming mode!" << endl << endl;
return;
}
saved_time = State->Getsim_time();
saved_time = sim_time;
FGTrimAnalysis trimAnalysis(this, (JSBSim::TrimAnalysisMode)mode);
@ -950,7 +976,7 @@ void FGFDMExec::DoTrimAnalysis(int mode)
if ( !result ) cerr << endl << "Trim Failed" << endl << endl;
trimAnalysis.Report();
State->Setsim_time(saved_time);
Setsim_time(saved_time);
EnableOutput();
cout << "\nOutput: " << GetOutputFileName() << endl;
@ -1025,7 +1051,7 @@ void FGFDMExec::Debug(int from)
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
if (from == 2) {
cout << "================== Frame: " << Frame << " Time: "
<< State->Getsim_time() << " dt: " << State->Getdt() << endl;
<< sim_time << " dt: " << dT << endl;
}
}
if (debug_lvl & 8 ) { // Runtime state variables

74
src/FDM/JSBSim/FGFDMExec.h
View file

@ -60,7 +60,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_FDMEXEC "$Id$"
#define ID_FDMEXEC "$Id: FGFDMExec.h,v 1.52 2010/07/04 13:50:21 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -169,7 +169,7 @@ CLASS DOCUMENTATION
property actually maps toa function call of DoTrim().
@author Jon S. Berndt
@version $Revision$
@version $Revision: 1.52 $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -270,23 +270,23 @@ public:
/** Loads a script
@param Script the full path name and file name for the script to be loaded.
@return true if successfully loadsd; false otherwise. */
bool LoadScript(string Script);
bool LoadScript(string Script, double deltaT);
/** Sets the path to the engine config file directories.
@param path path to the directory under which engine config
files are kept, for instance "engine" */
bool SetEnginePath(string path) { EnginePath = path; return true; }
bool SetEnginePath(string path) { EnginePath = RootDir + path; return true; }
/** Sets the path to the aircraft config file directories.
@param path path to the aircraft directory. For instance:
"aircraft". Under aircraft, then, would be directories for various
modeled aircraft such as C172/, x15/, etc. */
bool SetAircraftPath(string path) { AircraftPath = path; return true; }
bool SetAircraftPath(string path) { AircraftPath = RootDir + path; return true; }
/** Sets the path to the systems config file directories.
@param path path to the directory under which systems config
files are kept, for instance "systems" */
bool SetSystemsPath(string path) { SystemsPath = path; return true; }
bool SetSystemsPath(string path) { SystemsPath = RootDir + path; return true; }
/// @name Top-level executive State and Model retrieval mechanism
//@{
@ -318,8 +318,6 @@ public:
inline FGInput* GetInput(void) {return Input;}
/// Returns the FGGroundCallback pointer.
inline FGGroundCallback* GetGroundCallback(void) {return GroundCallback;}
/// Returns the FGState pointer.
inline FGState* GetState(void) {return State;}
/// Retrieves the script object
inline FGScript* GetScript(void) {return Script;}
// Returns a pointer to the FGInitialCondition object
@ -351,19 +349,19 @@ public:
/// Returns the model name.
string GetModelName(void) { return modelName; }
/*
/// Returns the current time.
double GetSimTime(void);
/// Returns the current frame time (delta T).
double GetDeltaT(void);
*/
/// Returns a pointer to the property manager object.
FGPropertyManager* GetPropertyManager(void);
/// Returns a vector of strings representing the names of all loaded models (future)
vector <string> EnumerateFDMs(void);
/// Gets the number of child FDMs.
int GetFDMCount(void) {return ChildFDMList.size();}
int GetFDMCount(void) {return (int)ChildFDMList.size();}
/// Gets a particular child FDM.
childData* GetChildFDM(int i) {return ChildFDMList[i];}
/// Marks this instance of the Exec object as a "child" object.
@ -454,6 +452,8 @@ public:
// Print the contents of the property catalog for the loaded aircraft.
void PrintPropertyCatalog(void);
vector<string>& GetPropertyCatalog(void) {return PropertyCatalog;}
/// Use the MSIS atmosphere model.
void UseAtmosphereMSIS(void);
@ -465,12 +465,61 @@ public:
void SetTrimMode(int mode){ ta_mode = mode; }
int GetTrimMode(void) const { return ta_mode; }
/// Returns the cumulative simulation time in seconds.
double GetSimTime(void) const { return sim_time; }
/// Returns the simulation delta T.
double GetDeltaT(void) {return dT;}
/// Suspends the simulation and sets the delta T to zero.
void SuspendIntegration(void) {saved_dT = dT; dT = 0.0;}
/// Resumes the simulation by resetting delta T to the correct value.
void ResumeIntegration(void) {dT = saved_dT;}
/** Returns the simulation suspension state.
@return true if suspended, false if executing */
bool IntegrationSuspended(void) {return dT == 0.0;}
/** Sets the current sim time.
@param cur_time the current time
@return the current simulation time. */
double Setsim_time(double cur_time) {
sim_time = cur_time;
return sim_time;
}
/** Sets the integration time step for the simulation executive.
@param delta_t the time step in seconds. */
void Setdt(double delta_t) { dT = delta_t; }
/** Sets the root directory where JSBSim starts looking for its system directories.
@param rootDir the string containing the root directory. */
void SetRootDir(string rootDir) {RootDir = rootDir;}
/** Retrieves teh Root Directory.
@return the string representing the root (base) JSBSim directory. */
string GetRootDir(void) const {return RootDir;}
/** Increments the simulation time.
@return the new simulation time. */
double IncrTime(void) {
sim_time += dT;
return sim_time;
}
/** Retrieves the current debug level setting. */
int GetDebugLevel(void) const {return debug_lvl;};
private:
static unsigned int FDMctr;
int Error;
unsigned int Frame;
unsigned int IdFDM;
unsigned short Terminate;
double dT;
double saved_dT;
double sim_time;
bool holding;
bool Constructing;
bool modelLoaded;
@ -482,6 +531,7 @@ private:
string SystemsPath;
string CFGVersion;
string Release;
string RootDir;
bool trim_status;
int ta_mode;
@ -489,7 +539,6 @@ private:
static FGPropertyManager *master;
FGGroundCallback* GroundCallback;
FGState* State;
FGAtmosphere* Atmosphere;
FGFCS* FCS;
FGPropulsion* Propulsion;
@ -521,6 +570,7 @@ private:
void ResetToInitialConditions(int mode);
bool Allocate(void);
bool DeAllocate(void);
void Initialize(FGInitialCondition *FGIC);
void Debug(int from);
};

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

@ -44,7 +44,7 @@ INCLUDES
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGJSBBase.cpp,v 1.29 2010/03/18 13:19:21 jberndt Exp $";
static const char *IdHdr = ID_JSBBASE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -77,8 +77,8 @@ CLASS IMPLEMENTATION
char FGJSBBase::fgdef[6] = {'\0' };
#endif
const double FGJSBBase::radtodeg = 57.29578;
const double FGJSBBase::degtorad = 1.745329E-2;
const double FGJSBBase::radtodeg = 57.295779513082320876798154814105;
const double FGJSBBase::degtorad = 0.017453292519943295769236907684886;
const double FGJSBBase::hptoftlbssec = 550.0;
const double FGJSBBase::psftoinhg = 0.014138;
const double FGJSBBase::psftopa = 47.88;

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

@ -63,7 +63,7 @@ namespace std
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_JSBBASE "$Id$"
#define ID_JSBBASE "$Id: FGJSBBase.h,v 1.30 2010/07/01 23:13:19 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -79,7 +79,7 @@ CLASS DOCUMENTATION
* This class provides universal constants, utility functions, messaging
* functions, and enumerated constants to JSBSim.
@author Jon S. Berndt
@version $Id$
@version $Id: FGJSBBase.h,v 1.30 2010/07/01 23:13:19 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -310,7 +310,7 @@ protected:
static std::queue <Message> Messages;
void Debug(int from) {};
void Debug(int) {};
static unsigned int messageId;

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

@ -45,7 +45,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGState.cpp,v 1.15 2009/10/24 22:59:30 jberndt Exp $";
static const char *IdHdr = ID_STATE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

@ -62,7 +62,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_STATE "$Id$"
#define ID_STATE "$Id: FGState.h,v 1.15 2009/10/02 10:30:07 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -78,7 +78,7 @@ CLASS DOCUMENTATION
<h3>Properties</h3>
@property sim-time-sec (read only) cumulative simulation in seconds.
@author Jon S. Berndt
@version $Revision$
@version $Revision: 1.15 $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

@ -5,20 +5,20 @@
// Copyright (C) 1999 Curtis L. Olson - curt@flightgear.org
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// modify it under the terms of the GNU Lesser 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.
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// You should have received a copy of the GNU Lesser General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
// $Id: JSBSim.cxx,v 1.62 2010/07/14 05:50:40 ehofman Exp $
#ifdef HAVE_CONFIG_H
@ -45,7 +45,6 @@
#include "JSBSim.hxx"
#include <FDM/JSBSim/FGFDMExec.h>
#include <FDM/JSBSim/FGJSBBase.h>
#include <FDM/JSBSim/FGState.h>
#include <FDM/JSBSim/initialization/FGInitialCondition.h>
#include <FDM/JSBSim/initialization/FGTrim.h>
#include <FDM/JSBSim/models/FGModel.h>
@ -58,6 +57,8 @@
#include <FDM/JSBSim/models/FGMassBalance.h>
#include <FDM/JSBSim/models/FGAerodynamics.h>
#include <FDM/JSBSim/models/FGLGear.h>
#include <FDM/JSBSim/models/FGGroundReactions.h>
#include <FDM/JSBSim/models/FGPropulsion.h>
#include <FDM/JSBSim/models/propulsion/FGEngine.h>
#include <FDM/JSBSim/models/propulsion/FGPiston.h>
#include <FDM/JSBSim/models/propulsion/FGTurbine.h>
@ -102,7 +103,7 @@ public:
double contact[3], normal[3], vel[3], agl = 0;
mInterface->get_agl_ft(t, loc_cart, SG_METER_TO_FEET*2, contact, normal,
vel, &agl);
n = FGColumnVector3( -normal[0], -normal[1], -normal[2] );
n = FGColumnVector3( normal[0], normal[1], normal[2] );
v = FGColumnVector3( vel[0], vel[1], vel[2] );
cont = FGColumnVector3( contact[0], contact[1], contact[2] );
return agl;
@ -147,7 +148,6 @@ FGJSBsim::FGJSBsim( double dt )
// Register ground callback.
fdmex->SetGroundCallback( new FGFSGroundCallback(this) );
State = fdmex->GetState();
Atmosphere = fdmex->GetAtmosphere();
FCS = fdmex->GetFCS();
MassBalance = fdmex->GetMassBalance();
@ -170,7 +170,13 @@ FGJSBsim::FGJSBsim( double dt )
SGPath systems_path( fgGetString("/sim/aircraft-dir") );
systems_path.append( "Systems" );
State->Setdt( dt );
// deprecate sim-time-sec for simulation/sim-time-sec
// remove alias with increased configuration file version number (2.1 or later)
SGPropertyNode * node = fgGetNode("/fdm/jsbsim/simulation/sim-time-sec");
fgGetNode("/fdm/jsbsim/sim-time-sec", true)->alias( node );
// end of sim-time-sec deprecation patch
fdmex->Setdt( dt );
result = fdmex->LoadModel( aircraft_path.str(),
engine_path.str(),
@ -257,6 +263,10 @@ FGJSBsim::FGJSBsim( double dt )
speedbrake_pos_pct->setDoubleValue(0);
spoilers_pos_pct->setDoubleValue(0);
ab_brake_engaged = fgGetNode("/autopilot/autobrake/engaged", true);
ab_brake_left_pct = fgGetNode("/autopilot/autobrake/brake-left-output", true);
ab_brake_right_pct = fgGetNode("/autopilot/autobrake/brake-right-output", true);
temperature = fgGetNode("/environment/temperature-degc",true);
pressure = fgGetNode("/environment/pressure-inhg",true);
density = fgGetNode("/environment/density-slugft3",true);
@ -296,8 +306,6 @@ FGJSBsim::~FGJSBsim(void)
void FGJSBsim::init()
{
double tmp;
SG_LOG( SG_FLIGHT, SG_INFO, "Starting and initializing JSBsim" );
// Explicitly call the superclass's
@ -328,6 +336,21 @@ void FGJSBsim::init()
<< ", " << fdmex->GetAtmosphere()->GetPressure()
<< ", " << fdmex->GetAtmosphere()->GetDensity() );
// deprecate egt_degf for egt-degf to have consistent naming
// TODO: raise log-level to ALERT in summer 2010,
// remove alias in fall 2010,
// remove this code in winter 2010
for (unsigned int i=0; i < Propulsion->GetNumEngines(); i++) {
SGPropertyNode * node = fgGetNode("engines/engine", i, true);
SGPropertyNode * egtn = node->getNode( "egt_degf" );
if( egtn != NULL ) {
SG_LOG(SG_FLIGHT,SG_WARN,
"Aircraft uses deprecated node egt_degf. Please upgrade to egt-degf");
node->getNode("egt-degf", true)->alias( egtn );
}
}
// end of egt_degf deprecation patch
if (fgGetBool("/sim/presets/running")) {
for (unsigned int i=0; i < Propulsion->GetNumEngines(); i++) {
SGPropertyNode * node = fgGetNode("engines/engine", i, true);
@ -437,7 +460,7 @@ void FGJSBsim::update( double dt )
cart = FGLocation(lon, lat, alt+slr);
}
double cart_pos[3] = { cart(1), cart(2), cart(3) };
double t0 = State->Getsim_time();
double t0 = fdmex->GetSimTime();
bool cache_ok = prepare_ground_cache_ft( t0, t0 + dt, cart_pos,
groundCacheRadius );
if (!cache_ok) {
@ -465,7 +488,7 @@ void FGJSBsim::update( double dt )
if ( needTrim ) {
if ( startup_trim->getBoolValue() ) {
double contact[3], d[3], agl;
get_agl_ft(State->Getsim_time(), cart_pos, SG_METER_TO_FEET*2, contact,
get_agl_ft(fdmex->GetSimTime(), cart_pos, SG_METER_TO_FEET*2, contact,
d, d, &agl);
double terrain_alt = sqrt(contact[0]*contact[0] + contact[1]*contact[1]
+ contact[2]*contact[2]) - fgic->GetSeaLevelRadiusFtIC();
@ -484,7 +507,7 @@ void FGJSBsim::update( double dt )
for ( i=0; i < multiloop; i++ ) {
fdmex->Run();
update_external_forces(State->Getsim_time() + i * State->Getdt());
update_external_forces(fdmex->GetSimTime() + i * fdmex->GetDeltaT());
}
FGJSBBase::Message* msg;
@ -543,6 +566,12 @@ bool FGJSBsim::copy_to_JSBsim()
double parking_brake = globals->get_controls()->get_brake_parking();
double left_brake = globals->get_controls()->get_brake_left();
double right_brake = globals->get_controls()->get_brake_right();
if (ab_brake_engaged->getBoolValue()) {
left_brake = ab_brake_left_pct->getDoubleValue();
right_brake = ab_brake_right_pct->getDoubleValue();
}
FCS->SetLBrake(FMAX(left_brake, parking_brake));
FCS->SetRBrake(FMAX(right_brake, parking_brake));
@ -773,7 +802,7 @@ bool FGJSBsim::copy_from_JSBsim()
FGTurbine* eng = (FGTurbine*)Propulsion->GetEngine(i);
node->setDoubleValue("n1", eng->GetN1());
node->setDoubleValue("n2", eng->GetN2());
node->setDoubleValue("egt_degf", 32 + eng->GetEGT()*9/5);
node->setDoubleValue("egt-degf", 32 + eng->GetEGT()*9/5);
node->setBoolValue("augmentation", eng->GetAugmentation());
node->setBoolValue("water-injection", eng->GetInjection());
node->setBoolValue("ignition", eng->GetIgnition());
@ -817,6 +846,8 @@ bool FGJSBsim::copy_from_JSBsim()
node->setDoubleValue("rpm", eng->getRPM());
} // end FGElectric code block
break;
case FGEngine::etUnknown:
break;
}
{ // FGEngine code block
@ -888,7 +919,7 @@ bool FGJSBsim::copy_from_JSBsim()
// force a sim crashed if crashed (altitude AGL < 0)
if (get_Altitude_AGL() < -100.0) {
State->SuspendIntegration();
fdmex->SuspendIntegration();
crashed = true;
}
@ -1138,9 +1169,6 @@ void FGJSBsim::do_trim(void)
} else {
trimmed->setBoolValue(true);
}
// if (FGJSBBase::debug_lvl > 0)
// State->ReportState();
delete fgtrim;
pitch_trim->setDoubleValue( FCS->GetPitchTrimCmd() );
@ -1257,7 +1285,6 @@ void FGJSBsim::update_external_forces(double t_off)
FGColumnVector3 hook_tip_body = hook_root_body;
hook_tip_body(1) -= hook_length * cos_fi;
hook_tip_body(3) += hook_length * sin_fi;
bool hook_tip_valid = true;
double contact[3];
double ground_normal[3];

12
src/FDM/JSBSim/JSBSim.hxx
View file

@ -8,16 +8,16 @@
------ Copyright (C) 1999 - 2000 Curtis L. Olson (curt@flightgear.org) ------
This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License as
modify it under the terms of the GNU Lesser 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.
Lesser General Public License for more details.
You should have received a copy of the GNU General Public License
You should have received a copy of the GNU Lesser General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
@ -86,7 +86,7 @@ CLASS DOCUMENTATION
documentation for main for direction on running JSBSim apart from FlightGear.
@author Curtis L. Olson (original)
@author Tony Peden (Maintained and refined)
@version $Id$
@version $Id: JSBSim.hxx,v 1.13 2010/07/07 20:46:36 andgi Exp $
@see main in file JSBSim.cpp (use main() wrapper for standalone usage)
*/
@ -253,6 +253,10 @@ private:
SGPropertyNode_ptr speedbrake_pos_pct;
SGPropertyNode_ptr spoilers_pos_pct;
SGPropertyNode_ptr ab_brake_engaged;
SGPropertyNode_ptr ab_brake_left_pct;
SGPropertyNode_ptr ab_brake_right_pct;
SGPropertyNode_ptr gear_pos_pct;
SGPropertyNode_ptr wing_fold_pos_pct;
SGPropertyNode_ptr tailhook_pos_pct;

293
src/FDM/JSBSim/initialization/FGInitialCondition.cpp
View file

@ -56,12 +56,13 @@ INCLUDES
#include <iostream>
#include <fstream>
#include <cstdlib>
#include "input_output/string_utilities.h"
using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGInitialCondition.cpp,v 1.40 2010/06/02 04:05:13 jberndt Exp $";
static const char *IdHdr = ID_INITIALCONDITION;
//******************************************************************************
@ -704,23 +705,22 @@ double FGInitialCondition::calcVcas(double Mach) {
double psl=fdmex->GetAtmosphere()->GetPressureSL();
double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
double pt,A,B,D,vcas;
if(Mach < 0) Mach=0;
if(Mach < 1) //calculate total pressure assuming isentropic flow
if (Mach < 0) Mach=0;
if (Mach < 1) //calculate total pressure assuming isentropic flow
pt=p*pow((1 + 0.2*Mach*Mach),3.5);
else {
// shock in front of pitot tube, we'll assume its normal and use
// the Rayleigh Pitot Tube Formula, i.e. the ratio of total
// pressure behind the shock to the static pressure in front
//the normal shock assumption should not be a bad one -- most supersonic
//aircraft place the pitot probe out front so that it is the forward
//most point on the aircraft. The real shock would, of course, take
//on something like the shape of a rounded-off cone but, here again,
//the assumption should be good since the opening of the pitot probe
//is very small and, therefore, the effects of the shock curvature
//should be small as well. AFAIK, this approach is fairly well accepted
//within the aerospace community
// pressure behind the shock to the static pressure in front of
// the normal shock assumption should not be a bad one -- most supersonic
// aircraft place the pitot probe out front so that it is the forward
// most point on the aircraft. The real shock would, of course, take
// on something like the shape of a rounded-off cone but, here again,
// the assumption should be good since the opening of the pitot probe
// is very small and, therefore, the effects of the shock curvature
// should be small as well. AFAIK, this approach is fairly well accepted
// within the aerospace community
B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);
@ -842,8 +842,6 @@ double FGInitialCondition::GetWindDirDegIC(void) const {
bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
{
int n;
string sep = "/";
if( useStoredPath ) {
init_file_name = fdmex->GetFullAircraftPath() + sep + rstfile + ".xml";
@ -864,6 +862,26 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
exit(-1);
}
double version = document->GetAttributeValueAsNumber("version");
if (version == HUGE_VAL) {
return Load_v1(); // Default to the old version
} else if (version >= 3.0) {
cerr << "Only initialization file formats 1 and 2 are currently supported" << endl;
exit (-1);
} else if (version >= 2.0) {
return Load_v2();
} else if (version >= 1.0) {
return Load_v1();
}
}
//******************************************************************************
bool FGInitialCondition::Load_v1(void)
{
int n;
if (document->FindElement("latitude"))
SetLatitudeDegIC(document->FindElementValueAsNumberConvertTo("latitude", "DEG"));
if (document->FindElement("longitude"))
@ -941,6 +959,249 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
//******************************************************************************
bool FGInitialCondition::Load_v2(void)
{
int n;
FGColumnVector3 vLoc, vOrient;
bool result = true;
FGInertial* Inertial = fdmex->GetInertial();
FGPropagate* Propagate = fdmex->GetPropagate();
if (document->FindElement("earth_position_angle")) {
double epa = document->FindElementValueAsNumberConvertTo("earth_position_angle", "RAD");
Inertial->SetEarthPositionAngle(epa);
}
// Initialize vehicle position
//
// Allowable frames:
// - ECI (Earth Centered Inertial)
// - ECEF (Earth Centered, Earth Fixed)
Element* position = document->FindElement("position");
if (position) {
vLoc = position->FindElementTripletConvertTo("FT");
string frame = position->GetAttributeValue("frame");
frame = to_lower(frame);
if (frame == "eci") {
// Need to transform vLoc to ECEF for storage and use in FGLocation.
vLoc = Propagate->GetTi2ec()*vLoc;
} else if (frame == "ecef") {
// Move vLoc query until after lat/lon/alt query to eliminate spurious warning msgs.
if (vLoc.Magnitude() == 0.0) {
Propagate->SetLatitudeDeg(position->FindElementValueAsNumberConvertTo("latitude", "DEG"));
Propagate->SetLongitudeDeg(position->FindElementValueAsNumberConvertTo("longitude", "DEG"));
if (position->FindElement("radius")) {
radius_to_vehicle = position->FindElementValueAsNumberConvertTo("radius", "FT");
SetAltitudeASLFtIC(radius_to_vehicle - sea_level_radius);
} else if (position->FindElement("altitudeAGL")) {
SetAltitudeAGLFtIC(position->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
} else if (position->FindElement("altitudeMSL")) {
SetAltitudeASLFtIC(position->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
} else {
cerr << endl << " No altitude or radius initial condition is given." << endl;
result = false;
}
}
} else {
cerr << endl << " Neither ECI nor ECEF frame is specified for initial position." << endl;
result = false;
}
} else {
cerr << endl << " Initial position not specified in this initialization file." << endl;
result = false;
}
// End of position initialization
// Initialize vehicle orientation
// Allowable frames
// - ECI (Earth Centered Inertial)
// - ECEF (Earth Centered, Earth Fixed)
// - Local
//
// Need to convert the provided orientation to an ECI orientation, using
// the given orientation and knowledge of the Earth position angle.
// This could be done using matrices (where in the subscript "b/a",
// it is meant "b with respect to a", and where b=body frame,
// i=inertial frame, and e=ecef frame) as:
//
// C_b/i = C_b/e * C_e/i
//
// Using quaternions (note reverse ordering compared to matrix representation):
//
// Q_b/i = Q_e/i * Q_b/e
//
// Use the specific matrices as needed. The above example of course is for the whole
// body to inertial orientation.
// The new orientation angles can be extracted from the matrix or the quaternion.
// ToDo: Do we need to deal with normalization of the quaternions here?
Element* orientation_el = document->FindElement("orientation");
if (orientation_el) {
string frame = orientation_el->GetAttributeValue("frame");
frame = to_lower(frame);
vOrient = orientation_el->FindElementTripletConvertTo("RAD");
FGQuaternion QuatI2Body;
if (frame == "eci") {
QuatI2Body = FGQuaternion(vOrient);
} else if (frame == "ecef") {
// In this case we are given the Euler angles representing the orientation of
// the body with respect to the ECEF system, represented by the C_b/e Matrix.
// We want the body orientation with respect to the inertial system:
//
// C_b/i = C_b/e * C_e/i
//
// Using quaternions (note reverse ordering compared to matrix representation):
//
// Q_b/i = Q_e/i * Q_b/e
FGQuaternion QuatEC2Body(vOrient); // Store relationship of Body frame wrt ECEF frame, Q_b/e
FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix
QuatI2Body = QuatI2EC * QuatEC2Body; // Q_b/i = Q_e/i * Q_b/e
} else if (frame == "local") {
// In this case, we are supplying the Euler angles for the vehicle with
// respect to the local (NED frame), also called the navigation frame.
// This is the most common way of initializing the orientation of
// aircraft. The matrix representation is:
//
// C_b/i = C_b/n * C_n/e * C_e/i
//
// Or, using quaternions (note reverse ordering compared to matrix representation):
//
// Q_b/i = Q_e/i * Q_n/e * Q_b/n
FGQuaternion QuatLocal2Body = FGQuaternion(vOrient); // Store relationship of Body frame wrt local (NED) frame, Q_b/n
FGQuaternion QuatEC2Local = Propagate->GetTec2l(); // Get Q_n/e from matrix
FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix
QuatI2Body = QuatI2EC * QuatEC2Local * QuatLocal2Body; // Q_b/i = Q_e/i * Q_n/e * Q_b/n
} else {
cerr << endl << fgred << " Orientation frame type: \"" << frame
<< "\" is not supported!" << reset << endl << endl;
result = false;
}
Propagate->SetInertialOrientation(QuatI2Body);
}
// Initialize vehicle velocity
// Allowable frames
// - ECI (Earth Centered Inertial)
// - ECEF (Earth Centered, Earth Fixed)
// - Local
// - Body
// The vehicle will be defaulted to (0,0,0) in the Body frame if nothing is provided.
Element* velocity_el = document->FindElement("velocity");
FGColumnVector3 vInertialVelocity;
FGColumnVector3 vInitVelocity = FGColumnVector3(0.0, 0.0, 0.0);
if (velocity_el) {
string frame = velocity_el->GetAttributeValue("frame");
frame = to_lower(frame);
FGColumnVector3 vInitVelocity = velocity_el->FindElementTripletConvertTo("FT/SEC");
FGColumnVector3 omega_cross_r = Inertial->omega() * Propagate->GetInertialPosition();
if (frame == "eci") {
vInertialVelocity = vInitVelocity;
} else if (frame == "ecef") {
FGMatrix33 mTec2i = Propagate->GetTec2i(); // Get C_i/e
vInertialVelocity = mTec2i * vInitVelocity + omega_cross_r;
} else if (frame == "local") {
FGMatrix33 mTl2i = Propagate->GetTl2i();
vInertialVelocity = mTl2i * vInitVelocity + omega_cross_r;
} else if (frame == "body") {
FGMatrix33 mTb2i = Propagate->GetTb2i();
vInertialVelocity = mTb2i * vInitVelocity + omega_cross_r;
} else {
cerr << endl << fgred << " Velocity frame type: \"" << frame
<< "\" is not supported!" << reset << endl << endl;
result = false;
}
} else {
FGMatrix33 mTb2i = Propagate->GetTb2i();
vInertialVelocity = mTb2i * vInitVelocity + (Inertial->omega() * Propagate->GetInertialPosition());
}
Propagate->SetInertialVelocity(vInertialVelocity);
// Allowable frames
// - ECI (Earth Centered Inertial)
// - ECEF (Earth Centered, Earth Fixed)
// - Body
FGColumnVector3 vInertialRate;
Element* attrate_el = document->FindElement("attitude_rate");
if (attrate_el) {
string frame = attrate_el->GetAttributeValue("frame");
frame = to_lower(frame);
FGColumnVector3 vAttRate = attrate_el->FindElementTripletConvertTo("RAD/SEC");
if (frame == "eci") {
vInertialRate = vAttRate;
} else if (frame == "ecef") {
// vInertialRate = vAttRate + Inertial->omega();
} else if (frame == "body") {
//Todo: determine local frame rate
FGMatrix33 mTb2l = Propagate->GetTb2l();
// vInertialRate = mTb2l*vAttRate + Inertial->omega();
} else if (!frame.empty()) { // misspelling of frame
cerr << endl << fgred << " Attitude rate frame type: \"" << frame
<< "\" is not supported!" << reset << endl << endl;
result = false;
} else if (frame.empty()) {
}
} else { // Body frame attitude rate assumed 0 relative to local.
/*
//Todo: determine local frame rate
FGMatrix33 mTi2l = Propagate->GetTi2l();
vVel = mTi2l * vInertialVelocity;
// Compute the local frame ECEF velocity
vVel = Tb2l * VState.vUVW;
FGColumnVector3 vOmegaLocal = FGColumnVector3(
radInv*vVel(eEast),
-radInv*vVel(eNorth),
-radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
*/
}
// Check to see if any engines are specified to be initialized in a running state
FGPropulsion* propulsion = fdmex->GetPropulsion();
Element* running_elements = document->FindElement("running");
while (running_elements) {
n = int(running_elements->GetDataAsNumber());
propulsion->InitRunning(n);
running_elements = document->FindNextElement("running");
}
// fdmex->RunIC();
return result;
}
//******************************************************************************
void FGInitialCondition::bind(void){
PropertyManager->Tie("ic/vc-kts", this,
&FGInitialCondition::GetVcalibratedKtsIC,

7
src/FDM/JSBSim/initialization/FGInitialCondition.h
View file

@ -56,7 +56,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_INITIALCONDITION "$Id$"
#define ID_INITIALCONDITION "$Id: FGInitialCondition.h,v 1.20 2010/02/15 03:22:57 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -202,7 +202,7 @@ CLASS DOCUMENTATION
@property ic/r-rad_sec (read/write) Yaw rate initial condition in radians/second
@author Tony Peden
@version "$Id$"
@version "$Id: FGInitialCondition.h,v 1.20 2010/02/15 03:22:57 jberndt Exp $"
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -647,6 +647,9 @@ private:
FGFDMExec *fdmex;
FGPropertyManager *PropertyManager;
bool Load_v1(void);
bool Load_v2(void);
bool Constructing;
bool getAlpha(void);
bool getTheta(void);

16
src/FDM/JSBSim/initialization/FGTrim.cpp
View file

@ -51,6 +51,8 @@ INCLUDES
#include "models/FGGroundReactions.h"
#include "models/FGInertial.h"
#include "models/FGAerodynamics.h"
#include "models/FGPropulsion.h"
#include "models/propulsion/FGEngine.h"
#include "math/FGColumnVector3.h"
#if _MSC_VER
@ -61,7 +63,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGTrim.cpp,v 1.13 2010/04/23 17:23:40 dpculp Exp $";
static const char *IdHdr = ID_TRIM;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -239,6 +241,8 @@ bool FGTrim::DoTrim(void) {
fdmex->DisableOutput();
setEngineTrimMode(true);
fgic->SetPRadpsIC(0.0);
fgic->SetQRadpsIC(0.0);
fgic->SetRRadpsIC(0.0);
@ -354,6 +358,7 @@ bool FGTrim::DoTrim(void) {
for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
}
setEngineTrimMode(false);
fdmex->EnableOutput();
return !trim_failed;
}
@ -620,6 +625,15 @@ void FGTrim::setDebug(void) {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGTrim::setEngineTrimMode(bool mode) {
FGPropulsion* prop = fdmex->GetPropulsion();
for (unsigned int i=0; i<prop->GetNumEngines(); i++) {
prop->GetEngine(i)->SetTrimMode(mode);
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGTrim::SetMode(TrimMode tt) {
ClearStates();
mode=tt;

6
src/FDM/JSBSim/initialization/FGTrim.h
View file

@ -60,7 +60,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_TRIM "$Id$"
#define ID_TRIM "$Id: FGTrim.h,v 1.7 2010/04/23 17:23:40 dpculp Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -120,7 +120,7 @@ CLASS DOCUMENTATION
@endcode
@author Tony Peden
@version "$Id$"
@version "$Id: FGTrim.h,v 1.7 2010/04/23 17:23:40 dpculp Exp $"
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -176,7 +176,7 @@ private:
void setupTurn(void);
void updateRates(void);
void setEngineTrimMode(bool mode);
void setDebug(void);
public:

10
src/FDM/JSBSim/initialization/FGTrimAxis.cpp
View file

@ -55,7 +55,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGTrimAxis.cpp,v 1.10 2010/07/08 11:36:28 jberndt Exp $";
static const char *IdHdr = ID_TRIMAXIS;
/*****************************************************************************/
@ -434,6 +434,10 @@ void FGTrimAxis::setThrottlesPct(void) {
/*****************************************************************************/
void FGTrimAxis::AxisReport(void) {
// Save original cout format characteristics
std::ios_base::fmtflags originalFormat = cout.flags();
std::streamsize originalPrecision = cout.precision();
std::streamsize originalWidth = cout.width();
cout << " " << setw(20) << GetControlName() << ": ";
cout << setw(6) << setprecision(2) << GetControl()*control_convert << ' ';
cout << setw(5) << GetStateName() << ": ";
@ -444,6 +448,10 @@ void FGTrimAxis::AxisReport(void) {
cout << " Passed" << endl;
else
cout << " Failed" << endl;
// Restore original cout format characteristics
cout.flags(originalFormat);
cout.precision(originalPrecision);
cout.width(originalWidth);
}
/*****************************************************************************/

2
src/FDM/JSBSim/initialization/FGTrimAxis.h
View file

@ -48,7 +48,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_TRIMAXIS "$Id$"
#define ID_TRIMAXIS "$Id: FGTrimAxis.h,v 1.4 2006/10/01 22:47:47 jberndt Exp $"
#define DEFAULT_TOLERANCE 0.001

9
src/FDM/JSBSim/input_output/FGGroundCallback.cpp
View file

@ -71,10 +71,11 @@ double FGGroundCallback::GetAGLevel(double t, const FGLocation& loc,
FGColumnVector3& vel) const
{
vel = FGColumnVector3(0.0, 0.0, 0.0);
normal = (-1/FGColumnVector3(loc).Magnitude())*FGColumnVector3(loc);
double radius = loc.GetRadius();
double agl = GetAltitude(loc);
contact = ((radius-agl)/radius)*FGColumnVector3(loc);
normal = FGColumnVector3(loc).Normalize();
double loc_radius = loc.GetRadius(); // Get the radius of the given location
// (e.g. the CG)
double agl = loc_radius - mReferenceRadius;
contact = (mReferenceRadius/loc_radius)*FGColumnVector3(loc);
return agl;
}

4
src/FDM/JSBSim/input_output/FGGroundCallback.h
View file

@ -45,7 +45,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_GROUNDCALLBACK "$Id$"
#define ID_GROUNDCALLBACK "$Id: FGGroundCallback.h,v 1.8 2009/10/02 10:30:09 jberndt Exp $"
namespace JSBSim {
@ -59,7 +59,7 @@ CLASS DOCUMENTATION
ball formed earth.
@author Mathias Froehlich
@version $Id$
@version $Id: FGGroundCallback.h,v 1.8 2009/10/02 10:30:09 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

13
src/FDM/JSBSim/input_output/FGPropertyManager.cpp
View file

@ -152,6 +152,19 @@ string FGPropertyManager::GetFullyQualifiedName(void) {
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
string FGPropertyManager::GetRelativeName( const string &path )
{
string temp_string = GetFullyQualifiedName();
size_t len = path.length();
if ( (len > 0) && (temp_string.substr(0,len) == path) ) {
temp_string = temp_string.erase(0,len);
}
return temp_string;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

11
src/FDM/JSBSim/input_output/FGPropertyManager.h
View file

@ -53,7 +53,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_PROPERTYMANAGER "$Id$"
#define ID_PROPERTYMANAGER "$Id: FGPropertyManager.h,v 1.17 2010/07/08 11:36:28 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -129,6 +129,15 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase
*/
std::string GetFullyQualifiedName(void);
/**
* Get the qualified name of a node relative to given base path,
* otherwise the fully qualified name.
* This function is very slow, so is probably useful for debugging only.
*
* @param path The path to strip off, if found.
*/
std::string GetRelativeName( const std::string &path = "/fdm/jsbsim/" );
/**
* Get a bool value for a property.
*

62
src/FDM/JSBSim/input_output/FGScript.cpp
View file

@ -42,17 +42,19 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGScript.h"
#include "input_output/FGXMLElement.h"
#include "input_output/FGXMLParse.h"
#include "initialization/FGTrim.h"
#include <iostream>
#include <cstdlib>
#include <iomanip>
using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGScript.cpp,v 1.41 2010/07/08 11:36:28 jberndt Exp $";
static const char *IdHdr = ID_FGSCRIPT;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -67,8 +69,8 @@ CLASS IMPLEMENTATION
FGScript::FGScript(FGFDMExec* fgex) : FDMExec(fgex)
{
State = FDMExec->GetState();
PropertyManager=FDMExec->GetPropertyManager();
Debug(0);
}
@ -89,7 +91,7 @@ FGScript::~FGScript()
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGScript::LoadScript( string script )
bool FGScript::LoadScript(string script, double deltaT)
{
string aircraft="", initialize="", comparison = "", prop_name="";
string notifyPropertyName="";
@ -135,10 +137,18 @@ bool FGScript::LoadScript( string script )
// Set sim timing
StartTime = run_element->GetAttributeValueAsNumber("start");
State->Setsim_time(StartTime);
FDMExec->Setsim_time(StartTime);
EndTime = run_element->GetAttributeValueAsNumber("end");
dt = run_element->GetAttributeValueAsNumber("dt");
State->Setdt(dt);
if (deltaT == 0.0)
dt = run_element->GetAttributeValueAsNumber("dt");
else {
dt = deltaT;
cout << endl << "Overriding simulation step size from the command line. New step size is: "
<< deltaT << " seconds (" << 1/deltaT << " Hz)" << endl << endl;
}
FDMExec->Setdt(dt);
// read aircraft and initialization files
@ -175,7 +185,7 @@ bool FGScript::LoadScript( string script )
if (output_file.empty()) {
cerr << "No logging directives file was specified." << endl;
} else {
FDMExec->SetOutputDirectives(output_file);
if (!FDMExec->SetOutputDirectives(output_file)) return false;
}
}
@ -226,7 +236,12 @@ bool FGScript::LoadScript( string script )
// Process the conditions
condition_element = event_element->FindElement("condition");
if (condition_element != 0) {
newCondition = new FGCondition(condition_element, PropertyManager);
try {
newCondition = new FGCondition(condition_element, PropertyManager);
} catch(string str) {
cout << endl << fgred << str << reset << endl << endl;
return false;
}
newEvent->Condition = newCondition;
} else {
cerr << "No condition specified in script event " << newEvent->Name << endl;
@ -320,7 +335,7 @@ bool FGScript::RunScript(void)
unsigned i, j;
unsigned event_ctr = 0;
double currentTime = State->Getsim_time();
double currentTime = FDMExec->GetSimTime();
double newSetValue = 0;
if (currentTime > EndTime) return false; //Script done!
@ -409,7 +424,7 @@ bool FGScript::RunScript(void)
cout << endl << " Event " << event_ctr << " (" << Events[ev_ctr].Name << ")"
<< " executed at time: " << currentTime << endl;
for (j=0; j<Events[ev_ctr].NotifyProperties.size();j++) {
cout << " " << Events[ev_ctr].NotifyProperties[j]->GetName()
cout << " " << Events[ev_ctr].NotifyProperties[j]->GetRelativeName()
<< " = " << Events[ev_ctr].NotifyProperties[j]->getDoubleValue() << endl;
}
cout << endl;
@ -453,7 +468,8 @@ void FGScript::Debug(int from)
cout << endl;
cout << "Script: \"" << ScriptName << "\"" << endl;
cout << " begins at " << StartTime << " seconds and runs to " << EndTime
<< " seconds with dt = " << State->Getdt() << endl;
<< " seconds with dt = " << setprecision(6) << FDMExec->GetDeltaT() << " (" <<
ceil(1.0/FDMExec->GetDeltaT()) << " Hz)" << endl;
cout << endl;
for (unsigned int i=0; i<local_properties.size(); i++) {
@ -476,7 +492,10 @@ void FGScript::Debug(int from)
Events[i].Condition->PrintCondition();
cout << endl << " Actions taken:" << endl << " {";
cout << endl << " Actions taken";
if (Events[i].Delay > 0.0)
cout << " (after a delay of " << Events[i].Delay << " secs)";
cout << ":" << endl << " {";
for (unsigned j=0; j<Events[i].SetValue.size(); j++) {
if (Events[i].SetValue[j] == 0.0 && Events[i].Functions[j] != 0L) {
if (Events[i].SetParam[j] == 0) {
@ -486,7 +505,7 @@ void FGScript::Debug(int from)
<< reset << endl;
exit(-1);
}
cout << endl << " set " << Events[i].SetParam[j]->GetName()
cout << endl << " set " << Events[i].SetParam[j]->GetRelativeName("/fdm/jsbsim/")
<< " to function value";
} else {
if (Events[i].SetParam[j] == 0) {
@ -496,7 +515,7 @@ void FGScript::Debug(int from)
<< reset << endl;
exit(-1);
}
cout << endl << " set " << Events[i].SetParam[j]->GetName()
cout << endl << " set " << Events[i].SetParam[j]->GetRelativeName("/fdm/jsbsim/")
<< " to " << Events[i].SetValue[j];
}
@ -529,8 +548,21 @@ void FGScript::Debug(int from)
if (Events[i].Action[j] == FG_RAMP || Events[i].Action[j] == FG_EXP)
cout << " with time constant " << Events[i].TC[j] << ")";
}
cout << endl << " }" << endl << endl;
cout << endl << " }" << endl;
// Print notifications
if (Events[i].Notify) {
if (Events[i].NotifyProperties.size() > 0) {
cout << " Notifications" << ":" << endl << " {" << endl;
for (unsigned j=0; j<Events[i].NotifyProperties.size();j++) {
cout << " "
<< Events[i].NotifyProperties[j]->GetRelativeName("/fdm/jsbsim/")
<< endl;
}
cout << " }" << endl;
}
}
cout << endl;
}
}
}

13
src/FDM/JSBSim/input_output/FGScript.h
View file

@ -38,7 +38,6 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGJSBBase.h"
#include "FGState.h"
#include "FGFDMExec.h"
#include "math/FGFunction.h"
#include "math/FGCondition.h"
@ -49,7 +48,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_FGSCRIPT "$Id$"
#define ID_FGSCRIPT "$Id: FGScript.h,v 1.18 2010/04/11 13:44:42 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -158,7 +157,7 @@ CLASS DOCUMENTATION
comes the &quot;run&quot; section, where the conditions are
described in &quot;event&quot; clauses.</p>
@author Jon S. Berndt
@version "$Id$"
@version "$Id: FGScript.h,v 1.18 2010/04/11 13:44:42 jberndt Exp $"
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -175,10 +174,13 @@ public:
~FGScript();
/** Loads a script to drive JSBSim (usually in standalone mode).
The language is the Script Directives for JSBSim.
The language is the Script Directives for JSBSim. If a simulation step size
has been supplied on the command line, it will be override the script-
specified simulation step size.
@param script the filename (including path name, if any) for the script.
@param deltaT a simulation step size from the command line
@return true if successful */
bool LoadScript( string script );
bool LoadScript(string script, double deltaT);
/** This function is called each pass through the executive Run() method IF
scripting is enabled.
@ -259,7 +261,6 @@ private:
vector <LocalProps*> local_properties;
FGFDMExec* FDMExec;
FGState* State;
FGPropertyManager* PropertyManager;
void Debug(int from);
};

6
src/FDM/JSBSim/input_output/FGXMLElement.cpp
View file

@ -42,7 +42,7 @@ FORWARD DECLARATIONS
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGXMLElement.cpp,v 1.29 2010/03/18 13:18:31 jberndt Exp $";
static const char *IdHdr = ID_XMLELEMENT;
bool Element::converterIsInitialized = false;
@ -114,7 +114,10 @@ Element::Element(const string& nm)
convert["KTS"]["FT/SEC"] = 1.68781;
convert["FT/SEC"]["KTS"] = 1.0/convert["KTS"]["FT/SEC"];
convert["M/S"]["FT/S"] = 3.2808399;
convert["M/SEC"]["FT/SEC"] = 3.2808399;
convert["FT/S"]["M/S"] = 1.0/convert["M/S"]["FT/S"];
convert["M/SEC"]["FT/SEC"] = 3.2808399;
convert["FT/SEC"]["M/SEC"] = 1.0/convert["M/SEC"]["FT/SEC"];
// Torque
convert["FT*LBS"]["N*M"] = 1.35581795;
convert["N*M"]["FT*LBS"] = 1/convert["FT*LBS"]["N*M"];
@ -185,6 +188,7 @@ Element::Element(const string& nm)
convert["FT/SEC"]["FT/SEC"] = 1.00;
convert["KTS"]["KTS"] = 1.00;
convert["M/S"]["M/S"] = 1.0;
convert["M/SEC"]["M/SEC"] = 1.0;
// Torque
convert["FT*LBS"]["FT*LBS"] = 1.00;
convert["N*M"]["N*M"] = 1.00;

4
src/FDM/JSBSim/input_output/FGXMLElement.h
View file

@ -44,7 +44,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_XMLELEMENT "$Id$"
#define ID_XMLELEMENT "$Id: FGXMLElement.h,v 1.16 2009/10/24 22:59:30 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -136,7 +136,7 @@ CLASS DOCUMENTATION
- GAL = gallon (U.S. liquid)
@author Jon S. Berndt
@version $Id$
@version $Id: FGXMLElement.h,v 1.16 2009/10/24 22:59:30 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/input_output/FGXMLFileRead.h
View file

@ -43,7 +43,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_XMLFILEREAD "$Id$"
#define ID_XMLFILEREAD "$Id: FGXMLFileRead.h,v 1.5 2009/11/28 20:12:47 andgi Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

2
src/FDM/JSBSim/input_output/FGXMLParse.cpp
View file

@ -40,7 +40,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGXMLParse.cpp,v 1.10 2009/10/24 22:59:30 jberndt Exp $";
static const char *IdHdr = ID_XMLPARSE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

4
src/FDM/JSBSim/input_output/FGXMLParse.h
View file

@ -40,7 +40,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_XMLPARSE "$Id$"
#define ID_XMLPARSE "$Id: FGXMLParse.h,v 1.7 2009/10/24 22:59:30 jberndt Exp $"
#define VALID_CHARS """`!@#$%^&*()_+`1234567890-={}[];':,.<>/?abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -57,7 +57,7 @@ CLASS DOCUMENTATION
/** Encapsulates an XML parser based on the EasyXML parser from the SimGear library.
@author Jon S. Berndt
@version $Id$
@version $Id: FGXMLParse.h,v 1.7 2009/10/24 22:59:30 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

6
src/FDM/JSBSim/input_output/FGfdmSocket.cpp
View file

@ -52,7 +52,7 @@ using std::string;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGfdmSocket.cpp,v 1.27 2010/05/13 03:07:59 jberndt Exp $";
static const char *IdHdr = ID_FDMSOCKET;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -77,7 +77,9 @@ FGfdmSocket::FGfdmSocket(const string& address, int port, int protocol)
cout << "Could not get host net address by name..." << endl;
}
} else {
if ((host = gethostbyaddr(address.c_str(), address.size(), PF_INET)) == NULL) {
unsigned int ip;
ip = inet_addr(address.c_str());
if ((host = gethostbyaddr((char*)&ip, address.size(), PF_INET)) == NULL) {
cout << "Could not get host net address by number..." << endl;
}
}

3
src/FDM/JSBSim/input_output/FGfdmSocket.h
View file

@ -51,6 +51,7 @@ INCLUDES
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <errno.h>
#include <sys/ioctl.h>
@ -64,7 +65,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_FDMSOCKET "$Id$"
#define ID_FDMSOCKET "$Id: FGfdmSocket.h,v 1.19 2010/05/13 03:07:59 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

20
src/FDM/JSBSim/input_output/string_utilities.h
View file

@ -45,7 +45,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_STRINGUTILS "$Id$"
#define ID_STRINGUTILS "$Id: string_utilities.h,v 1.13 2010/07/07 11:59:48 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -64,18 +64,19 @@ CLASS DECLARATION
extern std::string& trim_left(std::string& str);
extern std::string& trim_right(std::string& str);
extern std::string& trim(std::string& str);
extern std::string& trim_all_space(std::string& str);
extern std::string& to_upper(std::string& str);
extern std::string& to_lower(std::string& str);
extern bool is_number(const std::string& str);
std::vector <std::string> split(std::string str, char d);
#else
#include <ctype.h>
#include <cctype>
using namespace std;
string& trim_left(string& str)
{
while (str.size() && !isgraph(str[0])) {
while (str.size() && isspace((unsigned char)str[0])) {
str = str.erase(0,1);
}
return str;
@ -83,7 +84,7 @@ CLASS DECLARATION
string& trim_right(string& str)
{
while (str.size() && !isgraph(str[str.size()-1])) {
while (str.size() && isspace((unsigned char)str[str.size()-1])) {
str = str.erase(str.size()-1,1);
}
return str;
@ -96,6 +97,17 @@ CLASS DECLARATION
return str = trim_left(temp_str);
}
string& trim_all_space(string& str)
{
for (size_t i=0; i<str.size(); i++) {
if (isspace((unsigned char)str[i])) {
str = str.erase(i,1);
--i;
}
}
return str;
}
string& to_upper(string& str)
{
for (size_t i=0; i<str.size(); i++) str[i] = toupper(str[i]);

12
src/FDM/JSBSim/math/FGColumnVector3.cpp
View file

@ -47,7 +47,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGColumnVector3.cpp,v 1.12 2010/06/30 03:13:40 jberndt Exp $";
static const char *IdHdr = ID_COLUMNVECTOR3;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -65,9 +65,9 @@ FGColumnVector3::FGColumnVector3(void)
string FGColumnVector3::Dump(const string& delimiter) const
{
ostringstream buffer;
buffer << std::setw(18) << std::setprecision(16) << Entry(1) << delimiter;
buffer << std::setw(18) << std::setprecision(16) << Entry(2) << delimiter;
buffer << std::setw(18) << std::setprecision(16) << Entry(3);
buffer << std::setw(18) << std::setprecision(16) << data[0] << delimiter;
buffer << std::setw(18) << std::setprecision(16) << data[1] << delimiter;
buffer << std::setw(18) << std::setprecision(16) << data[2];
return buffer.str();
}
@ -110,10 +110,10 @@ FGColumnVector3& FGColumnVector3::operator/=(const double scalar)
double FGColumnVector3::Magnitude(void) const
{
if (Entry(1) == 0.0 && Entry(2) == 0.0 && Entry(3) == 0.0)
if (data[0] == 0.0 && data[1] == 0.0 && data[2] == 0.0)
return 0.0;
else
return sqrt( Entry(1)*Entry(1) + Entry(2)*Entry(2) + Entry(3)*Entry(3) );
return sqrt( data[0]*data[0] + data[1]*data[1] + data[2]*data[2] );
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

61
src/FDM/JSBSim/math/FGColumnVector3.h
View file

@ -47,7 +47,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_COLUMNVECTOR3 "$Id$"
#define ID_COLUMNVECTOR3 "$Id: FGColumnVector3.h,v 1.12 2010/06/30 03:13:40 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -61,7 +61,7 @@ CLASS DOCUMENTATION
/** This class implements a 3 element column vector.
@author Jon S. Berndt, Tony Peden, et. al.
@version $Id$
@version $Id: FGColumnVector3.h,v 1.12 2010/06/30 03:13:40 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -84,7 +84,7 @@ public:
data[0] = X;
data[1] = Y;
data[2] = Z;
Debug(0);
// Debug(0);
}
/** Copy constructor.
@ -94,25 +94,25 @@ public:
data[0] = v.data[0];
data[1] = v.data[1];
data[2] = v.data[2];
Debug(0);
// Debug(0);
}
/// Destructor.
~FGColumnVector3(void) { Debug(1); }
~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); }
double operator()(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.
Note that the index given in the argument is unchecked. */
double& operator()(unsigned int idx) { return Entry(idx); }
double& operator()(unsigned int idx) { return data[idx-1]; }
/** Read access the entries of the vector.
@param idx the component index.
@ -166,7 +166,7 @@ public:
@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));
return FGColumnVector3(scalar*data[0], scalar*data[1], scalar*data[2]);
}
/** Multiply by 1/scalar.
@ -181,42 +181,42 @@ public:
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) );
return FGColumnVector3( data[1] * V(3) - data[2] * V(2),
data[2] * V(1) - data[0] * V(3),
data[0] * V(2) - data[1] * V(1) );
}
/// Addition operator.
FGColumnVector3 operator+(const FGColumnVector3& B) const {
return FGColumnVector3( Entry(1) + B(1), Entry(2) + B(2), Entry(3) + B(3) );
return FGColumnVector3( data[0] + B(1), data[1] + B(2), data[2] + B(3) );
}
/// Subtraction operator.
FGColumnVector3 operator-(const FGColumnVector3& B) const {
return FGColumnVector3( Entry(1) - B(1), Entry(2) - B(2), Entry(3) - B(3) );
return FGColumnVector3( data[0] - B(1), data[1] - B(2), data[2] - B(3) );
}
/// Subtract an other vector.
FGColumnVector3& operator-=(const FGColumnVector3 &B) {
Entry(1) -= B(1);
Entry(2) -= B(2);
Entry(3) -= B(3);
data[0] -= B(1);
data[1] -= B(2);
data[2] -= B(3);
return *this;
}
/// Add an other vector.
FGColumnVector3& operator+=(const FGColumnVector3 &B) {
Entry(1) += B(1);
Entry(2) += B(2);
Entry(3) += B(3);
data[0] += B(1);
data[1] += B(2);
data[2] += B(3);
return *this;
}
/// Scale by a scalar.
FGColumnVector3& operator*=(const double scalar) {
Entry(1) *= scalar;
Entry(2) *= scalar;
Entry(3) *= scalar;
data[0] *= scalar;
data[1] *= scalar;
data[2] *= scalar;
return *this;
}
@ -237,7 +237,7 @@ public:
Compute and return the euclidean norm of this vector projected into
the coordinate axis plane idx1-idx2. */
double Magnitude(int idx1, int idx2) const {
return sqrt( Entry(idx1)*Entry(idx1) + Entry(idx2)*Entry(idx2) );
return sqrt( data[idx1-1]*data[idx1-1] + data[idx2-1]*data[idx2-1] );
}
/** Normalize.
@ -245,21 +245,6 @@ public:
is equal to zero it is left untouched. */
FGColumnVector3& Normalize(void);
// 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[3];

28
src/FDM/JSBSim/math/FGCondition.cpp
View file

@ -44,7 +44,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGCondition.cpp,v 1.13 2010/07/14 05:50:40 ehofman Exp $";
static const char *IdHdr = ID_CONDITION;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -122,12 +122,27 @@ FGCondition::FGCondition(const string& test, FGPropertyManager* PropertyManager)
exit(-1);
}
TestParam1 = PropertyManager->GetNode(property1, true);
TestParam1 = PropertyManager->GetNode(property1, false);
if (!TestParam1) {
cerr << fgred << " In condition: " << test << ". Unknown property "
<< property1 << " referenced." << endl
<< "Creating property. Check usage." << reset << endl;
TestParam1 = PropertyManager->GetNode(property1, true);
}
Comparison = mComparison[conditional];
if (Comparison == ecUndef) {
throw("Comparison operator: \""+conditional+"\" does not exist. Please check the conditional.");
}
if (is_number(property2)) {
TestValue = atof(property2.c_str());
} else {
TestParam2 = PropertyManager->GetNode(property2, true);
TestParam2 = PropertyManager->GetNode(property2, false);
if (!TestParam2) {
cerr << fgred << " In condition: " << test << ". Unknown property "
<< property2 << " referenced." << endl
<< "Creating property. Check usage." << reset << endl;
TestParam2 = PropertyManager->GetNode(property2, true);
}
}
}
@ -252,9 +267,12 @@ void FGCondition::PrintCondition(void )
} else {
if (TestParam2 != 0L)
cout << " " << TestParam1->GetName() << " " << conditional << " " << TestParam2->GetName();
cout << " " << TestParam1->GetRelativeName() << " "
<< conditional << " "
<< TestParam2->GetRelativeName();
else
cout << " " << TestParam1->GetName() << " " << conditional << " " << TestValue;
cout << " " << TestParam1->GetRelativeName() << " "
<< conditional << " " << TestValue;
}
}

2
src/FDM/JSBSim/math/FGCondition.h
View file

@ -44,7 +44,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_CONDITION "$Id$"
#define ID_CONDITION "$Id: FGCondition.h,v 1.5 2009/10/24 22:59:30 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

7
src/FDM/JSBSim/math/FGFunction.cpp
View file

@ -43,7 +43,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGFunction.cpp,v 1.32 2010/03/18 13:21:24 jberndt Exp $";
static const char *IdHdr = ID_FUNCTION;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -265,7 +265,10 @@ double FGFunction::GetValue(void) const
}
break;
case eQuotient:
temp /= Parameters[1]->GetValue();
if (Parameters[1]->GetValue() != 0.0)
temp /= Parameters[1]->GetValue();
else
temp = HUGE_VAL;
break;
case ePow:
temp = pow(temp,Parameters[1]->GetValue());

2
src/FDM/JSBSim/math/FGFunction.h
View file

@ -42,7 +42,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_FUNCTION "$Id$"
#define ID_FUNCTION "$Id: FGFunction.h,v 1.21 2009/11/18 04:49:02 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

77
src/FDM/JSBSim/math/FGLocation.cpp
View file

@ -45,7 +45,7 @@ INCLUDES
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGLocation.cpp,v 1.21 2010/07/02 01:48:12 jberndt Exp $";
static const char *IdHdr = ID_LOCATION;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -64,6 +64,19 @@ FGLocation::FGLocation(void)
e = 1.0;
eps2 = -1.0;
f = 1.0;
epa = 0.0;
mLon = mLat = mRadius = mGeodLat = GeodeticAltitude = 0.0;
// initial_longitude = 0.0;
mTl2ec.InitMatrix();
mTec2l.InitMatrix();
mTi2ec.InitMatrix();
mTec2i.InitMatrix();
mTi2l.InitMatrix();
mTl2i.InitMatrix();
mECLoc.InitMatrix();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -76,7 +89,7 @@ FGLocation::FGLocation(double lon, double lat, double radius)
double cosLat = cos(lat);
double sinLon = sin(lon);
double cosLon = cos(lon);
initial_longitude = lon;
a = 0.0;
b = 0.0;
a2 = 0.0;
@ -85,9 +98,15 @@ FGLocation::FGLocation(double lon, double lat, double radius)
e = 1.0;
eps2 = -1.0;
f = 1.0;
epa = 0.0;
mECLoc = FGColumnVector3( radius*cosLat*cosLon,
radius*cosLat*sinLon,
radius*sinLat );
mLon = mLat = mRadius = mGeodLat = GeodeticAltitude = 0.0;
// initial_longitude = 0.0
ComputeDerived();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -159,7 +178,7 @@ void FGLocation::SetPosition(double lon, double lat, double radius)
double cosLat = cos(lat);
double sinLon = sin(lon);
double cosLon = cos(lon);
initial_longitude = lon;
// initial_longitude = lon;
mECLoc = FGColumnVector3( radius*cosLat*cosLon,
radius*cosLat*sinLon,
radius*sinLat );
@ -176,7 +195,7 @@ void FGLocation::SetPositionGeodetic(double lon, double lat, double height)
mLon = lon;
GeodeticAltitude = height;
initial_longitude = mLon;
// initial_longitude = mLon;
double RN = a / sqrt(1.0 - e2*sin(mGeodLat)*sin(mGeodLat));
@ -204,25 +223,23 @@ void FGLocation::SetEllipse(double semimajor, double semiminor)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Compute the ECEF to ECI transformation matrix using Stevens and Lewis "Aircraft
// Control and Simulation", second edition, eqn. 1.4-12, pg. 39
// Control and Simulation", second edition, eqn. 1.4-12, pg. 39. In Stevens and Lewis
// notation, this is C_i/e, a transformation from ECEF to ECI.
const FGMatrix33& FGLocation::GetTec2i(double epa)
const FGMatrix33& FGLocation::GetTec2i(void)
{
double mu = epa - initial_longitude;
mTec2i = FGMatrix33( cos(mu), -sin(mu), 0.0,
sin(mu), cos(mu), 0.0,
0.0, 0.0, 1.0 );
return mTec2i;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// This is given in Stevens and Lewis "Aircraft
// Control and Simulation", second edition, eqn. 1.4-12, pg. 39
// The notation in Stevens and Lewis is: C_e/i. This represents a transformation
// from ECI to ECEF - and the orientation of the ECEF frame relative to the ECI
// frame.
const FGMatrix33& FGLocation::GetTi2ec(double epa)
const FGMatrix33& FGLocation::GetTi2ec(void)
{
double mu = epa - initial_longitude;
mTi2ec = FGMatrix33( cos(mu), sin(mu), 0.0,
-sin(mu), cos(mu), 0.0,
0.0, 0.0, 1.0 );
return mTi2ec;
}
@ -235,7 +252,8 @@ void FGLocation::ComputeDerivedUnconditional(void) const
// The distance of the location to the Z-axis, which is the axis
// through the poles.
double rxy = sqrt(mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY));
double r02 = mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY);
double rxy = sqrt(r02);
// Compute the sin/cos values of the longitude
double sinLon, cosLon;
@ -270,23 +288,42 @@ void FGLocation::ComputeDerivedUnconditional(void) const
// Compute the transform matrices from and to the earth centered frame.
// See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
// Eqn. 1.4-13, page 40.
// Eqn. 1.4-13, page 40. In Stevens and Lewis notation, this is C_n/e - the
// orientation of the navigation (local) frame relative to the ECEF frame,
// and a transformation from ECEF to nav (local) frame.
mTec2l = FGMatrix33( -cosLon*sinLat, -sinLon*sinLat, cosLat,
-sinLon , cosLon , 0.0 ,
-cosLon*cosLat, -sinLon*cosLat, -sinLat );
// In Stevens and Lewis notation, this is C_e/n - the
// orientation of the ECEF frame relative to the nav (local) frame,
// and a transformation from nav (local) to ECEF frame.
mTl2ec = mTec2l.Transposed();
// Calculate the inertial to ECEF and transpose matrices
double cos_epa = cos(epa);
double sin_epa = sin(epa);
mTi2ec = FGMatrix33( cos_epa, sin_epa, 0.0,
-sin_epa, cos_epa, 0.0,
0.0, 0.0, 1.0 );
mTec2i = mTi2ec.Transposed();
// Now calculate the local (or nav, or ned) frame to inertial transform matrix,
// and the inverse.
mTl2i = mTec2i * mTl2ec;
mTi2l = mTl2i.Transposed();
// Calculate the geodetic latitude base on AIAA Journal of Guidance and Control paper,
// "Improved Method for Calculating Exact Geodetic Latitude and Altitude", and
// "Improved Method for Calculating Exact Geodetic Latitude and Altitude, Revisited",
// author: I. Sofair
if (a != 0.0 && b != 0.0) {
double c, p, q, s, t, u, v, w, z, p2, u2, r02, r0;
double c, p, q, s, t, u, v, w, z, p2, u2, r0;
double Ne, P, Q0, Q, signz0, sqrt_q;
p = fabs(mECLoc(eZ))/eps2;
r02 = mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY);
s = r02/(e2*eps2);
p2 = p*p;
q = p2 - b2 + s;
@ -306,7 +343,7 @@ void FGLocation::ComputeDerivedUnconditional(void) const
z = signz0*sqrt_q*(w+sqrt(sqrt(t*t+v)-u*w-0.5*t-0.25));
Ne = a*sqrt(1+eps2*z*z/b2);
mGeodLat = asin((eps2+1.0)*(z/Ne));
r0 = sqrt(r02);
r0 = rxy;
GeodeticAltitude = r0*cos(mGeodLat) + mECLoc(eZ)*sin(mGeodLat) - a2/Ne;
}
}

180
src/FDM/JSBSim/math/FGLocation.h
View file

@ -48,7 +48,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_LOCATION "$Id$"
#define ID_LOCATION "$Id: FGLocation.h,v 1.21 2010/07/09 04:11:45 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -60,79 +60,89 @@ namespace JSBSim {
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/** Holds an arbitrary location in the earth centered reference frame.
This coordinate frame has its center in the middle of the earth.
Its x-axis points from the center of the earth towards a location
with zero latitude and longitude on the earths surface. The y-axis
points from the center of the earth towards a location with zero
latitude and 90deg longitude on the earths surface. The z-axis
points from the earths center to the geographic north pole.
/** FGLocation holds an arbitrary location in the Earth centered Earth fixed
reference frame (ECEF). This coordinate frame has its center in the middle
of the earth. The X-axis points from the center of the Earth towards a
location with zero latitude and longitude on the Earth surface. The Y-axis
points from the center of the Earth towards a location with zero latitude
and 90 deg East longitude on the Earth surface. The Z-axis points from the
Earth center to the geographic north pole.
This class provides access functions to set and get the location as
either the simple x, y and z values in ft or longitude/latitude and
the radial distance of the location from the earth center.
This class provides access functions to set and get the location as either
the simple X, Y and Z values in ft or longitude/latitude and the radial
distance of the location from the Earth center.
It is common to associate a parent frame with a location. This
frame is usually called the local horizontal frame or simply the local
frame. This frame has its x/y plane parallel to the surface of the earth
(with the assumption of a spherical earth). The x-axis points
towards north, the y-axis points towards east and the z-axis
points to the center of the earth.
It is common to associate a parent frame with a location. This frame is
usually called the local horizontal frame or simply the local frame. It is
also called the NED frame (North, East, Down), as well as the Navigation
frame. This frame has its X/Y plane parallel to the surface of the Earth
(with the assumption of a spherical Earth). The X-axis points towards north,
the Y-axis points east and the Z-axis points to the center of the Earth.
Since this frame is determined by the location, this class also
provides the rotation matrices required to transform from the
earth centered frame to the local horizontal frame and back. There
are also conversion functions for conversion of position vectors
given in the one frame to positions in the other frame.
Since the local frame is determined by the location (and NOT by the
orientation of the vehicle IN any frame), this class also provides the
rotation matrices required to transform from the Earth centered (ECEF) frame
to the local horizontal frame and back. This class also "owns" the
transformations that go from the inertial frame (Earth-centered Inertial, or
ECI) to and from the ECEF frame, as well as to and from the local frame.
Again, this is because the ECI, ECEF, and local frames do not involve the
actual orientation of the vehicle - only the location on the Earth surface,
and the angular difference between the ECI and ECEF frames. There are
conversion functions for conversion of position vectors given in the one
frame to positions in the other frame.
The earth centered reference frame is *NOT* an inertial frame
since it rotates with the earth.
The Earth centered reference frame is NOT an inertial frame since it rotates
with the Earth.
The coordinates in the earth centered frame are the master values.
All other values are computed from these master values and are
cached as long as the location is changed by access through a
non-const member function. Values are cached to improve performance.
It is best practice to work with a natural set of master values.
Other parameters that are derived from these master values are calculated
only when needed, and IF they are needed and calculated, then they are
cached (stored and remembered) so they do not need to be re-calculated
until the master values they are derived from are themselves changed
(and become stale).
The coordinates in the Earth centered frame are the master values. All other
values are computed from these master values and are cached as long as the
location is changed by access through a non-const member function. Values
are cached to improve performance. It is best practice to work with a
natural set of master values. Other parameters that are derived from these
master values are calculated only when needed, and IF they are needed and
calculated, then they are cached (stored and remembered) so they do not need
to be re-calculated until the master values they are derived from are
themselves changed (and become stale).
Accuracy and round off:
Accuracy and round off
Given that we model a vehicle near the earth, the earths surface
radius is about 2*10^7, ft and that we use double values for the
representation of the location, we have an accuracy of about
1e-16*2e7ft/1=2e-9ft left. This should be sufficient for our needs.
Note that this is the same relative accuracy we would have when we
compute directly with lon/lat/radius. For the radius value this
is clear. For the lon/lat pair this is easy to see. Take for
example KSFO located at about 37.61deg north 122.35deg west, which
corresponds to 0.65642rad north and 2.13541rad west. Both values
are of magnitude of about 1. But 1ft corresponds to about
1/(2e7*2*pi)=7.9577e-09rad. So the left accuracy with this
representation is also about 1*1e-16/7.9577e-09=1.2566e-08 which
is of the same magnitude as the representation chosen here.
Given,
The advantage of this representation is that it is a linear space
without singularities. The singularities are the north and south
pole and most notably the non-steady jump at -pi to pi. It is
harder to track this jump correctly especially when we need to
work with error norms and derivatives of the equations of motion
within the time-stepping code. Also, the rate of change is of the
same magnitude for all components in this representation which is
an advantage for numerical stability in implicit time-stepping too.
-that we model a vehicle near the Earth
-that the Earth surface radius is about 2*10^7, ft
-that we use double values for the representation of the location
Note: The latitude is a GEOCENTRIC value. FlightGear
converts latitude to a geodetic value and uses that. In order to get best
matching relative to a map, geocentric latitude must be converted to geodetic.
we have an accuracy of about
1e-16*2e7ft/1 = 2e-9 ft
left. This should be sufficient for our needs. Note that this is the same
relative accuracy we would have when we compute directly with
lon/lat/radius. For the radius value this is clear. For the lon/lat pair
this is easy to see. Take for example KSFO located at about 37.61 deg north
122.35 deg west, which corresponds to 0.65642 rad north and 2.13541 rad
west. Both values are of magnitude of about 1. But 1 ft corresponds to about
1/(2e7*2*pi) = 7.9577e-09 rad. So the left accuracy with this representation
is also about 1*1e-16/7.9577e-09 = 1.2566e-08 which is of the same magnitude
as the representation chosen here.
The advantage of this representation is that it is a linear space without
singularities. The singularities are the north and south pole and most
notably the non-steady jump at -pi to pi. It is harder to track this jump
correctly especially when we need to work with error norms and derivatives
of the equations of motion within the time-stepping code. Also, the rate of
change is of the same magnitude for all components in this representation
which is an advantage for numerical stability in implicit time-stepping.
Note: The latitude is a GEOCENTRIC value. FlightGear converts latitude to a
geodetic value and uses that. In order to get best matching relative to a
map, geocentric latitude must be converted to geodetic.
@see Stevens and Lewis, "Aircraft Control and Simulation", Second edition
@see W. C. Durham "Aircraft Dynamics & Control", section 2.2
@author Mathias Froehlich
@version $Id$
@version $Id: FGLocation.h,v 1.21 2010/07/09 04:11:45 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -244,6 +254,13 @@ public:
and semiminor axis lengths */
void SetEllipse(double semimajor, double semiminor);
/** Sets the Earth position angle.
This is the relative orientation of the ECEF frame with respect to the
Inertial frame.
@param EPA Earth fixed frame (ECEF) rotation offset about the axis with
respect to the Inertial (ECI) frame in radians. */
void SetEarthPositionAngle(double EPA) {epa = EPA; /*mCacheValid = false;*/ ComputeDerived();}
/** Get the longitude.
@return the longitude in rad of the location represented with this
class instance. The returned values are in the range between
@ -309,7 +326,7 @@ public:
@return the distance of the location represented with this class
instance to the center of the earth in ft. The radius value is
always positive. */
double GetRadius() const { ComputeDerived(); return mRadius; }
double GetRadius() const { return mECLoc.Magnitude(); }
/** Transform matrix from local horizontal to earth centered frame.
Returns a const reference to the rotation matrix of the transform from
@ -324,12 +341,16 @@ public:
/** Transform matrix from inertial to earth centered frame.
Returns a const reference to the rotation matrix of the transform from
the inertial frame to the earth centered frame (ECI to ECEF). */
const FGMatrix33& GetTi2ec(double epa);
const FGMatrix33& GetTi2ec(void);
/** Transform matrix from the earth centered to inertial frame.
Returns a const reference to the rotation matrix of the transform from
the earth centered frame to the inertial frame (ECEF to ECI). */
const FGMatrix33& GetTec2i(double epa);
const FGMatrix33& GetTec2i(void);
const FGMatrix33& GetTi2l(void) const {return mTi2l;}
const FGMatrix33& GetTl2i(void) const {return mTl2i;}
/** Conversion from Local frame coordinates to a location in the
earth centered and fixed frame.
@ -354,14 +375,14 @@ public:
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); }
double operator()(unsigned int idx) const { return mECLoc.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); }
double& operator()(unsigned int idx) { mCacheValid = false; return mECLoc.Entry(idx); }
/** Read access the entries of the vector.
@param idx the component index.
@ -385,6 +406,12 @@ public:
mCacheValid = false; return mECLoc.Entry(idx);
}
/** Sets this location via the supplied vector.
The location can be set by an Earth-centered, Earth-fixed (ECEF) frame
position vector. The cache is marked as invalid, so any future requests
for selected important data will cause the parameters to be calculated.
@param v the ECEF column vector in feet.
@return a reference to the FGLocation object. */
const FGLocation& operator=(const FGColumnVector3& v)
{
mECLoc(eX) = v(eX);
@ -395,6 +422,9 @@ public:
return *this;
}
/** Sets this location via the supplied location object.
@param v A location object reference.
@return a reference to the FGLocation object. */
const FGLocation& operator=(const FGLocation& l)
{
mECLoc = l.mECLoc;
@ -422,31 +452,47 @@ public:
return *this;
}
/** This operator returns true if the ECEF location vectors for the two
location objects are equal. */
bool operator==(const FGLocation& l) const {
return mECLoc == l.mECLoc;
}
/** This operator returns true if the ECEF location vectors for the two
location objects are not equal. */
bool operator!=(const FGLocation& l) const { return ! operator==(l); }
/** This operator adds the ECEF position vectors.
The supplied vector (right side) is added to the ECEF position vector
on the left side of the equality, and a pointer to this object is
returned. */
const FGLocation& operator+=(const FGLocation &l) {
mCacheValid = false;
mECLoc += l.mECLoc;
return *this;
}
const FGLocation& operator-=(const FGLocation &l) {
mCacheValid = false;
mECLoc -= l.mECLoc;
return *this;
}
const FGLocation& operator*=(double scalar) {
mCacheValid = false;
mECLoc *= scalar;
return *this;
}
const FGLocation& operator/=(double scalar) {
return operator*=(1.0/scalar);
}
FGLocation operator+(const FGLocation& l) const {
return FGLocation(mECLoc + l.mECLoc);
}
FGLocation operator-(const FGLocation& l) const {
return FGLocation(mECLoc - l.mECLoc);
}
@ -500,6 +546,10 @@ private:
mutable FGMatrix33 mTec2l;
mutable FGMatrix33 mTi2ec;
mutable FGMatrix33 mTec2i;
mutable FGMatrix33 mTi2l;
mutable FGMatrix33 mTl2i;
double epa;
/* Terms for geodetic latitude calculation. Values are from WGS84 model */
double a; // Earth semimajor axis in feet (6,378,137.0 meters)

351
src/FDM/JSBSim/math/FGMatrix33.cpp
View file

@ -48,7 +48,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGMatrix33.cpp,v 1.10 2010/07/01 23:13:19 jberndt Exp $";
static const char *IdHdr = ID_MATRIX33;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -70,20 +70,94 @@ FGMatrix33::FGMatrix33(void)
string FGMatrix33::Dump(const string& delimiter) const
{
ostringstream buffer;
buffer << std::setw(18) << std::setprecision(16) << Entry(1,1) << delimiter;
buffer << std::setw(18) << std::setprecision(16) << Entry(1,2) << delimiter;
buffer << std::setw(18) << std::setprecision(16) << Entry(1,3) << delimiter;
buffer << std::setw(18) << std::setprecision(16) << Entry(2,1) << delimiter;
buffer << std::setw(18) << std::setprecision(16) << Entry(2,2) << delimiter;
buffer << std::setw(18) << std::setprecision(16) << Entry(2,3) << delimiter;
buffer << std::setw(18) << std::setprecision(16) << Entry(3,1) << delimiter;
buffer << std::setw(18) << std::setprecision(16) << Entry(3,2) << delimiter;
buffer << std::setw(18) << std::setprecision(16) << Entry(3,3);
buffer << setw(12) << setprecision(10) << data[0] << delimiter;
buffer << setw(12) << setprecision(10) << data[3] << delimiter;
buffer << setw(12) << setprecision(10) << data[6] << delimiter;
buffer << setw(12) << setprecision(10) << data[1] << delimiter;
buffer << setw(12) << setprecision(10) << data[4] << delimiter;
buffer << setw(12) << setprecision(10) << data[7] << delimiter;
buffer << setw(12) << setprecision(10) << data[2] << delimiter;
buffer << setw(12) << setprecision(10) << data[5] << delimiter;
buffer << setw(12) << setprecision(10) << data[8];
return buffer.str();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
string FGMatrix33::Dump(const string& delimiter, const string& prefix) const
{
ostringstream buffer;
buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[0] << delimiter;
buffer << right << fixed << setw(9) << setprecision(6) << data[3] << delimiter;
buffer << right << fixed << setw(9) << setprecision(6) << data[6] << endl;
buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[1] << delimiter;
buffer << right << fixed << setw(9) << setprecision(6) << data[4] << delimiter;
buffer << right << fixed << setw(9) << setprecision(6) << data[7] << endl;
buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[2] << delimiter;
buffer << right << fixed << setw(9) << setprecision(6) << data[5] << delimiter;
buffer << right << fixed << setw(9) << setprecision(6) << data[8];
buffer << setw(0) << left;
return buffer.str();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGQuaternion FGMatrix33::GetQuaternion(void)
{
FGQuaternion Q;
double tempQ[4];
int idx;
tempQ[0] = 1.0 + data[0] + data[4] + data[8];
tempQ[1] = 1.0 + data[0] - data[4] - data[8];
tempQ[2] = 1.0 - data[0] + data[4] - data[8];
tempQ[3] = 1.0 - data[0] - data[4] + data[8];
// Find largest of the above
idx = 0;
for (int i=1; i<4; i++) if (tempQ[i] > tempQ[idx]) idx = i;
switch(idx) {
case 0:
Q(1) = 0.50*sqrt(tempQ[0]);
Q(2) = 0.25*(data[7] - data[5])/Q(1);
Q(3) = 0.25*(data[2] - data[6])/Q(1);
Q(4) = 0.25*(data[3] - data[1])/Q(1);
break;
case 1:
Q(2) = 0.50*sqrt(tempQ[1]);
Q(1) = 0.25*(data[7] - data[5])/Q(2);
Q(3) = 0.25*(data[3] + data[1])/Q(2);
Q(4) = 0.25*(data[2] + data[6])/Q(2);
break;
case 2:
Q(3) = 0.50*sqrt(tempQ[2]);
Q(1) = 0.25*(data[2] - data[6])/Q(3);
Q(2) = 0.25*(data[3] + data[1])/Q(3);
Q(4) = 0.25*(data[7] + data[5])/Q(3);
break;
case 3:
Q(4) = 0.50*sqrt(tempQ[3]);
Q(1) = 0.25*(data[3] - data[1])/Q(4);
Q(2) = 0.25*(data[6] + data[2])/Q(4);
Q(3) = 0.25*(data[7] + data[5])/Q(4);
break;
default:
//error
break;
}
return (Q);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ostream& operator<<(ostream& os, const FGMatrix33& M)
{
for (unsigned int i=1; i<=M.Rows(); i++) {
@ -112,9 +186,9 @@ istream& operator>>(istream& is, FGMatrix33& M)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGMatrix33::Determinant(void) const {
return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
+ Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
- Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
return data[0]*data[4]*data[8] + data[3]*data[7]*data[2]
+ data[6]*data[1]*data[5] - data[6]*data[4]*data[2]
- data[3]*data[1]*data[8] - data[7]*data[5]*data[0];
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -123,21 +197,28 @@ FGMatrix33 FGMatrix33::Inverse(void) const {
// Compute the inverse of a general matrix using Cramers rule.
// I guess googling for cramers rule gives tons of references
// for this. :)
double rdet = 1.0/Determinant();
double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
if (Determinant() != 0.0) {
double rdet = 1.0/Determinant();
return FGMatrix33( i11, i12, i13,
i21, i22, i23,
i31, i32, i33 );
double i11 = rdet*(data[4]*data[8]-data[7]*data[5]);
double i21 = rdet*(data[7]*data[2]-data[1]*data[8]);
double i31 = rdet*(data[1]*data[5]-data[4]*data[2]);
double i12 = rdet*(data[6]*data[5]-data[3]*data[8]);
double i22 = rdet*(data[0]*data[8]-data[6]*data[2]);
double i32 = rdet*(data[3]*data[2]-data[0]*data[5]);
double i13 = rdet*(data[3]*data[7]-data[6]*data[4]);
double i23 = rdet*(data[6]*data[1]-data[0]*data[7]);
double i33 = rdet*(data[0]*data[4]-data[3]*data[1]);
return FGMatrix33( i11, i12, i13,
i21, i22, i23,
i31, i32, i33 );
} else {
return FGMatrix33( 0, 0, 0,
0, 0, 0,
0, 0, 0 );
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -154,15 +235,15 @@ void FGMatrix33::InitMatrix(void)
FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
{
return FGMatrix33( Entry(1,1) - M(1,1),
Entry(1,2) - M(1,2),
Entry(1,3) - M(1,3),
Entry(2,1) - M(2,1),
Entry(2,2) - M(2,2),
Entry(2,3) - M(2,3),
Entry(3,1) - M(3,1),
Entry(3,2) - M(3,2),
Entry(3,3) - M(3,3) );
return FGMatrix33( data[0] - M.data[0],
data[3] - M.data[3],
data[6] - M.data[6],
data[1] - M.data[1],
data[4] - M.data[4],
data[7] - M.data[7],
data[2] - M.data[2],
data[5] - M.data[5],
data[8] - M.data[8] );
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -186,30 +267,30 @@ FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
{
return FGMatrix33( Entry(1,1) + M(1,1),
Entry(1,2) + M(1,2),
Entry(1,3) + M(1,3),
Entry(2,1) + M(2,1),
Entry(2,2) + M(2,2),
Entry(2,3) + M(2,3),
Entry(3,1) + M(3,1),
Entry(3,2) + M(3,2),
Entry(3,3) + M(3,3) );
return FGMatrix33( data[0] + M.data[0],
data[3] + M.data[3],
data[6] + M.data[6],
data[1] + M.data[1],
data[4] + M.data[4],
data[7] + M.data[7],
data[2] + M.data[2],
data[5] + M.data[5],
data[8] + M.data[8] );
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
{
Entry(1,1) += M(1,1);
Entry(1,2) += M(1,2);
Entry(1,3) += M(1,3);
Entry(2,1) += M(2,1);
Entry(2,2) += M(2,2);
Entry(2,3) += M(2,3);
Entry(3,1) += M(3,1);
Entry(3,2) += M(3,2);
Entry(3,3) += M(3,3);
data[0] += M.data[0];
data[3] += M.data[3];
data[6] += M.data[6];
data[1] += M.data[1];
data[4] += M.data[4];
data[7] += M.data[7];
data[2] += M.data[2];
data[5] += M.data[5];
data[8] += M.data[8];
return *this;
}
@ -218,19 +299,19 @@ FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
FGMatrix33 FGMatrix33::operator*(const double scalar) const
{
return FGMatrix33( scalar * Entry(1,1),
scalar * Entry(1,2),
scalar * Entry(1,3),
scalar * Entry(2,1),
scalar * Entry(2,2),
scalar * Entry(2,3),
scalar * Entry(3,1),
scalar * Entry(3,2),
scalar * Entry(3,3) );
return FGMatrix33( scalar * data[0],
scalar * data[3],
scalar * data[6],
scalar * data[1],
scalar * data[4],
scalar * data[7],
scalar * data[2],
scalar * data[5],
scalar * data[8] );
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/*
FGMatrix33 operator*(double scalar, FGMatrix33 &M)
{
return FGMatrix33( scalar * M(1,1),
@ -243,20 +324,20 @@ FGMatrix33 operator*(double scalar, FGMatrix33 &M)
scalar * M(3,2),
scalar * M(3,3) );
}
*/
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGMatrix33& FGMatrix33::operator*=(const double scalar)
{
Entry(1,1) *= scalar;
Entry(1,2) *= scalar;
Entry(1,3) *= scalar;
Entry(2,1) *= scalar;
Entry(2,2) *= scalar;
Entry(2,3) *= scalar;
Entry(3,1) *= scalar;
Entry(3,2) *= scalar;
Entry(3,3) *= scalar;
data[0] *= scalar;
data[3] *= scalar;
data[6] *= scalar;
data[1] *= scalar;
data[4] *= scalar;
data[7] *= scalar;
data[2] *= scalar;
data[5] *= scalar;
data[8] *= scalar;
return *this;
}
@ -265,18 +346,17 @@ FGMatrix33& FGMatrix33::operator*=(const double scalar)
FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
{
// FIXME: Make compiler friendlier
FGMatrix33 Product;
Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
Product.data[0] = data[0]*M.data[0] + data[3]*M.data[1] + data[6]*M.data[2];
Product.data[3] = data[0]*M.data[3] + data[3]*M.data[4] + data[6]*M.data[5];
Product.data[6] = data[0]*M.data[6] + data[3]*M.data[7] + data[6]*M.data[8];
Product.data[1] = data[1]*M.data[0] + data[4]*M.data[1] + data[7]*M.data[2];
Product.data[4] = data[1]*M.data[3] + data[4]*M.data[4] + data[7]*M.data[5];
Product.data[7] = data[1]*M.data[6] + data[4]*M.data[7] + data[7]*M.data[8];
Product.data[2] = data[2]*M.data[0] + data[5]*M.data[1] + data[8]*M.data[2];
Product.data[5] = data[2]*M.data[3] + data[5]*M.data[4] + data[8]*M.data[5];
Product.data[8] = data[2]*M.data[6] + data[5]*M.data[7] + data[8]*M.data[8];
return Product;
}
@ -288,20 +368,20 @@ FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
// FIXME: Make compiler friendlier
double a,b,c;
a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
a = data[0]; b=data[3]; c=data[6];
data[0] = a*M.data[0] + b*M.data[1] + c*M.data[2];
data[3] = a*M.data[3] + b*M.data[4] + c*M.data[5];
data[6] = a*M.data[6] + b*M.data[7] + c*M.data[8];
a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
a = data[1]; b=data[4]; c=data[7];
data[1] = a*M.data[0] + b*M.data[1] + c*M.data[2];
data[4] = a*M.data[3] + b*M.data[4] + c*M.data[5];
data[7] = a*M.data[6] + b*M.data[7] + c*M.data[8];
a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
a = data[2]; b=data[5]; c=data[8];
data[2] = a*M.data[0] + b*M.data[1] + c*M.data[2];
data[5] = a*M.data[3] + b*M.data[4] + c*M.data[5];
data[8] = a*M.data[6] + b*M.data[7] + c*M.data[8];
return *this;
}
@ -314,15 +394,15 @@ FGMatrix33 FGMatrix33::operator/(const double scalar) const
if ( scalar != 0 ) {
double tmp = 1.0/scalar;
Quot(1,1) = Entry(1,1) * tmp;
Quot(1,2) = Entry(1,2) * tmp;
Quot(1,3) = Entry(1,3) * tmp;
Quot(2,1) = Entry(2,1) * tmp;
Quot(2,2) = Entry(2,2) * tmp;
Quot(2,3) = Entry(2,3) * tmp;
Quot(3,1) = Entry(3,1) * tmp;
Quot(3,2) = Entry(3,2) * tmp;
Quot(3,3) = Entry(3,3) * tmp;
Quot.data[0] = data[0] * tmp;
Quot.data[3] = data[3] * tmp;
Quot.data[6] = data[6] * tmp;
Quot.data[1] = data[1] * tmp;
Quot.data[4] = data[4] * tmp;
Quot.data[7] = data[7] * tmp;
Quot.data[2] = data[2] * tmp;
Quot.data[5] = data[5] * tmp;
Quot.data[8] = data[8] * tmp;
} else {
MatrixException mE;
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
@ -337,15 +417,15 @@ FGMatrix33& FGMatrix33::operator/=(const double scalar)
{
if ( scalar != 0 ) {
double tmp = 1.0/scalar;
Entry(1,1) *= tmp;
Entry(1,2) *= tmp;
Entry(1,3) *= tmp;
Entry(2,1) *= tmp;
Entry(2,2) *= tmp;
Entry(2,3) *= tmp;
Entry(3,1) *= tmp;
Entry(3,2) *= tmp;
Entry(3,3) *= tmp;
data[0] *= tmp;
data[3] *= tmp;
data[6] *= tmp;
data[1] *= tmp;
data[4] *= tmp;
data[7] *= tmp;
data[2] *= tmp;
data[5] *= tmp;
data[8] *= tmp;
} else {
MatrixException mE;
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
@ -358,29 +438,40 @@ FGMatrix33& FGMatrix33::operator/=(const double scalar)
void FGMatrix33::T(void)
{
for (unsigned int i=1; i<=3; i++) {
for (unsigned int j=i+1; j<=3; j++) {
double tmp = Entry(i,j);
Entry(i,j) = Entry(j,i);
Entry(j,i) = tmp;
}
}
double tmp;
tmp = data[3];
data[3] = data[1];
data[1] = tmp;
tmp = data[6];
data[6] = data[2];
data[2] = tmp;
tmp = data[7];
data[7] = data[5];
data[5] = tmp;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
double tmp1 = v(1)*Entry(1,1);
double tmp2 = v(1)*Entry(2,1);
double tmp3 = v(1)*Entry(3,1);
FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const
{
double v1 = v(1);
double v2 = v(2);
double v3 = v(3);
tmp1 += v(2)*Entry(1,2);
tmp2 += v(2)*Entry(2,2);
tmp3 += v(2)*Entry(3,2);
double tmp1 = v1*data[0]; //[(col-1)*eRows+row-1]
double tmp2 = v1*data[1];
double tmp3 = v1*data[2];
tmp1 += v(3)*Entry(1,3);
tmp2 += v(3)*Entry(2,3);
tmp3 += v(3)*Entry(3,3);
tmp1 += v2*data[3];
tmp2 += v2*data[4];
tmp3 += v2*data[5];
tmp1 += v3*data[6];
tmp2 += v3*data[7];
tmp3 += v3*data[8];
return FGColumnVector3( tmp1, tmp2, tmp3 );
}

85
src/FDM/JSBSim/math/FGMatrix33.h
View file

@ -50,7 +50,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_MATRIX33 "$Id$"
#define ID_MATRIX33 "$Id: FGMatrix33.h,v 1.11 2010/06/30 03:13:40 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -59,6 +59,7 @@ FORWARD DECLARATIONS
namespace JSBSim {
class FGColumnVector3;
class FGQuaternion;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
@ -111,17 +112,17 @@ public:
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);
data[0] = M.data[0];
data[1] = M.data[1];
data[2] = M.data[2];
data[3] = M.data[3];
data[4] = M.data[4];
data[5] = M.data[5];
data[6] = M.data[6];
data[7] = M.data[7];
data[8] = M.data[8];
Debug(0);
// Debug(0);
}
/** Initialization by given values.
@ -141,28 +142,34 @@ public:
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;
data[0] = m11;
data[1] = m21;
data[2] = m31;
data[3] = m12;
data[4] = m22;
data[5] = m32;
data[6] = m13;
data[7] = m23;
data[8] = m33;
Debug(0);
// Debug(0);
}
/** Destructor.
*/
~FGMatrix33(void) { Debug(1); }
~FGMatrix33(void) { /* Debug(1); */ }
/** Prints the contents of the matrix.
@param delimeter the item separator (tab or comma)
@return a string with the delimeter-separated contents of the matrix */
std::string Dump(const std::string& delimeter) const;
/** Prints the contents of the matrix.
@param delimeter the item separator (tab or comma, etc.)
@param prefix an additional prefix that is used to indent the 3X3 matrix printout
@return a string with the delimeter-separated contents of the matrix */
std::string Dump(const std::string& delimiter, const std::string& prefix) const;
/** Read access the entries of the matrix.
@param row Row index.
@param col Column index.
@ -171,7 +178,7 @@ public:
column indices. Indices are counted starting with 1.
*/
double operator()(unsigned int row, unsigned int col) const {
return Entry(row, col);
return data[(col-1)*eRows+row-1];
}
/** Write access the entries of the matrix.
@ -184,7 +191,7 @@ public:
column indices. Indices are counted starting with 1.
*/
double& operator()(unsigned int row, unsigned int col) {
return Entry(row, col);
return data[(col-1)*eRows+row-1];
}
/** Read access the entries of the matrix.
@ -237,9 +244,9 @@ public:
@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) );
return FGMatrix33( data[0], data[1], data[2],
data[3], data[4], data[5],
data[6], data[7], data[8] );
}
/** Transposes this matrix.
@ -258,17 +265,21 @@ public:
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;
data[0] = m11;
data[1] = m21;
data[2] = m31;
data[3] = m12;
data[4] = m22;
data[5] = m32;
data[6] = m13;
data[7] = m23;
data[8] = m33;
}
/** Returns the quaternion associated with this direction cosine (rotation) matrix.
*/
FGQuaternion GetQuaternion(void);
/** Determinant of the matrix.
@return the determinant of the matrix.
*/
@ -458,4 +469,6 @@ std::istream& operator>>(std::istream& is, FGMatrix33& M);
} // namespace JSBSim
#include "FGQuaternion.h"
#endif

2
src/FDM/JSBSim/math/FGParameter.h
View file

@ -40,7 +40,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_PARAMETER "$Id$"
#define ID_PARAMETER "$Id: FGParameter.h,v 1.5 2009/08/30 03:51:28 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

2
src/FDM/JSBSim/math/FGPropertyValue.cpp
View file

@ -32,7 +32,7 @@ INCLUDES
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGPropertyValue.cpp,v 1.4 2009/08/30 03:51:28 jberndt Exp $";
static const char *IdHdr = ID_PROPERTYVALUE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/math/FGPropertyValue.h
View file

@ -41,7 +41,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_PROPERTYVALUE "$Id$"
#define ID_PROPERTYVALUE "$Id: FGPropertyValue.h,v 1.6 2009/10/02 10:30:09 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

205
src/FDM/JSBSim/math/FGQuaternion.cpp
View file

@ -40,6 +40,7 @@ SENTRY
#include <string>
#include <iostream>
#include <iomanip>
#include <cmath>
using std::cerr;
using std::cout;
@ -56,18 +57,18 @@ using std::endl;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.16 2010/06/30 03:13:40 jberndt Exp $";
static const char *IdHdr = ID_QUATERNION;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Initialize from q
FGQuaternion::FGQuaternion(const FGQuaternion& q)
: mCacheValid(q.mCacheValid) {
Entry(1) = q(1);
Entry(2) = q(2);
Entry(3) = q(3);
Entry(4) = q(4);
FGQuaternion::FGQuaternion(const FGQuaternion& q) : mCacheValid(q.mCacheValid)
{
data[0] = q(1);
data[1] = q(2);
data[2] = q(3);
data[3] = q(4);
if (mCacheValid) {
mT = q.mT;
mTInv = q.mTInv;
@ -80,8 +81,36 @@ FGQuaternion::FGQuaternion(const FGQuaternion& q)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Initialize with the three euler angles
FGQuaternion::FGQuaternion(double phi, double tht, double psi)
: mCacheValid(false) {
FGQuaternion::FGQuaternion(double phi, double tht, double psi): mCacheValid(false)
{
InitializeFromEulerAngles(phi, tht, psi);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGQuaternion::FGQuaternion(FGColumnVector3 vOrient): mCacheValid(false)
{
double phi = vOrient(ePhi);
double tht = vOrient(eTht);
double psi = vOrient(ePsi);
InitializeFromEulerAngles(phi, tht, psi);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
// This function computes the quaternion that describes the relationship of the
// body frame with respect to another frame, such as the ECI or ECEF frames. The
// Euler angles used represent a 3-2-1 (psi, theta, phi) rotation sequence from
// the reference frame to the body frame. See "Quaternions and Rotation
// Sequences", Jack B. Kuipers, sections 9.2 and 7.6.
void FGQuaternion::InitializeFromEulerAngles(double phi, double tht, double psi)
{
mEulerAngles(ePhi) = phi;
mEulerAngles(eTht) = tht;
mEulerAngles(ePsi) = psi;
double thtd2 = 0.5*tht;
double psid2 = 0.5*psi;
double phid2 = 0.5*phi;
@ -99,10 +128,27 @@ FGQuaternion::FGQuaternion(double phi, double tht, double psi)
double Sphid2Sthtd2 = Sphid2*Sthtd2;
double Sphid2Cthtd2 = Sphid2*Cthtd2;
Entry(1) = Cphid2Cthtd2*Cpsid2 + Sphid2Sthtd2*Spsid2;
Entry(2) = Sphid2Cthtd2*Cpsid2 - Cphid2Sthtd2*Spsid2;
Entry(3) = Cphid2Sthtd2*Cpsid2 + Sphid2Cthtd2*Spsid2;
Entry(4) = Cphid2Cthtd2*Spsid2 - Sphid2Sthtd2*Cpsid2;
data[0] = Cphid2Cthtd2*Cpsid2 + Sphid2Sthtd2*Spsid2;
data[1] = Sphid2Cthtd2*Cpsid2 - Cphid2Sthtd2*Spsid2;
data[2] = Cphid2Sthtd2*Cpsid2 + Sphid2Cthtd2*Spsid2;
data[3] = Cphid2Cthtd2*Spsid2 - Sphid2Sthtd2*Cpsid2;
Normalize();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Initialize with a direction cosine (rotation) matrix
FGQuaternion::FGQuaternion(const FGMatrix33& m) : mCacheValid(false)
{
data[0] = 0.50*sqrt(1.0 + m(1,1) + m(2,2) + m(3,3));
double t = 0.25/data[0];
data[1] = t*(m(2,3) - m(3,2));
data[2] = t*(m(3,1) - m(1,3));
data[3] = t*(m(1,2) - m(2,1));
ComputeDerivedUnconditional();
Normalize();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -111,37 +157,32 @@ FGQuaternion::FGQuaternion(double phi, double tht, double psi)
angular velocities PQR.
See Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
Equation 1.3-36.
Also see Jack Kuipers, "Quaternions and Rotation Sequences", Equation 11.12.
*/
FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR) const {
double norm = Magnitude();
if (norm == 0.0)
return FGQuaternion::zero();
double rnorm = 1.0/norm;
FGQuaternion QDot;
QDot(1) = -0.5*(Entry(2)*PQR(eP) + Entry(3)*PQR(eQ) + Entry(4)*PQR(eR));
QDot(2) = 0.5*(Entry(1)*PQR(eP) + Entry(3)*PQR(eR) - Entry(4)*PQR(eQ));
QDot(3) = 0.5*(Entry(1)*PQR(eQ) + Entry(4)*PQR(eP) - Entry(2)*PQR(eR));
QDot(4) = 0.5*(Entry(1)*PQR(eR) + Entry(2)*PQR(eQ) - Entry(3)*PQR(eP));
return rnorm*QDot;
FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR)
{
return FGQuaternion(
-0.5*( data[1]*PQR(eP) + data[2]*PQR(eQ) + data[3]*PQR(eR)),
0.5*( data[0]*PQR(eP) - data[3]*PQR(eQ) + data[2]*PQR(eR)),
0.5*( data[3]*PQR(eP) + data[0]*PQR(eQ) - data[1]*PQR(eR)),
0.5*(-data[2]*PQR(eP) + data[1]*PQR(eQ) + data[0]*PQR(eR))
);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGQuaternion::Normalize()
{
// Note: this does not touch the cache
// since it does not change the orientation ...
// Note: this does not touch the cache since it does not change the orientation
double norm = Magnitude();
if (norm == 0.0)
return;
if (norm == 0.0 || fabs(norm - 1.000) < 1e-10) return;
double rnorm = 1.0/norm;
Entry(1) *= rnorm;
Entry(2) *= rnorm;
Entry(3) *= rnorm;
Entry(4) *= rnorm;
data[0] *= rnorm;
data[1] *= rnorm;
data[2] *= rnorm;
data[3] *= rnorm;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -151,44 +192,41 @@ void FGQuaternion::ComputeDerivedUnconditional(void) const
{
mCacheValid = true;
// First normalize the 4-vector
double norm = Magnitude();
if (norm == 0.0)
return;
double rnorm = 1.0/norm;
double q1 = rnorm*Entry(1);
double q2 = rnorm*Entry(2);
double q3 = rnorm*Entry(3);
double q4 = rnorm*Entry(4);
double q0 = data[0]; // use some aliases/shorthand for the quat elements.
double q1 = data[1];
double q2 = data[2];
double q3 = data[3];
// Now compute the transformation matrix.
double q0q0 = q0*q0;
double q1q1 = q1*q1;
double q2q2 = q2*q2;
double q3q3 = q3*q3;
double q4q4 = q4*q4;
double q0q1 = q0*q1;
double q0q2 = q0*q2;
double q0q3 = q0*q3;
double q1q2 = q1*q2;
double q1q3 = q1*q3;
double q1q4 = q1*q4;
double q2q3 = q2*q3;
double q2q4 = q2*q4;
double q3q4 = q3*q4;
mT(1,1) = q1q1 + q2q2 - q3q3 - q4q4;
mT(1,2) = 2.0*(q2q3 + q1q4);
mT(1,3) = 2.0*(q2q4 - q1q3);
mT(2,1) = 2.0*(q2q3 - q1q4);
mT(2,2) = q1q1 - q2q2 + q3q3 - q4q4;
mT(2,3) = 2.0*(q3q4 + q1q2);
mT(3,1) = 2.0*(q2q4 + q1q3);
mT(3,2) = 2.0*(q3q4 - q1q2);
mT(3,3) = q1q1 - q2q2 - q3q3 + q4q4;
// Since this is an orthogonal matrix, the inverse is simply
// the transpose.
mT(1,1) = q0q0 + q1q1 - q2q2 - q3q3; // This is found from Eqn. 1.3-32 in
mT(1,2) = 2.0*(q1q2 + q0q3); // Stevens and Lewis
mT(1,3) = 2.0*(q1q3 - q0q2);
mT(2,1) = 2.0*(q1q2 - q0q3);
mT(2,2) = q0q0 - q1q1 + q2q2 - q3q3;
mT(2,3) = 2.0*(q2q3 + q0q1);
mT(3,1) = 2.0*(q1q3 + q0q2);
mT(3,2) = 2.0*(q2q3 - q0q1);
mT(3,3) = q0q0 - q1q1 - q2q2 + q3q3;
// Since this is an orthogonal matrix, the inverse is simply the transpose.
mTInv = mT;
mTInv.T();
// Compute the Euler-angles
// Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8..
if (mT(3,3) == 0.0)
mEulerAngles(ePhi) = 0.5*M_PI;
else
@ -218,6 +256,55 @@ void FGQuaternion::ComputeDerivedUnconditional(void) const
mEulerCosines(ePhi) = cos(mEulerAngles(ePhi));
mEulerCosines(eTht) = cos(mEulerAngles(eTht));
mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
// Debug(2);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// 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 FGQuaternion::Debug(int from) const
{
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: FGQuaternion" << endl;
if (from == 1) cout << "Destroyed: FGQuaternion" << 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 (!EqualToRoundoff(Magnitude(), 1.0)) {
cout << "Quaternion magnitude differs from 1.0 !" << endl;
cout << "\tdelta from 1.0: " << std::scientific << Magnitude()-1.0 << endl;
}
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
cout << IdSrc << endl;
cout << IdHdr << endl;
}
}
}
} // namespace JSBSim

165
src/FDM/JSBSim/math/FGQuaternion.h
View file

@ -41,18 +41,18 @@ SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGJSBBase.h"
#include "FGMatrix33.h"
#include "FGColumnVector3.h"
#include "input_output/FGPropertyManager.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_QUATERNION "$Id$"
#define ID_QUATERNION "$Id: FGQuaternion.h,v 1.17 2010/06/30 03:13:40 jberndt Exp $"
namespace JSBSim {
class FGMatrix33;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
@ -88,14 +88,13 @@ namespace JSBSim {
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
class FGQuaternion
: virtual FGJSBBase {
class FGQuaternion : virtual FGJSBBase {
public:
/** Default initializer.
Default initializer, initializes the class with the identity rotation. */
FGQuaternion() : mCacheValid(false) {
Entry(1) = 1.0;
Entry(2) = Entry(3) = Entry(4) = 0.0;
data[0] = 1.0;
data[1] = data[2] = data[3] = 0.0;
}
/** Copy constructor.
@ -110,6 +109,11 @@ public:
@param psi The euler Z axis (heading) angle in radians */
FGQuaternion(double phi, double tht, double psi);
/** Initializer by euler angle vector.
Initialize the quaternion with the euler angle vector.
@param vOrient The euler axis angle vector in radians (phi, tht, psi) */
FGQuaternion(FGColumnVector3 vOrient);
/** Initializer by one euler angle.
Initialize the quaternion with the single euler angle where its index
is given in the first argument.
@ -117,43 +121,50 @@ public:
@param angle The euler angle in radians */
FGQuaternion(int idx, double angle)
: mCacheValid(false) {
double angle2 = 0.5*angle;
double Sangle2 = sin(angle2);
double Cangle2 = cos(angle2);
if (idx == ePhi) {
Entry(1) = Cangle2;
Entry(2) = Sangle2;
Entry(3) = 0.0;
Entry(4) = 0.0;
data[0] = Cangle2;
data[1] = Sangle2;
data[2] = 0.0;
data[3] = 0.0;
} else if (idx == eTht) {
Entry(1) = Cangle2;
Entry(2) = 0.0;
Entry(3) = Sangle2;
Entry(4) = 0.0;
data[0] = Cangle2;
data[1] = 0.0;
data[2] = Sangle2;
data[3] = 0.0;
} else {
Entry(1) = Cangle2;
Entry(2) = 0.0;
Entry(3) = 0.0;
Entry(4) = Sangle2;
data[0] = Cangle2;
data[1] = 0.0;
data[2] = 0.0;
data[3] = Sangle2;
}
}
/** Initializer by matrix.
Initialize the quaternion with the matrix representing a transform from one frame
to another using the standard aerospace sequence, Yaw-Pitch-Roll (3-2-1).
@param m the rotation matrix */
FGQuaternion(const FGMatrix33& m);
/// Destructor.
~FGQuaternion() {}
/** Quaternion derivative for given angular rates.
Computes the quaternion derivative which results from the given
angular velocities
@param PQR a constant reference to the body rate vector
@param PQR a constant reference to a rotation rate vector
@return the quaternion derivative
@see Stevens and Lewis, "Aircraft Control and Simulation", Second Edition,
Equation 1.3-36. */
FGQuaternion GetQDot(const FGColumnVector3& PQR) const;
FGQuaternion GetQDot(const FGColumnVector3& PQR);
/** Transformation matrix.
@return a reference to the transformation/rotation matrix
@ -220,7 +231,7 @@ public:
Note that the index given in the argument is unchecked.
*/
double operator()(unsigned int idx) const { return Entry(idx); }
double operator()(unsigned int idx) const { return data[idx-1]; }
/** Write access the entries of the vector.
@ -231,7 +242,7 @@ public:
Note that the index given in the argument is unchecked.
*/
double& operator()(unsigned int idx) { return Entry(idx); }
double& operator()(unsigned int idx) { mCacheValid = false; return data[idx-1]; }
/** Read access the entries of the vector.
@ -246,7 +257,7 @@ public:
Note that the index given in the argument is unchecked.
*/
double Entry(unsigned int idx) const { return mData[idx-1]; }
double Entry(unsigned int idx) const { return data[idx-1]; }
/** Write access the entries of the vector.
@ -261,7 +272,10 @@ public:
Note that the index given in the argument is unchecked.
*/
double& Entry(unsigned int idx) { mCacheValid = false; return mData[idx-1]; }
double& Entry(unsigned int idx) {
mCacheValid = false;
return data[idx-1];
}
/** Assignment operator "=".
Assign the value of q to the current object. Cached values are
@ -270,10 +284,11 @@ public:
@return reference to a quaternion object */
const FGQuaternion& operator=(const FGQuaternion& q) {
// Copy the master values ...
Entry(1) = q(1);
Entry(2) = q(2);
Entry(3) = q(3);
Entry(4) = q(4);
data[0] = q(1);
data[1] = q(2);
data[2] = q(3);
data[3] = q(4);
ComputeDerived();
// .. and copy the derived values if they are valid
mCacheValid = q.mCacheValid;
if (mCacheValid) {
@ -286,12 +301,15 @@ public:
return *this;
}
/// Conversion from Quat to Matrix
operator FGMatrix33() const { return GetT(); }
/** Comparison operator "==".
@param q a quaternion reference
@return true if both quaternions represent the same rotation. */
bool operator==(const FGQuaternion& q) const {
return Entry(1) == q(1) && Entry(2) == q(2)
&& Entry(3) == q(3) && Entry(4) == q(4);
return data[0] == q(1) && data[1] == q(2)
&& data[2] == q(3) && data[3] == q(4);
}
/** Comparison operator "!=".
@ -300,10 +318,10 @@ public:
bool operator!=(const FGQuaternion& q) const { return ! operator==(q); }
const FGQuaternion& operator+=(const FGQuaternion& q) {
// Copy the master values ...
Entry(1) += q(1);
Entry(2) += q(2);
Entry(3) += q(3);
Entry(4) += q(4);
data[0] += q(1);
data[1] += q(2);
data[2] += q(3);
data[3] += q(4);
mCacheValid = false;
return *this;
}
@ -313,10 +331,10 @@ public:
@return a quaternion reference representing Q, where Q = Q - q. */
const FGQuaternion& operator-=(const FGQuaternion& q) {
// Copy the master values ...
Entry(1) -= q(1);
Entry(2) -= q(2);
Entry(3) -= q(3);
Entry(4) -= q(4);
data[0] -= q(1);
data[1] -= q(2);
data[2] -= q(3);
data[3] -= q(4);
mCacheValid = false;
return *this;
}
@ -325,10 +343,10 @@ public:
@param scalar a multiplicative value.
@return a quaternion reference representing Q, where Q = Q * scalar. */
const FGQuaternion& operator*=(double scalar) {
Entry(1) *= scalar;
Entry(2) *= scalar;
Entry(3) *= scalar;
Entry(4) *= scalar;
data[0] *= scalar;
data[1] *= scalar;
data[2] *= scalar;
data[3] *= scalar;
mCacheValid = false;
return *this;
}
@ -344,16 +362,16 @@ public:
@param q a quaternion to be summed.
@return a quaternion representing Q, where Q = Q + q. */
FGQuaternion operator+(const FGQuaternion& q) const {
return FGQuaternion(Entry(1)+q(1), Entry(2)+q(2),
Entry(3)+q(3), Entry(4)+q(4));
return FGQuaternion(data[0]+q(1), data[1]+q(2),
data[2]+q(3), data[3]+q(4));
}
/** Arithmetic operator "-".
@param q a quaternion to be subtracted.
@return a quaternion representing Q, where Q = Q - q. */
FGQuaternion operator-(const FGQuaternion& q) const {
return FGQuaternion(Entry(1)-q(1), Entry(2)-q(2),
Entry(3)-q(3), Entry(4)-q(4));
return FGQuaternion(data[0]-q(1), data[1]-q(2),
data[2]-q(3), data[3]-q(4));
}
/** Arithmetic operator "*".
@ -361,10 +379,10 @@ public:
@param q a quaternion to be multiplied.
@return a quaternion representing Q, where Q = Q * q. */
FGQuaternion operator*(const FGQuaternion& q) const {
return FGQuaternion(Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4),
Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3),
Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2),
Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1));
return FGQuaternion(data[0]*q(1)-data[1]*q(2)-data[2]*q(3)-data[3]*q(4),
data[0]*q(2)+data[1]*q(1)+data[2]*q(4)-data[3]*q(3),
data[0]*q(3)-data[1]*q(4)+data[2]*q(1)+data[3]*q(2),
data[0]*q(4)+data[1]*q(3)-data[2]*q(2)+data[3]*q(1));
}
/** Arithmetic operator "*=".
@ -372,14 +390,14 @@ public:
@param q a quaternion to be multiplied.
@return a quaternion reference representing Q, where Q = Q * q. */
const FGQuaternion& operator*=(const FGQuaternion& q) {
double q0 = Entry(1)*q(1)-Entry(2)*q(2)-Entry(3)*q(3)-Entry(4)*q(4);
double q1 = Entry(1)*q(2)+Entry(2)*q(1)+Entry(3)*q(4)-Entry(4)*q(3);
double q2 = Entry(1)*q(3)-Entry(2)*q(4)+Entry(3)*q(1)+Entry(4)*q(2);
double q3 = Entry(1)*q(4)+Entry(2)*q(3)-Entry(3)*q(2)+Entry(4)*q(1);
Entry(1) = q0;
Entry(2) = q1;
Entry(3) = q2;
Entry(4) = q3;
double q0 = data[0]*q(1)-data[1]*q(2)-data[2]*q(3)-data[3]*q(4);
double q1 = data[0]*q(2)+data[1]*q(1)+data[2]*q(4)-data[3]*q(3);
double q2 = data[0]*q(3)-data[1]*q(4)+data[2]*q(1)+data[3]*q(2);
double q3 = data[0]*q(4)+data[1]*q(3)-data[2]*q(2)+data[3]*q(1);
data[0] = q0;
data[1] = q1;
data[2] = q2;
data[3] = q3;
mCacheValid = false;
return *this;
}
@ -391,12 +409,12 @@ public:
the identity orientation.
*/
FGQuaternion Inverse(void) const {
double norm = Magnitude();
double norm = SqrMagnitude();
if (norm == 0.0)
return *this;
double rNorm = 1.0/norm;
return FGQuaternion( Entry(1)*rNorm, -Entry(2)*rNorm,
-Entry(3)*rNorm, -Entry(4)*rNorm );
return FGQuaternion( data[0]*rNorm, -data[1]*rNorm,
-data[2]*rNorm, -data[3]*rNorm );
}
/** Conjugate of the quaternion.
@ -405,7 +423,7 @@ public:
to the inverse iff the quaternion is normalized.
*/
FGQuaternion Conjugate(void) const {
return FGQuaternion( Entry(1), -Entry(2), -Entry(3), -Entry(4) );
return FGQuaternion( data[0], -data[1], -data[2], -data[3] );
}
friend FGQuaternion operator*(double, const FGQuaternion&);
@ -421,11 +439,11 @@ public:
Compute and return the square of the euclidean norm of this vector.
*/
double SqrMagnitude(void) const {
return Entry(1)*Entry(1)+Entry(2)*Entry(2)
+Entry(3)*Entry(3)+Entry(4)*Entry(4);
return data[0]*data[0] + data[1]*data[1]
+ data[2]*data[2] + data[3]*data[3];
}
/** Normialze.
/** Normalize.
Normalize the vector to have the Magnitude() == 1.0. If the vector
is equal to zero it is left untouched.
@ -439,7 +457,7 @@ public:
private:
/** Copying by assigning the vector valued components. */
FGQuaternion(double q1, double q2, double q3, double q4) : mCacheValid(false)
{ Entry(1) = q1; Entry(2) = q2; Entry(3) = q3; Entry(4) = q4; }
{ data[0] = q1; data[1] = q2; data[2] = q3; data[3] = q4; }
/** Computation of derived values.
This function recomputes the derived values like euler angles and
@ -450,16 +468,15 @@ private:
This function checks if the derived values like euler angles and
transformation matrices are already computed. If so, it
returns. If they need to be computed the real worker routine
\ref FGQuaternion::ComputeDerivedUnconditional(void) const
is called.
This function is inlined to avoid function calls in the fast path. */
FGQuaternion::ComputeDerivedUnconditional(void) const
is called. */
void ComputeDerived(void) const {
if (!mCacheValid)
ComputeDerivedUnconditional();
}
/** The quaternion values itself. This is the master copy. */
double mData[4];
double data[4];
/** A data validity flag.
This class implements caching of the derived values like the
@ -479,6 +496,10 @@ private:
/** The cached sines and cosines of the euler angles. */
mutable FGColumnVector3 mEulerSines;
mutable FGColumnVector3 mEulerCosines;
void Debug(int from) const;
void InitializeFromEulerAngles(double phi, double tht, double psi);
};
/** Scalar multiplication.
@ -494,4 +515,6 @@ inline FGQuaternion operator*(double scalar, const FGQuaternion& q) {
} // namespace JSBSim
#include "FGMatrix33.h"
#endif

2
src/FDM/JSBSim/math/FGRealValue.cpp
View file

@ -32,7 +32,7 @@ INCLUDES
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGRealValue.cpp,v 1.4 2009/08/30 03:51:28 jberndt Exp $";
static const char *IdHdr = ID_REALVALUE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/math/FGRealValue.h
View file

@ -40,7 +40,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_REALVALUE "$Id$"
#define ID_REALVALUE "$Id: FGRealValue.h,v 1.4 2009/08/30 03:51:28 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

230
src/FDM/JSBSim/math/FGRungeKutta.cpp Normal file
View file

@ -0,0 +1,230 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Header: FGRungeKutta.cpp
Author: Thomas Kreitler
Date started: 04/9/2010
------------- Copyright (C) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser 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 Lesser General Public License for more
details.
You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on
the world wide web at http://www.gnu.org.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include <cstdio>
#include <iostream>
#include <cmath>
#include "FGRungeKutta.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
using std::cout;
using std::endl;
namespace JSBSim {
static const char *IdSrc = "$Id: FGRungeKutta.cpp,v 1.1 2010/06/02 04:05:13 jberndt Exp $";
static const char *IdHdr = ID_RUNGEKUTTA;
const double FGRungeKutta::RealLimit = 1e30;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
FGRungeKutta::~FGRungeKutta() { };
int FGRungeKutta::init(double x_start, double x_end, int intervals)
{
x0 = x_start;
x1 = x_end;
h = (x_end - x_start)/intervals;
safer_x1 = x1 - h*1e-6; // avoid 'intervals*h < x1'
h05 = h*0.5;
err = 0.0;
if (x0>=x1) {
status &= eFaultyInit;
}
return status;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/*
Make sure that a numerical result is within +/-RealLimit.
This is a hapless try to be portable.
(There will be at least one architecture/compiler combination
where this will fail.)
*/
bool FGRungeKutta::sane_val(double x)
{
// assuming +/- inf behave as expected and 'nan' comparisons yield to false
if ( x < RealLimit && x > -RealLimit ) return true;
return false;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGRungeKutta::evolve(double y_0, FGRungeKuttaProblem *pf)
{
double x = x0;
double y = y_0;
pfo = pf;
iterations = 0;
if (!trace_values) {
while (x<safer_x1) {
y = approximate(x,y);
if (!sane_val(y)) { status &= eMathError; }
x += h;
iterations++;
}
} else {
while (x<safer_x1) {
cout << x << " " << y << endl;
y = approximate(x,y);
if (!sane_val(y)) { status &= eMathError; }
x += h;
iterations++;
}
cout << x << " " << y << endl;
}
x_end = x; // twimc, store the last x used.
return y;
}
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
FGRK4::~FGRK4() { };
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGRK4::approximate(double x, double y)
{
double k1,k2,k3,k4;
k1 = pfo->pFunc(x , y );
k2 = pfo->pFunc(x + h05, y + h05*k1);
k3 = pfo->pFunc(x + h05, y + h05*k2);
k4 = pfo->pFunc(x + h , y + h *k3);
y += h/6.0 * ( k1 + 2.0*k2 + 2.0*k3 + k4 );
return y;
}
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
// Butcher tableau
const double FGRKFehlberg::A2[] = { 0.0, 1.0/4.0 };
const double FGRKFehlberg::A3[] = { 0.0, 3.0/32.0, 9.0/32.0 };
const double FGRKFehlberg::A4[] = { 0.0, 1932.0/2197.0, -7200.0/2197.0, 7296.0/2197.0 };
const double FGRKFehlberg::A5[] = { 0.0, 439.0/216.0, -8.0, 3680.0/513.0, -845.0/4104.0 };
const double FGRKFehlberg::A6[] = { 0.0, -8.0/27.0, 2.0, -3544.0/2565.0, 1859.0/4104.0, -11.0/40.0 };
const double FGRKFehlberg::C[] = { 0.0, 0.0, 1.0/4.0, 3.0/8.0, 12.0/13.0, 1.0, 1.0/2.0 };
const double FGRKFehlberg::B[] = { 0.0, 16.0/135.0, 0.0, 6656.0/12825.0, 28561.0/56430.0, -9.0/50.0, 2.0/55.0 };
const double FGRKFehlberg::Bs[] = { 0.0, 25.0/216.0, 0.0, 1408.0/2565.0, 2197.0/4104.0, -1.0/5.0, 0.0 };
// use this if truncation is an issue
// const double Ee[] = { 0.0, 1.0/360.0, 0.0, -128.0/4275.0, -2197.0/75240.0, 1.0/50.0, 2.0/55.0 };
FGRKFehlberg::~FGRKFehlberg() { };
double FGRKFehlberg::approximate(double x, double y)
{
double k1,k2,k3,k4,k5,k6, as;
double y4_val;
double y5_val;
double abs_err;
double est_step;
int done = 0;
while (!done) {
err = h*h*h*h*h; // h might change
k1 = pfo->pFunc(x , y );
as = h*A2[1]*k1;
k2 = pfo->pFunc(x + C[2]*h , y + as );
as = h*(A3[1]*k1 + A3[2]*k2);
k3 = pfo->pFunc(x + C[3]*h , y + as );
as = h*(A4[1]*k1 + A4[2]*k2 + A4[3]*k3);
k4 = pfo->pFunc(x + C[4]*h , y + as );
as = h*(A5[1]*k1 + A5[2]*k2 + A5[3]*k3 + A5[4]*k4);
k5 = pfo->pFunc(x + C[5]*h , y + as );
as = h*(A6[1]*k1 + A6[2]*k2 + A6[3]*k3 + A6[4]*k4 + A6[5]*k5);
k6 = pfo->pFunc(x + C[6]*h , y + as );
/* B[2]*k2 and Bs[2]*k2 are zero */
y5_val = y + h * ( B[1]*k1 + B[3]*k3 + B[4]*k4 + B[5]*k5 + B[6]*k6);
y4_val = y + h * (Bs[1]*k1 + Bs[3]*k3 + Bs[4]*k4 + Bs[5]*k5);
abs_err = fabs(y4_val-y5_val);
// same in green
// abs_err = h * (Ee[1] * k1 + Ee[3] * k3 + Ee[4] * k4 + Ee[5] * k5 + Ee[6] * k6);
// estimate step size
if (abs_err > epsilon) {
est_step = sqrt(sqrt(epsilon*h/abs_err));
} else {
est_step=2.0*h; // cheat
}
// check if a smaller step size is proposed
if (shrink_avail>0 && est_step<h) {
h/=2.0;
shrink_avail--;
} else {
done = 1;
}
}
return y4_val;
}
} // namespace JSBSim

189
src/FDM/JSBSim/math/FGRungeKutta.h Normal file
View file

@ -0,0 +1,189 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Header: FGRungeKutta.h
Author: Thomas Kreitler
Date started: 04/9/2010
------------- Copyright (C) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser 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 Lesser General Public License for more
details.
You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on
the world wide web at http://www.gnu.org.
HISTORY
--------------------------------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#ifndef FGRUNGEKUTTA_H
#define FGRUNGEKUTTA_H
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
// #include "FGJSBBase.h" // later
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_RUNGEKUTTA "$Id: FGRungeKutta.h,v 1.1 2010/06/02 04:05:13 jberndt Exp $"
namespace JSBSim {
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/**
Minimalistic implementation of some Runge-Kutta methods. Runge-Kutta methods
are a standard for solving ordinary differential equation (ODE) initial
value problems. The code follows closely the description given on
Wikipedia, see http://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods.
For more powerfull routines see GNU Scientific Library (GSL)
or GNU Plotutils 'ode'.
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DECLARATION: FGRungeKuttaProblem
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/**
Abstract base for the function to solve.
*/
class FGRungeKuttaProblem {
public:
virtual double pFunc(double x, double y) = 0;
};
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DECLARATION: FGRungeKutta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/**
Abstract base.
*/
class FGRungeKutta {
public:
enum eStates { eNoError=0, eMathError=1, eFaultyInit=2, eEvolve=4, eUnknown=8} ;
int init(double x_start, double x_end, int intervals = 4);
double evolve(double y_0, FGRungeKuttaProblem *pf);
double getXEnd() { return x_end; }
double getError() { return err; }
int getStatus() { return status; }
int getIterations() { return iterations; }
void clearStatus() { status = eNoError; }
void setTrace(bool t) { trace_values = t; }
protected:
// avoid accidents
FGRungeKutta(): status(eNoError), trace_values(false), iterations(0) {};
virtual ~FGRungeKutta();
FGRungeKuttaProblem *pfo;
double h;
double h05; // h*0.5, halfwidth
double err;
private:
virtual double approximate(double x, double y) = 0;
bool sane_val(double x);
static const double RealLimit;
double x0, x1;
double safer_x1;
double x_end;
int status;
bool trace_values;
int iterations;
};
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DECLARATION: FGRK4
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/**
Classical RK4.
*/
class FGRK4 : public FGRungeKutta {
virtual ~FGRK4();
private:
double approximate(double x, double y);
};
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DECLARATION: FGRKFehlberg
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/**
Runge-Kutta-Fehlberg method.
This is a semi adaptive implementation of rkf - the interval only
shrinks. As a result interval calculations remain trivial, but
sometimes too many calculations are performed.
Rationale: this code is not meant to be a universal pain-reliever
for ode's. Rather it provides some safety if the number of
intervals is set too low, or the problem function behaves a bit
nasty in rare conditions.
*/
class FGRKFehlberg : public FGRungeKutta {
public:
FGRKFehlberg() : shrink_avail(4), epsilon(1e-12) { };
virtual ~FGRKFehlberg();
double getEpsilon() { return epsilon; }
int getShrinkAvail() { return shrink_avail; }
void setEpsilon(double e) { epsilon = e; }
void setShrinkAvail(int s) { shrink_avail = s; }
private:
double approximate(double x, double y);
int shrink_avail;
double epsilon;
static const double A2[], A3[], A4[], A5[], A6[];
static const double B[], Bs[], C[];
};
} // namespace JSBSim
#endif

8
src/FDM/JSBSim/math/FGTable.cpp
View file

@ -47,7 +47,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGTable.cpp,v 1.21 2010/04/07 03:08:37 jberndt Exp $";
static const char *IdHdr = ID_TABLE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -165,9 +165,7 @@ FGTable::FGTable(FGPropertyManager* propMan, Element* el) : PropertyManager(prop
node = PropertyManager->GetNode(property_string);
if (node == 0) {
cerr << "IndependenVar property, " << property_string
<< " in Table definition is not defined." << endl;
abort();
throw("IndependenVar property, " + property_string + " in Table definition is not defined.");
}
lookup_axis = axisElement->GetAttributeValue("lookup");
@ -177,6 +175,8 @@ FGTable::FGTable(FGPropertyManager* propMan, Element* el) : PropertyManager(prop
lookupProperty[eColumn] = node;
} else if (lookup_axis == string("table")) {
lookupProperty[eTable] = node;
} else if (!lookup_axis.empty()) {
throw("Lookup table axis specification not understood: " + lookup_axis);
} else { // assumed single dimension table; row lookup
lookupProperty[eRow] = node;
}

4
src/FDM/JSBSim/math/FGTable.h
View file

@ -47,7 +47,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_TABLE "$Id$"
#define ID_TABLE "$Id: FGTable.h,v 1.11 2009/10/24 22:59:30 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -233,7 +233,7 @@ combustion_efficiency = Lookup_Combustion_Efficiency->GetValue(equivalence_ratio
@endcode
@author Jon S. Berndt
@version $Id$
@version $Id: FGTable.h,v 1.11 2009/10/24 22:59:30 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

4
src/FDM/JSBSim/math/Makefile.am
View file

@ -2,10 +2,10 @@ noinst_LIBRARIES = libMath.a
libMath_a_SOURCES = FGColumnVector3.cpp FGFunction.cpp FGLocation.cpp FGMatrix33.cpp \
FGPropertyValue.cpp FGQuaternion.cpp FGRealValue.cpp FGTable.cpp \
FGCondition.cpp
FGCondition.cpp FGRungeKutta.cpp
noinst_HEADERS = FGColumnVector3.h FGFunction.h FGLocation.h FGMatrix33.h \
FGParameter.h FGPropertyValue.h FGQuaternion.h FGRealValue.h FGTable.h \
FGCondition.h
FGCondition.h FGRungeKutta.h
INCLUDES = -I$(top_srcdir)/src/FDM/JSBSim

2
src/FDM/JSBSim/models/FGAerodynamics.cpp
View file

@ -52,7 +52,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGAerodynamics.cpp,v 1.31 2009/11/28 14:30:11 andgi Exp $";
static const char *IdHdr = ID_AERODYNAMICS;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

@ -52,7 +52,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_AERODYNAMICS "$Id$"
#define ID_AERODYNAMICS "$Id: FGAerodynamics.h,v 1.20 2009/11/12 13:08:11 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -109,7 +109,7 @@ CLASS DOCUMENTATION
Systems may NOT be combined, or a load error will occur.
@author Jon S. Berndt, Tony Peden
@version $Revision$
@version $Revision: 1.20 $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -214,7 +214,6 @@ private:
typedef std::map<std::string,int> AxisIndex;
AxisIndex AxisIdx;
FGFunction* AeroRPShift;
std::vector <FGFunction*> variables;
typedef vector <FGFunction*> CoeffArray;
CoeffArray* Coeff;
FGColumnVector3 vFnative;

5
src/FDM/JSBSim/models/FGAircraft.cpp
View file

@ -68,7 +68,7 @@ DEFINITIONS
GLOBAL DATA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGAircraft.cpp,v 1.26 2010/02/15 03:28:24 jberndt Exp $";
static const char *IdHdr = ID_AIRCRAFT;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -123,6 +123,9 @@ bool FGAircraft::Run(void)
vForces += GroundReactions->GetForces();
vForces += ExternalReactions->GetForces();
vForces += BuoyantForces->GetForces();
} else {
const FGMatrix33& mTl2b = Propagate->GetTl2b();
vForces = mTl2b * FGColumnVector3(0,0,-MassBalance->GetWeight());
}
vMoments.InitMatrix();

4
src/FDM/JSBSim/models/FGAircraft.h
View file

@ -49,7 +49,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_AIRCRAFT "$Id$"
#define ID_AIRCRAFT "$Id: FGAircraft.h,v 1.15 2009/10/24 22:59:30 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -90,7 +90,7 @@ CLASS DOCUMENTATION
@endcode
@author Jon S. Berndt
@version $Id$
@version $Id: FGAircraft.h,v 1.15 2009/10/24 22:59:30 jberndt Exp $
@see 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

8
src/FDM/JSBSim/models/FGAtmosphere.cpp
View file

@ -48,11 +48,11 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGAtmosphere.h"
#include "FGState.h"
#include "FGFDMExec.h"
#include "FGAircraft.h"
#include "FGPropagate.h"
#include "FGInertial.h"
#include "FGAuxiliary.h"
#include "FGFDMExec.h"
#include "input_output/FGPropertyManager.h"
#include <iostream>
#include <cstdlib>
@ -61,7 +61,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGAtmosphere.cpp,v 1.33 2010/02/25 05:21:36 jberndt Exp $";
static const char *IdHdr = ID_ATMOSPHERE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -383,7 +383,7 @@ void FGAtmosphere::SetWindPsi(double dir)
void FGAtmosphere::Turbulence(void)
{
double DeltaT = rate*State->Getdt();
double DeltaT = rate*FDMExec->GetDeltaT();
switch (turbType) {
case ttStandard: {

4
src/FDM/JSBSim/models/FGAtmosphere.h
View file

@ -49,7 +49,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_ATMOSPHERE "$Id$"
#define ID_ATMOSPHERE "$Id: FGAtmosphere.h,v 1.22 2009/10/02 10:30:09 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -63,7 +63,7 @@ CLASS DOCUMENTATION
/** Models the 1976 Standard Atmosphere.
@author Tony Peden, Jon Berndt
@version $Id$
@version $Id: FGAtmosphere.h,v 1.22 2009/10/02 10:30:09 jberndt Exp $
@see Anderson, John D. "Introduction to Flight, Third Edition", McGraw-Hill,
1989, ISBN 0-07-001641-0
*/

15
src/FDM/JSBSim/models/FGAuxiliary.cpp
View file

@ -59,7 +59,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGAuxiliary.cpp,v 1.39 2010/07/09 12:06:11 jberndt Exp $";
static const char *IdHdr = ID_AUXILIARY;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -249,11 +249,12 @@ bool FGAuxiliary::Run()
vPilotAccel.InitMatrix();
if ( Vt > 1.0 ) {
vAircraftAccel = Aerodynamics->GetForces()
+ Propulsion->GetForces()
+ GroundReactions->GetForces()
+ ExternalReactions->GetForces()
+ BuoyantForces->GetForces();
// Use the "+=" operator to avoid the creation of temporary objects.
vAircraftAccel = Aerodynamics->GetForces();
vAircraftAccel += Propulsion->GetForces();
vAircraftAccel += GroundReactions->GetForces();
vAircraftAccel += ExternalReactions->GetForces();
vAircraftAccel += BuoyantForces->GetForces();
vAircraftAccel /= MassBalance->GetMass();
// Nz is Acceleration in "g's", along normal axis (-Z body axis)
@ -276,7 +277,7 @@ bool FGAuxiliary::Run()
// VRP computation
const FGLocation& vLocation = Propagate->GetLocation();
FGColumnVector3 vrpStructural = Aircraft->GetXYZvrp();
FGColumnVector3& vrpStructural = Aircraft->GetXYZvrp();
FGColumnVector3 vrpBody = MassBalance->StructuralToBody( vrpStructural );
FGColumnVector3 vrpLocal = Propagate->GetTb2l() * vrpBody;
vLocationVRP = vLocation.LocalToLocation( vrpLocal );

4
src/FDM/JSBSim/models/FGAuxiliary.h
View file

@ -48,7 +48,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_AUXILIARY "$Id$"
#define ID_AUXILIARY "$Id: FGAuxiliary.h,v 1.17 2009/10/24 22:59:30 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -100,7 +100,7 @@ CLASS DOCUMENTATION
The radius R is calculated below in the vector vToEyePt.
@author Tony Peden, Jon Berndt
@version $Id$
@version $Id: FGAuxiliary.h,v 1.17 2009/10/24 22:59:30 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

25
src/FDM/JSBSim/models/FGBuoyantForces.cpp
View file

@ -5,7 +5,7 @@
Date started: 01/21/08
Purpose: Encapsulates the buoyant forces
------------- Copyright (C) 2008 - 2009 Anders Gidenstam -------------
------------- Copyright (C) 2008 - 2010 Anders Gidenstam -------------
------------- Copyright (C) 2008 Jon S. Berndt (jon@jsbsim.org) -------------
This program is free software; you can redistribute it and/or modify it under
@ -38,14 +38,14 @@ INCLUDES
#include "FGBuoyantForces.h"
#include "FGMassBalance.h"
#include "input_output/FGPropertyManager.h" // Need?
#include "input_output/FGPropertyManager.h"
#include <iostream>
using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGBuoyantForces.cpp,v 1.12 2010/05/07 18:59:55 andgi Exp $";
static const char *IdHdr = ID_BUOYANTFORCES;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -63,8 +63,6 @@ FGBuoyantForces::FGBuoyantForces(FGFDMExec* FDMExec) : FGModel(FDMExec)
gasCellJ.InitMatrix();
bind();
Debug(0);
}
@ -141,6 +139,10 @@ bool FGBuoyantForces::Load(Element *element)
FGModel::PostLoad(element);
if (!NoneDefined) {
bind();
}
return true;
}
@ -258,6 +260,19 @@ string FGBuoyantForces::GetBuoyancyValues(string delimeter)
void FGBuoyantForces::bind(void)
{
typedef double (FGBuoyantForces::*PMF)(int) const;
PropertyManager->Tie("moments/l-buoyancy-lbsft", this, eL,
(PMF)&FGBuoyantForces::GetMoments);
PropertyManager->Tie("moments/m-buoyancy-lbsft", this, eM,
(PMF)&FGBuoyantForces::GetMoments);
PropertyManager->Tie("moments/n-buoyancy-lbsft", this, eN,
(PMF)&FGBuoyantForces::GetMoments);
PropertyManager->Tie("forces/fbx-buoyancy-lbs", this, eX,
(PMF)&FGBuoyantForces::GetForces);
PropertyManager->Tie("forces/fby-buoyancy-lbs", this, eY,
(PMF)&FGBuoyantForces::GetForces);
PropertyManager->Tie("forces/fbz-buoyancy-lbs", this, eZ,
(PMF)&FGBuoyantForces::GetForces);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

14
src/FDM/JSBSim/models/FGBuoyantForces.h
View file

@ -4,7 +4,7 @@
Author: Anders Gidenstam, Jon S. Berndt
Date started: 01/21/08
------------- Copyright (C) 2008 Anders Gidenstam -------------
------------- Copyright (C) 2008 - 2010 Anders Gidenstam -------------
------------- Copyright (C) 2008 Jon S. Berndt (jon@jsbsim.org) -------------
This program is free software; you can redistribute it and/or modify it under
@ -51,7 +51,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_BUOYANTFORCES "$Id$"
#define ID_BUOYANTFORCES "$Id: FGBuoyantForces.h,v 1.11 2010/05/07 20:38:34 andgi Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -96,7 +96,7 @@ CLASS DOCUMENTATION
See FGGasCell for the full configuration file format for gas cells.
@author Anders Gidenstam, Jon S. Berndt
@version $Id$
@version $Id: FGBuoyantForces.h,v 1.11 2010/05/07 20:38:34 andgi Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -130,10 +130,18 @@ public:
@return a force vector. */
const FGColumnVector3& GetForces(void) const {return vTotalForces;}
/** Gets a component of the total Buoyant force vector.
@return a component of the force vector. */
double GetForces(int idx) const {return vTotalForces(idx);}
/** Gets the total Buoyancy moment vector.
@return a moment vector. */
const FGColumnVector3& GetMoments(void) const {return vTotalMoments;}
/** Gets a component of the total Buoyancy moment vector.
@return a component of the moment vector. */
double GetMoments(int idx) const {return vTotalMoments(idx);}
/** Gets the total gas mass. The gas mass is part of the aircraft's
inertia.
@return mass in slugs. */

2
src/FDM/JSBSim/models/FGExternalForce.cpp
View file

@ -60,7 +60,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGExternalForce.cpp,v 1.10 2009/10/24 22:59:30 jberndt Exp $";
static const char *IdHdr = ID_EXTERNALFORCE;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/models/FGExternalForce.h
View file

@ -51,7 +51,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_EXTERNALFORCE "$Id$"
#define ID_EXTERNALFORCE "$Id: FGExternalForce.h,v 1.8 2009/10/02 10:30:09 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

2
src/FDM/JSBSim/models/FGExternalReactions.cpp
View file

@ -53,7 +53,7 @@ DEFINITIONS
GLOBAL DATA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGExternalReactions.cpp,v 1.8 2009/11/12 13:08:11 jberndt Exp $";
static const char *IdHdr = ID_EXTERNALREACTIONS;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/models/FGExternalReactions.h
View file

@ -46,7 +46,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_EXTERNALREACTIONS "$Id$"
#define ID_EXTERNALREACTIONS "$Id: FGExternalReactions.h,v 1.9 2009/10/24 22:59:30 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

42
src/FDM/JSBSim/models/FGFCS.cpp
View file

@ -63,7 +63,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGFCS.cpp,v 1.68 2010/03/18 13:21:24 jberndt Exp $";
static const char *IdHdr = ID_FCS;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -533,19 +533,17 @@ bool FGFCS::Load(Element* el, SystemType systype)
Components=0;
string separator = "/";
// ToDo: The handling of name and file attributes could be improved, here,
// considering that a name can be in the external file, as well.
name = el->GetAttributeValue("name");
if (name.empty()) {
if (name.empty() || !el->GetAttributeValue("file").empty()) {
fname = el->GetAttributeValue("file");
if (systype == stSystem) {
file = FindSystemFullPathname(fname);
} else {
file = FDMExec->GetFullAircraftPath() + separator + fname + ".xml";
file = FDMExec->GetFullAircraftPath() + "/" + fname + ".xml";
}
if (fname.empty()) {
cerr << "FCS, Autopilot, or system does not appear to be defined inline nor in a file" << endl;
@ -692,49 +690,55 @@ double FGFCS::GetBrake(FGLGear::BrakeGroup bg)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
string FGFCS::FindSystemFullPathname(const string& system_filename)
string FGFCS::FindSystemFullPathname(const string& sysfilename)
{
string fullpath, localpath;
string system_filename = sysfilename;
string systemPath = FDMExec->GetSystemsPath();
string aircraftPath = FDMExec->GetFullAircraftPath();
ifstream system_file;
string separator = "/";
fullpath = systemPath + "/";
localpath = aircraftPath + "/Systems/";
fullpath = systemPath + separator;
localpath = aircraftPath + separator + "Systems" + separator;
if (system_filename.length() <=4 || system_filename.substr(system_filename.length()-4, 4) != ".xml") {
system_filename.append(".xml");
}
system_file.open(string(fullpath + system_filename + ".xml").c_str());
system_file.open(string(fullpath + system_filename).c_str());
if ( !system_file.is_open()) {
system_file.open(string(localpath + system_filename + ".xml").c_str());
system_file.open(string(localpath + system_filename).c_str());
if ( !system_file.is_open()) {
cerr << " Could not open system file: " << system_filename << " in path "
<< fullpath << " or " << localpath << endl;
return string("");
} else {
return string(localpath + system_filename + ".xml");
return string(localpath + system_filename);
}
}
return string(fullpath + system_filename + ".xml");
return string(fullpath + system_filename);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ifstream* FGFCS::FindSystemFile(const string& system_filename)
ifstream* FGFCS::FindSystemFile(const string& sysfilename)
{
string fullpath, localpath;
string system_filename = sysfilename;
string systemPath = FDMExec->GetSystemsPath();
string aircraftPath = FDMExec->GetFullAircraftPath();
ifstream* system_file = new ifstream();
string separator = "/";
fullpath = systemPath + "/";
localpath = aircraftPath + "/Systems/";
fullpath = systemPath + separator;
localpath = aircraftPath + separator + "Systems" + separator;
if (system_filename.substr(system_filename.length()-4, 4) != ".xml") {
system_filename.append(".xml");
}
system_file->open(string(fullpath + system_filename + ".xml").c_str());
system_file->open(string(fullpath + system_filename).c_str());
if ( !system_file->is_open()) {
system_file->open(string(localpath + system_filename + ".xml").c_str());
system_file->open(string(localpath + system_filename).c_str());
if ( !system_file->is_open()) {
cerr << " Could not open system file: " << system_filename << " in path "
<< fullpath << " or " << localpath << endl;

12
src/FDM/JSBSim/models/FGFCS.h
View file

@ -51,7 +51,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_FCS "$Id$"
#define ID_FCS "$Id: FGFCS.h,v 1.28 2010/01/18 13:12:25 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -168,7 +168,7 @@ CLASS DOCUMENTATION
@property gear/tailhook-pos-norm
@author Jon S. Berndt
@version $Revision$
@version $Revision: 1.28 $
@see FGActuator
@see FGDeadBand
@see FGFCSFunction
@ -407,12 +407,12 @@ public:
/** Sets the throttle command for the specified engine
@param engine engine ID number
@param cmd throttle command in percent (0 - 100)*/
@param cmd normalized throttle command (0.0 - 1.0)*/
void SetThrottleCmd(int engine, double cmd);
/** Sets the mixture command for the specified engine
@param engine engine ID number
@param cmd mixture command in percent (0 - 100)*/
@param cmd normalized mixture command (0.0 - 1.0)*/
void SetMixtureCmd(int engine, double cmd);
/** Set the gear extend/retract command, defaults to down
@ -462,12 +462,12 @@ public:
/** Sets the actual throttle setting for the specified engine
@param engine engine ID number
@param cmd throttle setting in percent (0 - 100)*/
@param cmd normalized throttle setting (0.0 - 1.0)*/
void SetThrottlePos(int engine, double cmd);
/** Sets the actual mixture setting for the specified engine
@param engine engine ID number
@param cmd mixture setting in percent (0 - 100)*/
@param cmd normalized mixture setting (0.0 - 1.0)*/
void SetMixturePos(int engine, double cmd);
/** Sets the steering position

2
src/FDM/JSBSim/models/FGGasCell.cpp
View file

@ -53,7 +53,7 @@ using std::max;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGGasCell.cpp,v 1.12 2009/10/24 22:59:30 jberndt Exp $";
static const char *IdHdr = ID_GASCELL;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/models/FGGasCell.h
View file

@ -50,7 +50,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_GASCELL "$Id$"
#define ID_GASCELL "$Id: FGGasCell.h,v 1.10 2009/10/24 22:59:30 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

2
src/FDM/JSBSim/models/FGGroundReactions.cpp
View file

@ -46,7 +46,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGGroundReactions.cpp,v 1.26 2009/11/12 13:08:11 jberndt Exp $";
static const char *IdHdr = ID_GROUNDREACTIONS;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/models/FGGroundReactions.h
View file

@ -45,7 +45,7 @@ INCLUDES
#include "math/FGColumnVector3.h"
#include "input_output/FGXMLElement.h"
#define ID_GROUNDREACTIONS "$Id$"
#define ID_GROUNDREACTIONS "$Id: FGGroundReactions.h,v 1.15 2009/10/02 10:30:09 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

33
src/FDM/JSBSim/models/FGInertial.cpp
View file

@ -38,7 +38,6 @@ INCLUDES
#include "FGInertial.h"
#include "FGFDMExec.h"
#include "FGPropagate.h"
#include "FGState.h"
#include "FGMassBalance.h"
#include <iostream>
@ -46,7 +45,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGInertial.cpp,v 1.18 2010/03/28 05:57:00 jberndt Exp $";
static const char *IdHdr = ID_INERTIAL;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -119,7 +118,7 @@ bool FGInertial::Run(void)
// Gravitation accel
double r = Propagate->GetRadius();
gAccel = GetGAccel(r);
earthPosAngle += State->Getdt()*RotationRate;
earthPosAngle += FDMExec->GetDeltaT()*RotationRate;
RunPostFunctions();
@ -133,6 +132,34 @@ double FGInertial::GetGAccel(double r) const
return GM/(r*r);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
// Calculate the WGS84 gravitation value in ECEF frame. Pass in the ECEF position
// via the position parameter. The J2Gravity value returned is in ECEF frame,
// and therefore may need to be expressed (transformed) in another frame,
// depending on how it is used. See Stevens and Lewis eqn. 1.4-16.
FGColumnVector3 FGInertial::GetGravityJ2(FGColumnVector3 position) const
{
FGColumnVector3 J2Gravity;
// Gravitation accel
double r = position.Magnitude();
double lat = Propagate->GetLatitude();
double sinLat = sin(lat);
double preCommon = 1.5*J2*(a/r)*(a/r);
double xy = 1.0 - 5.0*(sinLat*sinLat);
double z = 3.0 - 5.0*(sinLat*sinLat);
double GMOverr2 = GM/(r*r);
J2Gravity(1) = -GMOverr2 * ((1.0 + (preCommon * xy)) * position(eX)/r);
J2Gravity(2) = -GMOverr2 * ((1.0 + (preCommon * xy)) * position(eY)/r);
J2Gravity(3) = -GMOverr2 * ((1.0 + (preCommon * z)) * position(eZ)/r);
return J2Gravity;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGInertial::bind(void)

5
src/FDM/JSBSim/models/FGInertial.h
View file

@ -47,7 +47,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_INERTIAL "$Id$"
#define ID_INERTIAL "$Id: FGInertial.h,v 1.15 2010/01/27 04:01:09 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -82,10 +82,13 @@ public:
double GetEarthPositionAngle(void) const { return earthPosAngle; }
double GetEarthPositionAngleDeg(void) const { return earthPosAngle*radtodeg;}
double GetGAccel(double r) const;
FGColumnVector3 GetGravityJ2(FGColumnVector3 position) const;
double GetRefRadius(void) const {return RadiusReference;}
double GetSemimajor(void) const {return a;}
double GetSemiminor(void) const {return b;}
void SetEarthPositionAngle(double epa) {earthPosAngle = epa;}
private:
double gAccel;
double gAccelReference;

6
src/FDM/JSBSim/models/FGInput.cpp
View file

@ -39,7 +39,7 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGInput.h"
#include "FGState.h"
#include "FGAircraft.h"
#include "FGFDMExec.h"
#include "input_output/FGfdmSocket.h"
@ -53,7 +53,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGInput.cpp,v 1.19 2010/02/25 05:21:36 jberndt Exp $";
static const char *IdHdr = ID_INPUT;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -192,7 +192,7 @@ bool FGInput::Run(void)
info << "JSBSim version: " << JSBSim_version << endl;
info << "Config File version: " << needed_cfg_version << endl;
info << "Aircraft simulated: " << Aircraft->GetAircraftName() << endl;
info << "Simulation time: " << setw(8) << setprecision(3) << State->Getsim_time() << endl;
info << "Simulation time: " << setw(8) << setprecision(3) << FDMExec->GetSimTime() << endl;
socket->Reply(info.str());
} else if (command == "help") { // HELP

2
src/FDM/JSBSim/models/FGInput.h
View file

@ -46,7 +46,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_INPUT "$Id$"
#define ID_INPUT "$Id: FGInput.h,v 1.8 2009/10/24 22:59:30 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

53
src/FDM/JSBSim/models/FGLGear.cpp
View file

@ -41,7 +41,6 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGLGear.h"
#include "FGState.h"
#include "FGGroundReactions.h"
#include "FGFCS.h"
#include "FGAuxiliary.h"
@ -62,7 +61,7 @@ DEFINITIONS
GLOBAL DATA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGLGear.cpp,v 1.74 2010/05/18 10:54:14 jberndt Exp $";
static const char *IdHdr = ID_LGEAR;
// Body To Structural (body frame is rotated 180 deg about Y and lengths are given in
@ -76,7 +75,8 @@ CLASS IMPLEMENTATION
FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) :
FGForce(fdmex),
GearNumber(number),
SteerAngle(0.0)
SteerAngle(0.0),
Castered(false)
{
Element *force_table=0;
Element *dampCoeff=0;
@ -202,7 +202,7 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) :
if (sSteerType == "STEERABLE") eSteerType = stSteer;
else if (sSteerType == "FIXED" ) eSteerType = stFixed;
else if (sSteerType == "CASTERED" ) eSteerType = stCaster;
else if (sSteerType == "CASTERED" ) {eSteerType = stCaster; Castered = true;}
else if (sSteerType.empty() ) {eSteerType = stFixed;
sSteerType = "FIXED (defaulted)";}
else {
@ -223,15 +223,14 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) :
}
}
State = fdmex->GetState();
Aircraft = fdmex->GetAircraft();
Propagate = fdmex->GetPropagate();
Auxiliary = fdmex->GetAuxiliary();
FCS = fdmex->GetFCS();
MassBalance = fdmex->GetMassBalance();
LongForceLagFilterCoeff = 1/State->Getdt(); // default longitudinal force filter coefficient
LatForceLagFilterCoeff = 1/State->Getdt(); // default lateral force filter coefficient
LongForceLagFilterCoeff = 1/fdmex->GetDeltaT(); // default longitudinal force filter coefficient
LatForceLagFilterCoeff = 1/fdmex->GetDeltaT(); // default lateral force filter coefficient
Element* force_lag_filter_elem = el->FindElement("force_lag_filter");
if (force_lag_filter_elem) {
@ -243,17 +242,17 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) :
}
}
LongForceFilter = Filter(LongForceLagFilterCoeff, State->Getdt());
LatForceFilter = Filter(LatForceLagFilterCoeff, State->Getdt());
LongForceFilter = Filter(LongForceLagFilterCoeff, fdmex->GetDeltaT());
LatForceFilter = Filter(LatForceLagFilterCoeff, fdmex->GetDeltaT());
WheelSlipLagFilterCoeff = 1/State->Getdt();
WheelSlipLagFilterCoeff = 1/fdmex->GetDeltaT();
Element *wheel_slip_angle_lag_elem = el->FindElement("wheel_slip_filter");
if (wheel_slip_angle_lag_elem) {
WheelSlipLagFilterCoeff = wheel_slip_angle_lag_elem->GetDataAsNumber();
}
WheelSlipFilter = Filter(WheelSlipLagFilterCoeff, State->Getdt());
WheelSlipFilter = Filter(WheelSlipLagFilterCoeff, fdmex->GetDeltaT());
GearUp = false;
GearDown = true;
@ -307,8 +306,8 @@ FGLGear::~FGLGear()
FGColumnVector3& FGLGear::GetBodyForces(void)
{
double t = fdmex->GetState()->Getsim_time();
dT = State->Getdt()*fdmex->GetGroundReactions()->GetRate();
double t = fdmex->GetSimTime();
dT = fdmex->GetDeltaT()*fdmex->GetGroundReactions()->GetRate();
vFn.InitMatrix();
@ -324,7 +323,7 @@ FGColumnVector3& FGLGear::GetBodyForces(void)
// Compute the height of the theoretical location of the wheel (if strut is not compressed) with
// respect to the ground level
double height = fdmex->GetGroundCallback()->GetAGLevel(t, gearLoc, contact, normal, cvel);
vGroundNormal = -1. * Propagate->GetTec2b() * normal;
vGroundNormal = Propagate->GetTec2b() * normal;
// The height returned above is the AGL and is expressed in the Z direction of the local
// coordinate frame. We now need to transform this height in actual compression of the strut (BOGEY)
@ -335,7 +334,7 @@ FGColumnVector3& FGLGear::GetBodyForces(void)
compressLength = verticalZProj > 0.0 ? -height / verticalZProj : 0.0;
break;
case ctSTRUCTURE:
verticalZProj = (Propagate->GetTec2l()*normal)(eZ);
verticalZProj = -(Propagate->GetTec2l()*normal)(eZ);
compressLength = fabs(verticalZProj) > 0.0 ? -height / verticalZProj : 0.0;
break;
}
@ -516,7 +515,13 @@ void FGLGear::ComputeSteeringAngle(void)
SteerAngle = 0.0;
break;
case stCaster:
SteerAngle = atan2(vWhlVelVec(eY), fabs(vWhlVelVec(eX)));
if (!Castered)
SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber);
else {
// Check that the speed is non-null otherwise use the current angle
if (vWhlVelVec.Magnitude(eX,eY) > 1E-3)
SteerAngle = atan2(vWhlVelVec(eY), fabs(vWhlVelVec(eX)));
}
break;
default:
cerr << "Improper steering type membership detected for this gear." << endl;
@ -571,7 +576,7 @@ void FGLGear::InitializeReporting(void)
void FGLGear::ReportTakeoffOrLanding(void)
{
double deltaT = State->Getdt()*fdmex->GetGroundReactions()->GetRate();
double deltaT = fdmex->GetDeltaT()*fdmex->GetGroundReactions()->GetRate();
if (FirstContact)
LandingDistanceTraveled += Auxiliary->GetVground()*deltaT;
@ -608,10 +613,10 @@ void FGLGear::CrashDetect(void)
if ( (compressLength > 500.0 ||
vFn.Magnitude() > 100000000.0 ||
GetMoments().Magnitude() > 5000000000.0 ||
SinkRate > 1.4666*30 ) && !State->IntegrationSuspended())
SinkRate > 1.4666*30 ) && !fdmex->IntegrationSuspended())
{
PutMessage("Crash Detected: Simulation FREEZE.");
State->SuspendIntegration();
fdmex->SuspendIntegration();
}
}
@ -760,8 +765,10 @@ void FGLGear::bind(void)
fdmex->GetPropertyManager()->Tie( property_name.c_str(), &staticFCoeff );
if (eSteerType == stCaster) {
property_name = base_property_name + "/steering-angle-rad";
fdmex->GetPropertyManager()->Tie( property_name.c_str(), &SteerAngle );
property_name = base_property_name + "/steering-angle-deg";
fdmex->GetPropertyManager()->Tie( property_name.c_str(), this, &FGLGear::GetSteerAngleDeg );
property_name = base_property_name + "/castered";
fdmex->GetPropertyManager()->Tie( property_name.c_str(), &Castered);
}
}
@ -780,7 +787,7 @@ void FGLGear::Report(ReportType repType)
switch(repType) {
case erLand:
cout << endl << "Touchdown report for " << name << " (WOW at time: "
<< fdmex->GetState()->Getsim_time() << " seconds)" << endl;
<< fdmex->GetSimTime() << " seconds)" << endl;
cout << " Sink rate at contact: " << SinkRate << " fps, "
<< SinkRate*0.3048 << " mps" << endl;
cout << " Contact ground speed: " << GroundSpeed*.5925 << " knots, "
@ -795,7 +802,7 @@ void FGLGear::Report(ReportType repType)
break;
case erTakeoff:
cout << endl << "Takeoff report for " << name << " (Liftoff at time: "
<< fdmex->GetState()->Getsim_time() << " seconds)" << endl;
<< fdmex->GetSimTime() << " seconds)" << endl;
cout << " Distance traveled: " << TakeoffDistanceTraveled
<< " ft, " << TakeoffDistanceTraveled*0.3048 << " meters" << endl;
cout << " Distance traveled (over 50'): " << TakeoffDistanceTraveled50ft

7
src/FDM/JSBSim/models/FGLGear.h
View file

@ -46,7 +46,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_LGEAR "$Id$"
#define ID_LGEAR "$Id: FGLGear.h,v 1.38 2010/03/23 22:44:36 andgi Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -84,7 +84,6 @@ CLASS DOCUMENTATION
<h3>Operational Properties</h3>
<ol>
<li>Name</li>
<li>Steerability attribute {one of STEERABLE | FIXED | CASTERED}</li>
<li>Brake Group Membership {one of LEFT | CENTER | RIGHT | NOSE | TAIL | NONE}</li>
<li>Max Steer Angle, in degrees</li>
</ol></p>
@ -190,7 +189,7 @@ CLASS DOCUMENTATION
</contact>
@endcode
@author Jon S. Berndt
@version $Id$
@version $Id: FGLGear.h,v 1.38 2010/03/23 22:44:36 andgi Exp $
@see Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
NASA-Ames", NASA CR-2497, January 1975
@see Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
@ -289,6 +288,7 @@ public:
double GetWheelVel(int axis) const { return vWhlVelVec(axis);}
bool IsBogey(void) const { return (eContactType == ctBOGEY);}
double GetGearUnitPos(void);
double GetSteerAngleDeg(void) const { return radtodeg*SteerAngle; }
void bind(void);
@ -337,6 +337,7 @@ private:
bool isRetractable;
bool GearUp, GearDown;
bool Servicable;
bool Castered;
std::string name;
std::string sSteerType;
std::string sBrakeGroup;

110
src/FDM/JSBSim/models/FGMassBalance.cpp
View file

@ -40,16 +40,18 @@ INCLUDES
#include "FGMassBalance.h"
#include "FGPropulsion.h"
#include "propulsion/FGTank.h"
#include "FGBuoyantForces.h"
#include "input_output/FGPropertyManager.h"
#include <iostream>
#include <iomanip>
#include <cstdlib>
using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGMassBalance.cpp,v 1.31 2010/02/19 00:30:00 jberndt Exp $";
static const char *IdHdr = ID_MASSBALANCE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -124,7 +126,9 @@ bool FGMassBalance::Load(Element* el)
SetAircraftBaseInertias(FGMatrix33( bixx, -bixy, bixz,
-bixy, biyy, -biyz,
bixz, -biyz, bizz ));
EmptyWeight = el->FindElementValueAsNumberConvertTo("emptywt", "LBS");
if (el->FindElement("emptywt")) {
EmptyWeight = el->FindElementValueAsNumberConvertTo("emptywt", "LBS");
}
element = el->FindElement("location");
while (element) {
@ -241,6 +245,7 @@ bool FGMassBalance::Run(void)
void FGMassBalance::AddPointMass(Element* el)
{
double radius=0, length=0;
Element* loc_element = el->FindElement("location");
string pointmass_name = el->GetAttributeValue("name");
if (!loc_element) {
@ -251,8 +256,36 @@ void FGMassBalance::AddPointMass(Element* el)
double w = el->FindElementValueAsNumberConvertTo("weight", "LBS");
FGColumnVector3 vXYZ = loc_element->FindElementTripletConvertTo("IN");
PointMasses.push_back(new PointMass(w, vXYZ));
PointMasses.back()->bind(PropertyManager, PointMasses.size()-1);
PointMass *pm = new PointMass(w, vXYZ);
pm->SetName(pointmass_name);
Element* form_element = el->FindElement("form");
if (form_element) {
string shape = form_element->GetAttributeValue("shape");
Element* radius_element = form_element->FindElement("radius");
Element* length_element = form_element->FindElement("length");
if (radius_element) radius = form_element->FindElementValueAsNumberConvertTo("radius", "FT");
if (length_element) length = form_element->FindElementValueAsNumberConvertTo("length", "FT");
if (shape == "tube") {
pm->SetPointMassShapeType(PointMass::esTube);
pm->SetRadius(radius);
pm->SetLength(length);
pm->CalculateShapeInertia();
} else if (shape == "cylinder") {
pm->SetPointMassShapeType(PointMass::esCylinder);
pm->SetRadius(radius);
pm->SetLength(length);
pm->CalculateShapeInertia();
} else if (shape == "sphere") {
pm->SetPointMassShapeType(PointMass::esSphere);
pm->SetRadius(radius);
pm->CalculateShapeInertia();
} else {
}
}
pm->bind(PropertyManager, PointMasses.size());
PointMasses.push_back(pm);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -290,8 +323,10 @@ FGMatrix33& FGMassBalance::CalculatePMInertias(void)
pmJ = FGMatrix33();
for (unsigned int i=0; i<size; i++)
for (unsigned int i=0; i<size; i++) {
pmJ += GetPointmassInertia( lbtoslug * PointMasses[i]->Weight, PointMasses[i]->Location );
pmJ += PointMasses[i]->GetPointMassInertia();
}
return pmJ;
}
@ -339,7 +374,7 @@ void FGMassBalance::bind(void)
PropertyManager->Tie("inertia/weight-lbs", this,
&FGMassBalance::GetWeight);
PropertyManager->Tie("inertia/empty-weight-lbs", this,
&FGMassBalance::GetWeight, &FGMassBalance::SetEmptyWeight);
&FGMassBalance::GetEmptyWeight);
PropertyManager->Tie("inertia/cg-x-in", this,1,
(PMF)&FGMassBalance::GetXYZcg);
PropertyManager->Tie("inertia/cg-y-in", this,2,
@ -349,7 +384,9 @@ void FGMassBalance::bind(void)
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
// This function binds properties for each pointmass object created.
//
void FGMassBalance::PointMass::bind(FGPropertyManager* PropertyManager, int num) {
string tmp = CreateIndexedPropertyName("inertia/pointmass-weight-lbs", num);
PropertyManager->Tie( tmp.c_str(), this, &PointMass::GetPointMassWeight,
@ -366,6 +403,63 @@ void FGMassBalance::PointMass::bind(FGPropertyManager* PropertyManager, int num)
&PointMass::SetPointMassLocation);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGMassBalance::GetMassPropertiesReport(void) const
{
cout << endl << fgblue << highint
<< " Mass Properties Report (English units: lbf, in, slug-ft^2)"
<< reset << endl;
cout << " " << underon << " Weight CG-X CG-Y"
<< " CG-Z Ixx Iyy Izz" << underoff << endl;
cout.precision(1);
cout << highint << setw(34) << left << " Base Vehicle " << normint
<< right << setw(10) << EmptyWeight << setw(8) << vbaseXYZcg(eX) << setw(8)
<< vbaseXYZcg(eY) << setw(8) << vbaseXYZcg(eZ) << setw(12) << baseJ(1,1)
<< setw(12) << baseJ(2,2) << setw(12) << baseJ(3,3) << endl;
for (unsigned int i=0;i<PointMasses.size();i++) {
PointMass* pm = PointMasses[i];
double pmweight = pm->GetPointMassWeight();
cout << highint << left << setw(4) << i << setw(30) << pm->GetName() << normint
<< right << setw(10) << pmweight << setw(8) << pm->GetLocation()(eX)
<< setw(8) << pm->GetLocation()(eY) << setw(8) << pm->GetLocation()(eZ)
<< setw(12) << pm->GetPointMassMoI(1,1) << setw(12) << pm->GetPointMassMoI(2,2)
<< setw(12) << pm->GetPointMassMoI(3,3) << endl;
}
for (unsigned int i=0;i<Propulsion->GetNumTanks() ;i++) {
FGTank* tank = Propulsion->GetTank(i);
string tankname="";
if (tank->GetType() == FGTank::ttFUEL && tank->GetGrainType() != FGTank::gtUNKNOWN) {
tankname = "Solid Fuel";
} else if (tank->GetType() == FGTank::ttFUEL) {
tankname = "Fuel";
} else if (tank->GetType() == FGTank::ttOXIDIZER) {
tankname = "Oxidizer";
} else {
tankname = "(Unknown tank type)";
}
cout << highint << left << setw(4) << i << setw(30) << tankname << normint
<< right << setw(10) << tank->GetContents() << setw(8) << tank->GetXYZ(eX)
<< setw(8) << tank->GetXYZ(eY) << setw(8) << tank->GetXYZ(eZ)
<< setw(12) << "*" << setw(12) << "*"
<< setw(12) << "*" << endl;
}
cout << underon << setw(104) << " " << underoff << endl;
cout << highint << left << setw(30) << " Total: " << right << setw(14) << Weight
<< setw(8) << vXYZcg(eX)
<< setw(8) << vXYZcg(eY)
<< setw(8) << vXYZcg(eZ)
<< setw(12) << mJ(1,1)
<< setw(12) << mJ(2,2)
<< setw(12) << mJ(3,3)
<< normint << endl;
cout.setf(ios_base::fixed);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// The bitmasked value choices are as follows:
// unset: In this case (the default) JSBSim would only print
@ -398,7 +492,7 @@ void FGMassBalance::Debug(int from)
cout << " baseIxy: " << baseJ(1,2) << " slug-ft2" << endl;
cout << " baseIxz: " << baseJ(1,3) << " slug-ft2" << endl;
cout << " baseIyz: " << baseJ(2,3) << " slug-ft2" << endl;
cout << " EmptyWeight: " << EmptyWeight << " lbm" << endl;
cout << " Empty Weight: " << EmptyWeight << " lbm" << endl;
cout << " CG (x, y, z): " << vbaseXYZcg << endl;
// ToDo: Need to add point mass outputs here
for (unsigned int i=0; i<PointMasses.size(); i++) {

55
src/FDM/JSBSim/models/FGMassBalance.h
View file

@ -43,17 +43,20 @@ INCLUDES
#include "math/FGMatrix33.h"
#include "input_output/FGXMLElement.h"
#include <vector>
#include <string>
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_MASSBALANCE "$Id$"
#define ID_MASSBALANCE "$Id: FGMassBalance.h,v 1.20 2010/02/04 13:09:26 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONSS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
using std::string;
namespace JSBSim {
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -109,9 +112,10 @@ public:
double GetMass(void) const {return Mass;}
double GetWeight(void) const {return Weight;}
FGColumnVector3& GetXYZcg(void) {return vXYZcg;}
double GetEmptyWeight(void) const {return EmptyWeight;}
const FGColumnVector3& GetXYZcg(void) const {return vXYZcg;}
double GetXYZcg(int axis) const {return vXYZcg(axis);}
FGColumnVector3& GetDeltaXYZcg(void) {return vDeltaXYZcg;}
const FGColumnVector3& GetDeltaXYZcg(void) const {return vDeltaXYZcg;}
double GetDeltaXYZcg(int axis) const {return vDeltaXYZcg(axis);}
/** Computes the inertia contribution of a pointmass.
@ -158,6 +162,7 @@ public:
FGMatrix33& GetJ(void) {return mJ;}
FGMatrix33& GetJinv(void) {return mJinv;}
void SetAircraftBaseInertias(FGMatrix33 BaseJ) {baseJ = BaseJ;}
void GetMassPropertiesReport(void) const;
private:
double Weight;
@ -177,17 +182,57 @@ private:
FGColumnVector3 PointMassCG;
FGMatrix33& CalculatePMInertias(void);
/** The PointMass structure encapsulates a point mass object, moments of inertia
mass, location, etc. */
struct PointMass {
PointMass(double w, FGColumnVector3& vXYZ) {
Weight = w;
Location = vXYZ;
mPMInertia.InitMatrix();
Radius = 0.0;
Length = 0.0;
}
void CalculateShapeInertia(void) {
switch(eShapeType) {
case esTube:
mPMInertia(1,1) = (Weight/(32.16))*Radius*Radius; // mr^2
mPMInertia(2,2) = (Weight/(32.16*12))*(6*Radius*Radius + Length*Length);
mPMInertia(3,3) = mPMInertia(2,2);
break;
case esCylinder:
mPMInertia(1,1) = (Weight/(32.16*2))*Radius*Radius; // 0.5*mr^2
mPMInertia(2,2) = (Weight/(32.16*12))*(3*Radius*Radius + Length*Length);
mPMInertia(3,3) = mPMInertia(2,2);
break;
default:
break;
}
}
enum esShape {esUnspecified, esTube, esCylinder, esSphere} eShapeType;
FGColumnVector3 Location;
double Weight;
double Weight; /// Weight in pounds.
double Radius; /// Radius in feet.
double Length; /// Length in feet.
string Name;
FGMatrix33 mPMInertia;
double GetPointMassLocation(int axis) const {return Location(axis);}
double GetPointMassWeight(void) const {return Weight;}
esShape GetShapeType(void) {return eShapeType;}
FGColumnVector3 GetLocation(void) {return Location;}
FGMatrix33 GetPointMassInertia(void) {return mPMInertia;}
string GetName(void) {return Name;}
void SetPointMassLocation(int axis, double value) {Location(axis) = value;}
void SetPointMassWeight(double wt) {Weight = wt;}
double GetPointMassWeight(void) const {return Weight;}
void SetPointMassShapeType(esShape st) {eShapeType = st;}
void SetRadius(double r) {Radius = r;}
void SetLength(double l) {Length = l;}
void SetName(string name) {Name = name;}
double GetPointMassMoI(int r, int c) {return mPMInertia(r,c);}
void bind(FGPropertyManager* PropertyManager, int num);
};

8
src/FDM/JSBSim/models/FGModel.cpp
View file

@ -39,7 +39,6 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGModel.h"
#include "FGState.h"
#include "FGFDMExec.h"
#include "FGAtmosphere.h"
#include "FGFCS.h"
@ -58,7 +57,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGModel.cpp,v 1.14 2010/02/25 05:21:36 jberndt Exp $";
static const char *IdHdr = ID_MODEL;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -74,7 +73,6 @@ FGModel::FGModel(FGFDMExec* fdmex)
FDMExec = fdmex;
NextModel = 0L;
State = 0;
Atmosphere = 0;
FCS = 0;
Propulsion = 0;
@ -115,7 +113,6 @@ FGModel::~FGModel()
bool FGModel::InitModel(void)
{
State = FDMExec->GetState();
Atmosphere = FDMExec->GetAtmosphere();
FCS = FDMExec->GetFCS();
Propulsion = FDMExec->GetPropulsion();
@ -129,8 +126,7 @@ bool FGModel::InitModel(void)
Propagate = FDMExec->GetPropagate();
Auxiliary = FDMExec->GetAuxiliary();
if (!State ||
!Atmosphere ||
if (!Atmosphere ||
!FCS ||
!Propulsion ||
!MassBalance ||

2
src/FDM/JSBSim/models/FGModel.h
View file

@ -48,7 +48,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_MODEL "$Id$"
#define ID_MODEL "$Id: FGModel.h,v 1.14 2009/11/12 13:08:11 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS

34
src/FDM/JSBSim/models/FGOutput.cpp
View file

@ -40,7 +40,6 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGOutput.h"
#include "FGState.h"
#include "FGFDMExec.h"
#include "FGAtmosphere.h"
#include "FGFCS.h"
@ -53,6 +52,7 @@ INCLUDES
#include "FGPropagate.h"
#include "FGAuxiliary.h"
#include "FGInertial.h"
#include "FGPropulsion.h"
#include "models/propulsion/FGEngine.h"
#include "models/propulsion/FGTank.h"
#include "models/propulsion/FGPiston.h"
@ -77,7 +77,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGOutput.cpp,v 1.48 2010/04/12 12:25:19 jberndt Exp $";
static const char *IdHdr = ID_OUTPUT;
// (stolen from FGFS native_fdm.cxx)
@ -187,7 +187,7 @@ bool FGOutput::Run(void)
{
if (FGModel::Run()) return true;
if (enabled && !State->IntegrationSuspended()&& !FDMExec->Holding()) {
if (enabled && !FDMExec->IntegrationSuspended()&& !FDMExec->Holding()) {
RunPreFunctions();
if (Type == otSocket) {
SocketOutput();
@ -267,7 +267,8 @@ void FGOutput::DelimitedOutput(const string& fname)
if (SubSystems & ssRates) {
outstream << delimeter;
outstream << "P (deg/s)" + delimeter + "Q (deg/s)" + delimeter + "R (deg/s)" + delimeter;
outstream << "P dot (deg/s^2)" + delimeter + "Q dot (deg/s^2)" + delimeter + "R dot (deg/s^2)";
outstream << "P dot (deg/s^2)" + delimeter + "Q dot (deg/s^2)" + delimeter + "R dot (deg/s^2)" + delimeter;
outstream << "P_{inertial} (deg/s)" + delimeter + "Q_{inertial} (deg/s)" + delimeter + "R_{inertial} (deg/s)";
}
if (SubSystems & ssVelocities) {
outstream << delimeter;
@ -277,6 +278,7 @@ void FGOutput::DelimitedOutput(const string& fname)
outstream << "V_{Inertial} (ft/s)" + delimeter;
outstream << "UBody" + delimeter + "VBody" + delimeter + "WBody" + delimeter;
outstream << "Aero V_{X Body} (ft/s)" + delimeter + "Aero V_{Y Body} (ft/s)" + delimeter + "Aero V_{Z Body} (ft/s)" + delimeter;
outstream << "V_{X_{inertial}} (ft/s)" + delimeter + "V_{Y_{inertial}} (ft/s)" + delimeter + "V_{Z_{inertial}} (ft/s)" + delimeter;
outstream << "V_{North} (ft/s)" + delimeter + "V_{East} (ft/s)" + delimeter + "V_{Down} (ft/s)";
}
if (SubSystems & ssForces) {
@ -334,8 +336,9 @@ void FGOutput::DelimitedOutput(const string& fname)
outstream << "Beta (deg)" + delimeter;
outstream << "Latitude (deg)" + delimeter;
outstream << "Longitude (deg)" + delimeter;
outstream << "ECEF X (ft)" + delimeter + "ECEF Y (ft)" + delimeter + "ECEF Z (ft)" + delimeter;
outstream << "EPA (deg)" + delimeter;
outstream << "X_{ECI} (ft)" + delimeter + "Y_{ECI} (ft)" + delimeter + "Z_{ECI} (ft)" + delimeter;
outstream << "X_{ECEF} (ft)" + delimeter + "Y_{ECEF} (ft)" + delimeter + "Z_{ECEF} (ft)" + delimeter;
outstream << "Earth Position Angle (deg)" + delimeter;
outstream << "Distance AGL (ft)" + delimeter;
outstream << "Terrain Elevation (ft)";
}
@ -365,7 +368,7 @@ void FGOutput::DelimitedOutput(const string& fname)
dFirstPass = false;
}
outstream << State->Getsim_time();
outstream << FDMExec->GetSimTime();
if (SubSystems & ssSimulation) {
}
if (SubSystems & ssAerosurfaces) {
@ -383,7 +386,8 @@ void FGOutput::DelimitedOutput(const string& fname)
if (SubSystems & ssRates) {
outstream << delimeter;
outstream << (radtodeg*Propagate->GetPQR()).Dump(delimeter) << delimeter;
outstream << (radtodeg*Propagate->GetPQRdot()).Dump(delimeter);
outstream << (radtodeg*Propagate->GetPQRdot()).Dump(delimeter) << delimeter;
outstream << (radtodeg*Propagate->GetPQRi()).Dump(delimeter);
}
if (SubSystems & ssVelocities) {
outstream << delimeter;
@ -393,6 +397,7 @@ void FGOutput::DelimitedOutput(const string& fname)
outstream << Propagate->GetInertialVelocityMagnitude() << delimeter;
outstream << setprecision(12) << Propagate->GetUVW().Dump(delimeter) << delimeter;
outstream << Auxiliary->GetAeroUVW().Dump(delimeter) << delimeter;
outstream << Propagate->GetInertialVelocity().Dump(delimeter) << delimeter;
outstream << Propagate->GetVel().Dump(delimeter);
outstream.precision(10);
}
@ -445,6 +450,7 @@ void FGOutput::DelimitedOutput(const string& fname)
outstream << Propagate->GetLocation().GetLatitudeDeg() << delimeter;
outstream << Propagate->GetLocation().GetLongitudeDeg() << delimeter;
outstream.precision(18);
outstream << ((FGColumnVector3)Propagate->GetInertialPosition()).Dump(delimeter) << delimeter;
outstream << ((FGColumnVector3)Propagate->GetLocation()).Dump(delimeter) << delimeter;
outstream.precision(14);
outstream << Inertial->GetEarthPositionAngleDeg() << delimeter;
@ -817,7 +823,7 @@ void FGOutput::SocketOutput(void)
}
socket->Clear();
socket->Append(State->Getsim_time());
socket->Append(FDMExec->GetSimTime());
if (SubSystems & ssAerosurfaces) {
socket->Append(FCS->GetDaCmd());
@ -952,13 +958,15 @@ bool FGOutput::Load(Element* element)
output_file_name = DirectivesFile; // one found in the config file.
document = LoadXMLDocument(output_file_name);
} else if (!element->GetAttributeValue("file").empty()) {
output_file_name = element->GetAttributeValue("file");
output_file_name = FDMExec->GetRootDir() + element->GetAttributeValue("file");
document = LoadXMLDocument(output_file_name);
} else {
document = element;
}
name = document->GetAttributeValue("name");
if (!document) return false;
name = FDMExec->GetRootDir() + document->GetAttributeValue("name");
type = document->GetAttributeValue("type");
SetType(type);
if (!document->GetAttributeValue("port").empty() && type == string("SOCKET")) {
@ -1035,7 +1043,7 @@ void FGOutput::SetRate(int rtHz)
{
rtHz = rtHz>1000?1000:(rtHz<0?0:rtHz);
if (rtHz > 0) {
rate = (int)(0.5 + 1.0/(State->Getdt()*rtHz));
rate = (int)(0.5 + 1.0/(FDMExec->GetDeltaT()*rtHz));
Enable();
} else {
rate = 1;
@ -1085,7 +1093,7 @@ void FGOutput::Debug(int from)
}
switch (Type) {
case otCSV:
cout << scratch << " in CSV format output at rate " << 1/(State->Getdt()*rate) << " Hz" << endl;
cout << scratch << " in CSV format output at rate " << 1/(FDMExec->GetDeltaT()*rate) << " Hz" << endl;
break;
case otNone:
default:

4
src/FDM/JSBSim/models/FGOutput.h
View file

@ -50,7 +50,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_OUTPUT "$Id$"
#define ID_OUTPUT "$Id: FGOutput.h,v 1.17 2009/10/24 22:59:30 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -123,7 +123,7 @@ CLASS DOCUMENTATION
propulsion ON|OFF
</pre>
NOTE that Time is always output with the data.
@version $Id$
@version $Id: FGOutput.h,v 1.17 2009/10/24 22:59:30 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

547
src/FDM/JSBSim/models/FGPropagate.cpp
View file

@ -56,10 +56,11 @@ INCLUDES
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <iomanip>
#include "FGPropagate.h"
#include "FGGroundReactions.h"
#include "FGFDMExec.h"
#include "FGState.h"
#include "FGAircraft.h"
#include "FGMassBalance.h"
#include "FGInertial.h"
@ -69,7 +70,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.55 2010/07/09 04:11:45 jberndt Exp $";
static const char *IdHdr = ID_PROPAGATE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -80,30 +81,23 @@ FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
{
Debug(0);
Name = "FGPropagate";
gravType = gtWGS84;
last2_vPQRdot.InitMatrix();
last_vPQRdot.InitMatrix();
vPQRdot.InitMatrix();
last2_vQtrndot = FGQuaternion(0,0,0);
last_vQtrndot = FGQuaternion(0,0,0);
vQtrndot = FGQuaternion(0,0,0);
last2_vUVWdot.InitMatrix();
last_vUVWdot.InitMatrix();
vUVWdot.InitMatrix();
last2_vLocationDot.InitMatrix();
last_vLocationDot.InitMatrix();
vLocationDot.InitMatrix();
vOmegaLocal.InitMatrix();
vInertialVelocity.InitMatrix();
integrator_rotational_rate = eAdamsBashforth2;
integrator_translational_rate = eTrapezoidal;
integrator_rotational_position = eAdamsBashforth2;
integrator_translational_position = eTrapezoidal;
VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
bind();
Debug(0);
}
@ -126,25 +120,17 @@ bool FGPropagate::InitModel(void)
VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
vOmega = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
vOmegaEarth = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
last2_vPQRdot.InitMatrix();
last_vPQRdot.InitMatrix();
vPQRdot.InitMatrix();
last2_vQtrndot = FGQuaternion(0,0,0);
last_vQtrndot = FGQuaternion(0,0,0);
vQtrndot = FGQuaternion(0,0,0);
last2_vUVWdot.InitMatrix();
last_vUVWdot.InitMatrix();
vUVWdot.InitMatrix();
vInertialVelocity.InitMatrix();
last2_vLocationDot.InitMatrix();
last_vLocationDot.InitMatrix();
vLocationDot.InitMatrix();
vOmegaLocal.InitMatrix();
VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
integrator_rotational_rate = eAdamsBashforth2;
integrator_translational_rate = eTrapezoidal;
@ -161,51 +147,92 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
// Set the position lat/lon/radius
VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
FGIC->GetLatitudeRadIC(),
FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
VehicleRadius = GetRadius();
radInv = 1.0/VehicleRadius;
// Set the Orientation from the euler angles
VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
FGIC->GetThetaRadIC(),
FGIC->GetPsiRadIC() );
// Initialize the State Vector elements and the transformation matrices
// Set the position lat/lon/radius
VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
FGIC->GetLatitudeRadIC(),
FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle());
Ti2ec = GetTi2ec(); // ECI to ECEF transform
Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
Tl2ec = GetTl2ec(); // local to ECEF transform
Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
Ti2l = GetTi2l();
Tl2i = Ti2l.Transposed();
// Set the orientation from the euler angles (is normalized within the
// constructor). The Euler angles represent the orientation of the body
// frame relative to the local frame.
VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
FGIC->GetThetaRadIC(),
FGIC->GetPsiRadIC() );
VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
Ti2b = GetTi2b(); // ECI to body frame transform
Tb2i = Ti2b.Transposed(); // body to ECI frame transform
Tl2b = VState.qAttitudeLocal; // local to body frame transform
Tb2l = Tl2b.Transposed(); // body to local frame transform
Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
// Set the velocities in the instantaneus body frame
VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
FGIC->GetVBodyFpsIC(),
FGIC->GetWBodyFpsIC() );
FGIC->GetVBodyFpsIC(),
FGIC->GetWBodyFpsIC() );
// Set the angular velocities in the instantaneus body frame.
VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
FGIC->GetQRadpsIC(),
FGIC->GetRRadpsIC() );
VState.vInertialPosition = Tec2i * VState.vLocation;
// Compute the local frame ECEF velocity
vVel = GetTb2l()*VState.vUVW;
vVel = Tb2l * VState.vUVW;
// Finally, make sure that the quaternion stays normalized.
VState.vQtrn.Normalize();
// Refer to Stevens and Lewis, 1.5-14a, pg. 49.
// This is the rotation rate of the "Local" frame, expressed in the local frame.
FGColumnVector3 vOmegaLocal = FGColumnVector3(
radInv*vVel(eEast),
-radInv*vVel(eNorth),
-radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
// Set the angular velocities of the body frame relative to the ECEF frame,
// expressed in the body frame. Effectively, this is:
// w_b/e = w_b/l + w_l/e
VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
FGIC->GetQRadpsIC(),
FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;
VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
// Make an initial run and set past values
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
CalculateQuatdot(); // Angular orientation derivative
CalculateInertialVelocity(); // Translational position derivative
// Initialize past values deques
VState.dqPQRdot.clear();
VState.dqUVWdot.clear();
VState.dqInertialVelocity.clear();
VState.dqQtrndot.clear();
for (int i=0; i<4; i++) {
VState.dqPQRdot.push_front(vPQRdot);
VState.dqUVWdot.push_front(vUVWdot);
VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
VState.dqQtrndot.push_front(vQtrndot);
}
// Recompute the LocalTerrainRadius.
RecomputeLocalTerrainRadius();
// These local copies of the transformation matrices are for use for
// initial conditions only.
Tl2b = GetTl2b(); // local to body frame transform
Tb2l = Tl2b.Transposed(); // body to local frame transform
Tl2ec = GetTl2ec(); // local to ECEF transform
Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
Ti2ec = GetTi2ec(); // ECI to ECEF transform
Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
Ti2b = Tec2b*Ti2ec; // ECI to body frame transform
Tb2i = Ti2b.Transposed(); // body to ECI frame transform
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -230,123 +257,65 @@ Inertial.
bool FGPropagate::Run(void)
{
static int ctr;
if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
if (FDMExec->Holding()) return false;
double dt = FDMExec->GetDeltaT()*rate; // The 'stepsize'
RunPreFunctions();
// Calculate state derivatives
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
CalculateQuatdot(); // Angular orientation derivative
CalculateInertialVelocity(); // Translational position derivative
// Propagate rotational / translational velocity, angular /translational position, respectively.
Integrate(VState.vPQRi, vPQRdot, VState.dqPQRdot, dt, integrator_rotational_rate);
Integrate(VState.vUVW, vUVWdot, VState.dqUVWdot, dt, integrator_translational_rate);
Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
VState.qAttitudeECI.Normalize(); // Normalize the ECI Attitude quaternion
VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle()); // Update the Earth position angle (EPA)
// Update the "Location-based" transformation matrices from the vLocation vector.
Ti2ec = GetTi2ec(); // ECI to ECEF transform
Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
Tl2ec = GetTl2ec(); // local to ECEF transform
Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
Ti2l = GetTi2l();
Tl2i = Ti2l.Transposed();
// Update the "Orientation-based" transformation matrices from the orientation quaternion
Ti2b = GetTi2b(); // ECI to body frame transform
Tb2i = Ti2b.Transposed(); // body to ECI frame transform
Tl2b = Ti2b*Tl2i; // local to body frame transform
Tb2l = Tl2b.Transposed(); // body to local frame transform
Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
// Set auxililary state variables
VState.vLocation = Ti2ec*VState.vInertialPosition;
RecomputeLocalTerrainRadius();
// Calculate current aircraft radius from center of planet
VehicleRadius = GetRadius();
VehicleRadius = VState.vInertialPosition.Magnitude();
radInv = 1.0/VehicleRadius;
// These local copies of the transformation matrices are for use this
// pass through Run() only.
VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
Tl2b = GetTl2b(); // local to body frame transform
Tb2l = Tl2b.Transposed(); // body to local frame transform
Tl2ec = GetTl2ec(); // local to ECEF transform
Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
Ti2ec = GetTi2ec(); // ECI to ECEF transform
Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
Ti2b = Tec2b*Ti2ec; // ECI to body frame transform
Tb2i = Ti2b.Transposed(); // body to ECI frame transform
VState.qAttitudeLocal = Tl2b.GetQuaternion();
// Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
vVel = Tb2l * VState.vUVW;
// Inertial angular velocity measured in the body frame.
vPQRi = VState.vPQR + Tec2b*vOmega;
// Calculate state derivatives
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
CalculateQuatdot(); // Angular orientation derivative
CalculateLocationdot(); // Translational position derivative
// Integrate to propagate the state
double dt = State->Getdt()*rate; // The 'stepsize'
// Propagate rotational velocity
switch(integrator_rotational_rate) {
case eRectEuler: VState.vPQR += dt*vPQRdot;
break;
case eTrapezoidal: VState.vPQR += 0.5*dt*(vPQRdot + last_vPQRdot);
break;
case eAdamsBashforth2: VState.vPQR += dt*(1.5*vPQRdot - 0.5*last_vPQRdot);
break;
case eAdamsBashforth3: VState.vPQR += (1/12.0)*dt*(23.0*vPQRdot - 16.0*last_vPQRdot + 5.0*last2_vPQRdot);
break;
case eNone: // do nothing, freeze angular rate
break;
}
// Propagate translational velocity
switch(integrator_translational_rate) {
case eRectEuler: VState.vUVW += dt*vUVWdot;
break;
case eTrapezoidal: VState.vUVW += 0.5*dt*(vUVWdot + last_vUVWdot);
break;
case eAdamsBashforth2: VState.vUVW += dt*(1.5*vUVWdot - 0.5*last_vUVWdot);
break;
case eAdamsBashforth3: VState.vUVW += (1/12.0)*dt*(23.0*vUVWdot - 16.0*last_vUVWdot + 5.0*last2_vUVWdot);
break;
case eNone: // do nothing, freeze translational rate
break;
}
// Propagate angular position
switch(integrator_rotational_position) {
case eRectEuler: VState.vQtrn += dt*vQtrndot;
break;
case eTrapezoidal: VState.vQtrn += 0.5*dt*(vQtrndot + last_vQtrndot);
break;
case eAdamsBashforth2: VState.vQtrn += dt*(1.5*vQtrndot - 0.5*last_vQtrndot);
break;
case eAdamsBashforth3: VState.vQtrn += (1/12.0)*dt*(23.0*vQtrndot - 16.0*last_vQtrndot + 5.0*last2_vQtrndot);
break;
case eNone: // do nothing, freeze angular position
break;
}
// Propagate translational position
switch(integrator_translational_position) {
case eRectEuler: VState.vLocation += dt*vLocationDot;
break;
case eTrapezoidal: VState.vLocation += 0.5*dt*(vLocationDot + last_vLocationDot);
break;
case eAdamsBashforth2: VState.vLocation += dt*(1.5*vLocationDot - 0.5*last_vLocationDot);
break;
case eAdamsBashforth3: VState.vLocation += (1/12.0)*dt*(23.0*vLocationDot - 16.0*last_vLocationDot + 5.0*last2_vLocationDot);
break;
case eNone: // do nothing, freeze translational position
break;
}
// Set past values
last2_vPQRdot = last_vPQRdot;
last_vPQRdot = vPQRdot;
last2_vUVWdot = last_vUVWdot;
last_vUVWdot = vUVWdot;
last2_vQtrndot = last_vQtrndot;
last_vQtrndot = vQtrndot;
last2_vLocationDot = last_vLocationDot;
last_vLocationDot = vLocationDot;
RunPreFunctions();
RunPostFunctions();
Debug(2);
return false;
@ -361,7 +330,7 @@ bool FGPropagate::Run(void)
// J is the inertia matrix
// Jinv is the inverse inertia matrix
// vMoments is the moment vector in the body frame
// vPQRi is the total inertial angular velocity of the vehicle
// VState.vPQRi is the total inertial angular velocity of the vehicle
// expressed in the body frame.
// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
// Second edition (2004), eqn 1.5-16e (page 50)
@ -376,7 +345,7 @@ void FGPropagate::CalculatePQRdot(void)
// moments and the total inertial angular velocity expressed in the body
// frame.
vPQRdot = Jinv*(vMoments - vPQRi*(J*vPQRi));
vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -388,72 +357,140 @@ void FGPropagate::CalculatePQRdot(void)
void FGPropagate::CalculateQuatdot(void)
{
vOmegaLocal.InitMatrix( radInv*vVel(eEast),
-radInv*vVel(eNorth),
-radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
// Compute quaternion orientation derivative on current body rates
vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*vOmegaLocal);
vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// This set of calculations results in the body frame accelerations being
// computed.
// Compute body frame accelerations based on the current body forces.
// Include centripetal and coriolis accelerations.
// vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
// so it has to be transformed to the body frame. More completely,
// vOmegaEarth is the rate of the ECEF frame relative to the Inertial
// frame (ECI), expressed in the Inertial frame.
// vForces is the total force on the vehicle in the body frame.
// VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
// in the body frame.
// VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
// in the body frame.
// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
// Second edition (2004), eqn 1.5-16d (page 50)
// Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
void FGPropagate::CalculateUVWdot(void)
{
double mass = MassBalance->GetMass(); // mass
const FGColumnVector3& vForces = Aircraft->GetForces(); // current forces
const FGColumnVector3 vGravAccel( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
// Begin to compute body frame accelerations based on the current body forces
vUVWdot = vForces/mass - VState.vPQR * VState.vUVW;
// Include Coriolis acceleration.
vUVWdot -= 2.0 * (Ti2b *vOmega) * VState.vUVW;
// Include Centrifugal acceleration.
if (!GroundReactions->GetWOW()) {
vUVWdot -= Ti2b*(vOmega*(vOmega*(Tec2i*VState.vLocation)));
// Include Centripetal acceleration.
if (!GroundReactions->GetWOW() && Aircraft->GetHoldDown() == 0) {
vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
}
// Include Gravitation accel
FGColumnVector3 gravAccel = Tl2b*vGravAccel;
vUVWdot += gravAccel;
switch (gravType) {
case gtStandard:
vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
break;
case gtWGS84:
vGravAccel = Tec2b * Inertial->GetGravityJ2(VState.vLocation);
break;
}
vUVWdot += vGravAccel;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Transform the velocity vector of the body relative to the origin (Earth
// center) to be expressed in the inertial frame, and add the vehicle velocity
// contribution due to the rotation of the planet.
// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
// Second edition (2004), eqn 1.5-16c (page 50)
void FGPropagate::CalculateInertialVelocity(void)
{
VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::CalculateLocationdot(void)
void FGPropagate::Integrate( FGColumnVector3& Integrand,
FGColumnVector3& Val,
deque <FGColumnVector3>& ValDot,
double dt,
eIntegrateType integration_type)
{
// Transform the vehicle velocity relative to the ECEF frame, expressed
// in the body frame, to be expressed in the ECEF frame.
vLocationDot = Tb2ec * VState.vUVW;
ValDot.push_front(Val);
ValDot.pop_back();
// Now, transform the velocity vector of the body relative to the origin (Earth
// center) to be expressed in the inertial frame, and add the vehicle velocity
// contribution due to the rotation of the planet. The above velocity is only
// relative to the rotating ECEF frame.
// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
// Second edition (2004), eqn 1.5-16c (page 50)
switch(integration_type) {
case eRectEuler: Integrand += dt*ValDot[0];
break;
case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
break;
case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
break;
case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
break;
case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
break;
case eNone: // do nothing, freeze translational rate
break;
}
}
vInertialVelocity = Tec2i * vLocationDot + (vOmega * (Tec2i * VState.vLocation));
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::Integrate( FGQuaternion& Integrand,
FGQuaternion& Val,
deque <FGQuaternion>& ValDot,
double dt,
eIntegrateType integration_type)
{
ValDot.push_front(Val);
ValDot.pop_back();
switch(integration_type) {
case eRectEuler: Integrand += dt*ValDot[0];
break;
case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
break;
case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
break;
case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
break;
case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
break;
case eNone: // do nothing, freeze translational rate
break;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
VState.qAttitudeECI = Qi;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
VState.vInertialVelocity = Vi;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::RecomputeLocalTerrainRadius(void)
{
double t = State->Getsim_time();
double t = FDMExec->GetSimTime();
// Get the LocalTerrain radius.
FGLocation contactloc;
FGColumnVector3 dv;
FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
LocalTerrainRadius = contactloc.GetRadius();
// FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
// LocalTerrainRadius = contactloc.GetRadius();
LocalTerrainRadius = FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -472,17 +509,19 @@ double FGPropagate::GetTerrainElevation(void) const
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//Todo: when should this be called - when should the new EPA be used to
// calculate the transformation matrix, so that the matrix is not a step
// ahead of the sim and the associated calculations?
const FGMatrix33& FGPropagate::GetTi2ec(void)
{
return VState.vLocation.GetTi2ec(Inertial->GetEarthPositionAngle());
return VState.vLocation.GetTi2ec();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
const FGMatrix33& FGPropagate::GetTec2i(void)
{
return VState.vLocation.GetTec2i(Inertial->GetEarthPositionAngle());
return VState.vLocation.GetTec2i();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -518,7 +557,7 @@ void FGPropagate::SetDistanceAGL(double tt)
void FGPropagate::bind(void)
{
typedef double (FGPropagate::*PMF)(int) const;
// typedef double (FGPropagate::*dPMF)() const;
PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
@ -572,10 +611,11 @@ void FGPropagate::bind(void)
PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
PropertyManager->Tie("simulation/integrator/rate/rotational", &integrator_rotational_rate);
PropertyManager->Tie("simulation/integrator/rate/translational", &integrator_translational_rate);
PropertyManager->Tie("simulation/integrator/position/rotational", &integrator_rotational_position);
PropertyManager->Tie("simulation/integrator/position/translational", &integrator_translational_position);
PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
PropertyManager->Tie("simulation/gravity-model", &gravType);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -612,7 +652,96 @@ void FGPropagate::Debug(int from)
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 ) { // Runtime state variables
if (debug_lvl & 8 && from == 2) { // Runtime state variables
cout << endl << fgblue << highint << left
<< " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
<< reset << endl;
cout << endl;
cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
<< Inertial->GetEarthPositionAngleDeg() << endl;
cout << endl;
cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
cout << endl;
cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
<< reset << endl << Tec2b.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
<< reset << endl << Tb2ec.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
<< reset << endl << Tl2b.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
<< reset << endl << Tb2l.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
<< reset << endl << Tl2ec.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
<< reset << endl << Tec2l.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
<< reset << endl << Tec2i.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
<< reset << endl << Ti2ec.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
<< reset << endl << Ti2b.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
<< reset << endl << Tb2i.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
<< reset << endl << Ti2l.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
<< reset << endl << Tl2i.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << setprecision(6); // reset the output stream
}
if (debug_lvl & 16) { // Sanity checking
if (from == 2) { // State sanity checking

152
src/FDM/JSBSim/models/FGPropagate.h
View file

@ -43,12 +43,13 @@ INCLUDES
#include "math/FGLocation.h"
#include "math/FGQuaternion.h"
#include "math/FGMatrix33.h"
#include <deque>
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_PROPAGATE "$Id$"
#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.41 2010/07/09 04:11:45 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -56,6 +57,7 @@ FORWARD DECLARATIONS
namespace JSBSim {
using std::deque;
class FGInitialCondition;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -66,6 +68,16 @@ CLASS DOCUMENTATION
The Equations of Motion (EOM) for JSBSim are integrated to propagate the
state of the vehicle given the forces and moments that act on it. The
integration accounts for a rotating Earth.
The general execution of this model follows this process:
-Calculate the angular accelerations
-Calculate the translational accelerations
-Calculate the angular rate
-Calculate the translational velocity
-Integrate accelerations and rates
Integration of rotational and translation position and rate can be
customized as needed or frozen by the selection of no integrator. The
selection of which integrator to use is done through the setting of
@ -86,10 +98,11 @@ CLASS DOCUMENTATION
2: Trapezoidal
3: Adams Bashforth 2
4: Adams Bashforth 3
5: Adams Bashforth 4
@endcode
@author Jon S. Berndt, Mathias Froehlich
@version $Id$
@version $Id: FGPropagate.h,v 1.41 2010/07/09 04:11:45 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -106,17 +119,38 @@ public:
fixed (ECEF) frame.
units ft */
FGLocation vLocation;
/** The velocity vector of the vehicle with respect to the ECEF frame,
expressed in the body system.
units ft/sec */
FGColumnVector3 vUVW;
/** The angular velocity vector for the vehicle relative to the ECEF frame,
expressed in the body frame.
units rad/sec */
FGColumnVector3 vPQR;
/** The angular velocity vector for the vehicle body frame relative to the
ECI frame, expressed in the body frame.
units rad/sec */
FGColumnVector3 vPQRi;
/** The current orientation of the vehicle, that is, the orientation of the
body frame relative to the local, vehilce-carried, NED frame. */
FGQuaternion vQtrn;
body frame relative to the local, NED frame. */
FGQuaternion qAttitudeLocal;
/** The current orientation of the vehicle, that is, the orientation of the
body frame relative to the inertial (ECI) frame. */
FGQuaternion qAttitudeECI;
FGColumnVector3 vInertialVelocity;
FGColumnVector3 vInertialPosition;
deque <FGColumnVector3> dqPQRdot;
deque <FGColumnVector3> dqUVWdot;
deque <FGColumnVector3> dqInertialVelocity;
deque <FGQuaternion> dqQtrndot;
};
/** Constructor.
@ -133,7 +167,10 @@ public:
~FGPropagate();
/// These define the indices use to select the various integrators.
enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3};
enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
/// These define the indices use to select the gravitation models.
enum eGravType {gtStandard, gtWGS84};
/** Initializes the FGPropagate class after instantiation and prior to first execution.
The base class FGModel::InitModel is called first, initializing pointers to the
@ -144,6 +181,12 @@ public:
@return false if no error */
bool Run(void);
void CalculatePQRdot(void);
void CalculateQuatdot(void);
void CalculateInertialVelocity(void);
void CalculateUVWdot(void);
const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
/** Retrieves the velocity vector.
The vector returned is represented by an FGColumnVector reference. The vector
for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
@ -207,9 +250,9 @@ public:
in FGJSBBase. The relevant enumerators for the vector returned by this call are,
eP=1, eQ=2, eR=3.
units rad/sec
@return The body frame angular rates in rad/sec.
@return The body frame inertial angular rates in rad/sec.
*/
const FGColumnVector3& GetPQRi(void) const {return vPQRi;}
const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
/** Retrieves the body axis angular acceleration vector.
Retrieves the body axis angular acceleration vector in rad/sec^2. The
@ -241,7 +284,7 @@ public:
angle about the Y axis, and the third item is the angle
about the Z axis (Phi, Theta, Psi).
*/
const FGColumnVector3& GetEuler(void) const { return VState.vQtrn.GetEuler(); }
const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
/** Retrieves a body frame velocity component.
Retrieves a body frame velocity component. The velocity returned is
@ -284,7 +327,15 @@ public:
/** Retrieves the total inertial velocity in ft/sec.
*/
double GetInertialVelocityMagnitude(void) const { return vInertialVelocity.Magnitude(); }
double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
/** Retrieves the inertial velocity vector in ft/sec.
*/
const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
/** Retrieves the inertial position vector.
*/
const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
/** Returns the current altitude above sea level.
This function returns the altitude above sea level.
@ -324,7 +375,7 @@ public:
@param axis the index of the angular velocity component desired (1-based).
@return The body frame angular velocity component.
*/
double GetPQRi(int axis) const {return vPQRi(axis);}
double GetPQRi(int axis) const {return VState.vPQRi(axis);}
/** Retrieves a body frame angular acceleration component.
Retrieves a body frame angular acceleration component. The angular
@ -350,7 +401,7 @@ public:
units radians
@return An Euler angle.
*/
double GetEuler(int axis) const { return VState.vQtrn.GetEuler(axis); }
double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
/** Retrieves the cosine of a vehicle Euler angle component.
Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
@ -362,7 +413,7 @@ public:
units none
@return The cosine of an Euler angle.
*/
double GetCosEuler(int idx) const { return VState.vQtrn.GetCosEuler(idx); }
double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
/** Retrieves the sine of a vehicle Euler angle component.
Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
@ -374,7 +425,7 @@ public:
units none
@return The sine of an Euler angle.
*/
double GetSinEuler(int idx) const { return VState.vQtrn.GetSinEuler(idx); }
double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
/** Returns the current altitude rate.
Returns the current altitude rate (rate of climb).
@ -414,13 +465,13 @@ public:
The quaternion class, being the means by which the orientation of the
vehicle is stored, manages the local-to-body transformation matrix.
@return a reference to the local-to-body transformation matrix. */
const FGMatrix33& GetTl2b(void) const { return VState.vQtrn.GetT(); }
const FGMatrix33& GetTl2b(void) const { return VState.qAttitudeLocal.GetT(); }
/** Retrieves the body-to-local transformation matrix.
The quaternion class, being the means by which the orientation of the
vehicle is stored, manages the body-to-local transformation matrix.
@return a reference to the body-to-local matrix. */
const FGMatrix33& GetTb2l(void) const { return VState.vQtrn.GetTInv(); }
const FGMatrix33& GetTb2l(void) const { return VState.qAttitudeLocal.GetTInv(); }
/** Retrieves the ECEF-to-body transformation matrix.
@return a reference to the ECEF-to-body transformation matrix. */
@ -430,6 +481,14 @@ public:
@return a reference to the body-to-ECEF matrix. */
const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
/** Retrieves the ECI-to-body transformation matrix.
@return a reference to the ECI-to-body transformation matrix. */
const FGMatrix33& GetTi2b(void) const { return VState.qAttitudeECI.GetT(); }
/** Retrieves the body-to-ECI transformation matrix.
@return a reference to the body-to-ECI matrix. */
const FGMatrix33& GetTb2i(void) const { return VState.qAttitudeECI.GetTInv(); }
/** Retrieves the ECEF-to-ECI transformation matrix.
@return a reference to the ECEF-to-ECI transformation matrix. */
const FGMatrix33& GetTec2i(void);
@ -450,16 +509,33 @@ public:
@return a reference to the local-to-ECEF matrix. */
const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
/** Retrieves the local-to-inertial transformation matrix.
@return a reference to the local-to-inertial transformation matrix. */
const FGMatrix33& GetTl2i(void) { return VState.vLocation.GetTl2i(); }
/** Retrieves the inertial-to-local transformation matrix.
@return a reference to the inertial-to-local matrix. */
const FGMatrix33& GetTi2l(void) { return VState.vLocation.GetTi2l(); }
VehicleState* GetVState(void) { return &VState; }
void SetVState(VehicleState* vstate) {
VState.vLocation = vstate->vLocation;
VState.vUVW = vstate->vUVW;
VState.vPQR = vstate->vPQR;
VState.vQtrn = vstate->vQtrn; // ... mmh
VState.qAttitudeLocal = vstate->qAttitudeLocal;
VState.qAttitudeECI = vstate->qAttitudeECI;
VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
}
const FGQuaternion GetQuaternion(void) const { return VState.vQtrn; }
void SetInertialOrientation(FGQuaternion Qi);
void SetInertialVelocity(FGColumnVector3 Vi);
const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
void SetPQR(unsigned int i, double val) {
if ((i>=1) && (i<=3) )
@ -492,11 +568,6 @@ public:
VState.vLocation -= vDeltaXYZEC;
}
void CalculatePQRdot(void);
void CalculateQuatdot(void);
void CalculateLocationdot(void);
void CalculateUVWdot(void);
private:
// state vector
@ -504,16 +575,14 @@ private:
struct VehicleState VState;
FGColumnVector3 vVel;
FGColumnVector3 vPQRdot;
FGColumnVector3 vUVWdot;
FGColumnVector3 vInertialVelocity;
FGColumnVector3 vPQRdot, last_vPQRdot, last2_vPQRdot;
FGColumnVector3 vUVWdot, last_vUVWdot, last2_vUVWdot;
FGColumnVector3 vLocationDot, last_vLocationDot, last2_vLocationDot;
FGColumnVector3 vLocation;
FGColumnVector3 vDeltaXYZEC;
FGColumnVector3 vPQRi; // Inertial frame angular velocity
FGColumnVector3 vOmega; // The Earth angular velocity vector
FGColumnVector3 vOmegaLocal; // The local frame angular velocity vector
FGQuaternion vQtrndot, last_vQtrndot, last2_vQtrndot;
FGColumnVector3 vGravAccel;
FGColumnVector3 vOmegaEarth; // The Earth angular velocity vector
FGQuaternion vQtrndot;
FGMatrix33 Tec2b;
FGMatrix33 Tb2ec;
FGMatrix33 Tl2b; // local to body frame matrix copy for immediate local use
@ -524,13 +593,30 @@ private:
FGMatrix33 Ti2ec; // ECI to ECEF frame matrix copy for immediate local use
FGMatrix33 Ti2b; // ECI to body frame rotation matrix
FGMatrix33 Tb2i; // body to ECI frame rotation matrix
FGMatrix33 Ti2l;
FGMatrix33 Tl2i;
FGLocation contactloc;
FGColumnVector3 dv;
double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
double radInv;
int integrator_rotational_rate;
int integrator_translational_rate;
int integrator_rotational_position;
int integrator_translational_position;
eIntegrateType integrator_rotational_rate;
eIntegrateType integrator_translational_rate;
eIntegrateType integrator_rotational_position;
eIntegrateType integrator_translational_position;
int gravType;
void Integrate( FGColumnVector3& Integrand,
FGColumnVector3& Val,
deque <FGColumnVector3>& ValDot,
double dt,
eIntegrateType integration_type);
void Integrate( FGQuaternion& Integrand,
FGQuaternion& Val,
deque <FGQuaternion>& ValDot,
double dt,
eIntegrateType integration_type);
void bind(void);
void Debug(int from);

5
src/FDM/JSBSim/models/FGPropulsion.cpp
View file

@ -45,7 +45,6 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGPropulsion.h"
#include "FGState.h"
#include "models/FGFCS.h"
#include "models/FGMassBalance.h"
#include "models/propulsion/FGThruster.h"
@ -66,7 +65,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGPropulsion.cpp,v 1.39 2010/02/25 05:21:36 jberndt Exp $";
static const char *IdHdr = ID_PROPULSION;
extern short debug_lvl;
@ -149,7 +148,7 @@ bool FGPropulsion::Run(void)
RunPreFunctions();
double dt = State->Getdt();
double dt = FDMExec->GetDeltaT();
vForces.InitMatrix();
vMoments.InitMatrix();

12
src/FDM/JSBSim/models/FGPropulsion.h
View file

@ -49,7 +49,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_PROPULSION "$Id$"
#define ID_PROPULSION "$Id: FGPropulsion.h,v 1.25 2010/01/02 17:58:01 andgi Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -91,7 +91,7 @@ CLASS DOCUMENTATION
@endcode
@author Jon S. Berndt
@version $Id$
@version $Id: FGPropulsion.h,v 1.25 2010/01/02 17:58:01 andgi Exp $
@see
FGEngine
FGTank
@ -132,8 +132,8 @@ public:
@return the address of the specific engine, or zero if no such engine is
available */
inline FGEngine* GetEngine(unsigned int index) {
if (index <= Engines.size()-1) return Engines[index];
else return 0L; }
if (index < Engines.size()) return Engines[index];
else return 0L; }
/// Retrieves the number of tanks defined for the aircraft.
inline unsigned int GetNumTanks(void) const {return (unsigned int)Tanks.size();}
@ -143,8 +143,8 @@ public:
@return the address of the specific tank, or zero if no such tank is
available */
inline FGTank* GetTank(unsigned int index) {
if (index <= Tanks.size()-1) return Tanks[index];
else return 0L; }
if (index < Tanks.size()) return Tanks[index];
else return 0L; }
/** Returns the number of fuel tanks currently actively supplying fuel */
inline int GetnumSelectedFuelTanks(void) const {return numSelectedFuelTanks;}

4
src/FDM/JSBSim/models/atmosphere/FGMSIS.cpp
View file

@ -58,7 +58,7 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGMSIS.h"
#include "FGState.h"
#include "models/FGAuxiliary.h"
#include <cmath> /* maths functions */
#include <iostream> // for cout, endl
@ -66,7 +66,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGMSIS.cpp,v 1.13 2010/02/25 05:21:36 jberndt Exp $";
static const char *IdHdr = ID_MSIS;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

5
src/FDM/JSBSim/models/atmosphere/FGMSIS.h
View file

@ -41,12 +41,13 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "models/FGAtmosphere.h"
#include "FGFDMExec.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_MSIS "$Id$"
#define ID_MSIS "$Id: FGMSIS.h,v 1.7 2010/02/25 05:21:36 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -74,7 +75,7 @@ CLASS DOCUMENTATION
and check http://www.brodo.de/english/pub/nrlmsise/index.html for
updated releases of this package.
@author David Culp
@version $Id$
@version $Id: FGMSIS.h,v 1.7 2010/02/25 05:21:36 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

3
src/FDM/JSBSim/models/atmosphere/FGMars.cpp
View file

@ -42,14 +42,13 @@ INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGMars.h"
#include "FGState.h"
#include <iostream>
using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGMars.cpp,v 1.10 2010/02/25 05:21:36 jberndt Exp $";
static const char *IdHdr = ID_MARS;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

4
src/FDM/JSBSim/models/atmosphere/FGMars.h
View file

@ -44,7 +44,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_MARS "$Id$"
#define ID_MARS "$Id: FGMars.h,v 1.8 2009/10/02 10:30:09 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -58,7 +58,7 @@ CLASS DOCUMENTATION
/** Models the Martian atmosphere.
@author Jon Berndt
@version $Id$
@version $Id: FGMars.h,v 1.8 2009/10/02 10:30:09 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp
View file

@ -45,7 +45,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGAccelerometer.cpp,v 1.8 2009/10/24 22:59:30 jberndt Exp $";
static const char *IdHdr = ID_ACCELEROMETER;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

4
src/FDM/JSBSim/models/flight_control/FGAccelerometer.h
View file

@ -50,7 +50,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_ACCELEROMETER "$Id$"
#define ID_ACCELEROMETER "$Id: FGAccelerometer.h,v 1.4 2009/10/02 10:30:09 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -110,7 +110,7 @@ even varying all the way from 0.95 to 1.05 in adjacent frames - whatever the del
time.
@author Jon S. Berndt
@version $Revision$
@version $Revision: 1.4 $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/models/flight_control/FGActuator.cpp Executable file → Normal file
View file

@ -43,7 +43,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGActuator.cpp,v 1.14 2009/10/24 22:59:30 jberndt Exp $";
static const char *IdHdr = ID_ACTUATOR;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

4
src/FDM/JSBSim/models/flight_control/FGActuator.h Executable file → Normal file
View file

@ -44,7 +44,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_ACTUATOR "$Id$"
#define ID_ACTUATOR "$Id: FGActuator.h,v 1.11 2009/10/02 10:30:09 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -111,7 +111,7 @@ Example:
@endcode
@author Jon S. Berndt
@version $Revision$
@version $Revision: 1.11 $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/models/flight_control/FGDeadBand.cpp
View file

@ -46,7 +46,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGDeadBand.cpp,v 1.9 2009/10/24 22:59:30 jberndt Exp $";
static const char *IdHdr = ID_DEADBAND;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

4
src/FDM/JSBSim/models/flight_control/FGDeadBand.h
View file

@ -43,7 +43,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_DEADBAND "$Id$"
#define ID_DEADBAND "$Id: FGDeadBand.h,v 1.9 2009/10/24 22:59:30 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -80,7 +80,7 @@ CLASS DOCUMENTATION
produce no output. For example, say that the width value is 2.0. If the
input is between -1.0 and +1.0, the output will be zero.
@author Jon S. Berndt
@version $Id$
@version $Id: FGDeadBand.h,v 1.9 2009/10/24 22:59:30 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2
src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp
View file

@ -47,7 +47,7 @@ using namespace std;
namespace JSBSim {
static const char *IdSrc = "$Id$";
static const char *IdSrc = "$Id: FGFCSComponent.cpp,v 1.27 2009/10/24 22:59:30 jberndt Exp $";
static const char *IdHdr = ID_FCSCOMPONENT;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Some files were not shown because too many files have changed in this diff Show more