INCLUDES
*******************************************************************************/
-#ifdef FGFS
-# include <simgear/compiler.h>
-# ifdef SG_HAVE_STD_INCLUDES
-# include <fstream>
-# else
-# include <fstream.h>
-# endif
-#else
-# if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
-# include <fstream.h>
-# else
-# include <fstream>
-# endif
-#endif
-
#include "FGInitialCondition.h"
-#include <FGFDMExec.h>
-#include <models/FGInertial.h>
-#include <models/FGAtmosphere.h>
-#include <models/FGAerodynamics.h>
-#include <models/FGPropagate.h>
-#include <input_output/FGPropertyManager.h>
-#include <models/FGPropulsion.h>
-#include <input_output/FGXMLParse.h>
+#include "FGFDMExec.h"
+#include "models/FGInertial.h"
+#include "models/FGAtmosphere.h"
+#include "models/FGAerodynamics.h"
+#include "models/FGPropagate.h"
+#include "input_output/FGPropertyManager.h"
+#include "input_output/FGXMLElement.h"
+#include "models/FGPropulsion.h"
+#include "input_output/FGXMLParse.h"
+#include "math/FGQuaternion.h"
+#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;
//******************************************************************************
-FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec)
+FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) : fdmex(FDMExec)
+{
+ InitializeIC();
+
+ if(FDMExec != NULL ) {
+ fdmex->GetPropagate()->SetAltitudeASL(altitudeASL);
+ fdmex->GetAtmosphere()->Run();
+ PropertyManager=fdmex->GetPropertyManager();
+ Constructing = true;
+ bind();
+ Constructing = false;
+ } else {
+ cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
+ }
+
+ Debug(0);
+}
+
+//******************************************************************************
+
+FGInitialCondition::~FGInitialCondition()
+{
+ Debug(1);
+}
+
+//******************************************************************************
+
+void FGInitialCondition::ResetIC(double u0, double v0, double w0,
+ double p0, double q0, double r0,
+ double alpha0, double beta0,
+ double phi0, double theta0, double psi0,
+ double latRad0, double lonRad0, double altAGLFt0,
+ double gamma0)
+{
+ InitializeIC();
+
+ u = u0; v = v0; w = w0;
+ p = p0; q = q0; r = r0;
+ alpha = alpha0; beta = beta0;
+ phi = phi0; theta = theta0; psi = psi0;
+ gamma = gamma0;
+
+ latitude = latRad0;
+ longitude = lonRad0;
+ SetAltitudeAGLFtIC(altAGLFt0);
+
+ cphi = cos(phi); sphi = sin(phi); // phi, rad
+ ctheta = cos(theta); stheta = sin(theta); // theta, rad
+ cpsi = cos(psi); spsi = sin(psi); // psi, rad
+
+ FGQuaternion Quat( phi, theta, psi );
+ Quat.Normalize();
+
+ const FGMatrix33& _Tl2b = Quat.GetT(); // local to body frame
+ const FGMatrix33& _Tb2l = Quat.GetTInv(); // body to local
+
+ FGColumnVector3 _vUVW_BODY(u,v,w);
+ FGColumnVector3 _vUVW_NED = _Tb2l * _vUVW_BODY;
+ FGColumnVector3 _vWIND_NED(wnorth,weast,wdown);
+// FGColumnVector3 _vUVWAero = _Tl2b * ( _vUVW_NED + _vWIND_NED );
+
+ uw=_vWIND_NED(1); vw=_vWIND_NED(2); ww=_vWIND_NED(3);
+
+}
+
+//******************************************************************************
+
+void FGInitialCondition::InitializeIC(void)
{
vt=vc=ve=vg=0;
mach=0;
alpha=beta=gamma=0;
theta=phi=psi=0;
- altitude=hdot=0;
+ altitudeASL=hdot=0;
latitude=longitude=0;
u=v=w=0;
p=q=r=0;
wdir=wmag=0;
lastSpeedSet=setvt;
lastWindSet=setwned;
- sea_level_radius = FDMExec->GetInertial()->RefRadius();
- radius_to_vehicle = FDMExec->GetInertial()->RefRadius();
- terrain_altitude = 0;
+ radius_to_vehicle = sea_level_radius = fdmex->GetInertial()->GetRefRadius();
+ terrain_elevation = 0;
+
+ targetNlfIC = 1.0;
salpha=sbeta=stheta=sphi=spsi=sgamma=0;
calpha=cbeta=ctheta=cphi=cpsi=cgamma=1;
-
- if(FDMExec != NULL ) {
- fdmex=FDMExec;
- fdmex->GetPropagate()->Seth(altitude);
- fdmex->GetAtmosphere()->Run();
- PropertyManager=fdmex->GetPropertyManager();
- bind();
- } else {
- cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
- }
-
- Debug(0);
}
//******************************************************************************
-FGInitialCondition::~FGInitialCondition()
+void FGInitialCondition::WriteStateFile(int num)
{
- unbind();
- Debug(1);
+ if (Constructing) return;
+
+ string filename = fdmex->GetFullAircraftPath();
+
+ if (filename.empty())
+ filename = "initfile.xml";
+ else
+ filename.append("/initfile.xml");
+
+ ofstream outfile(filename.c_str());
+ FGPropagate* Propagate = fdmex->GetPropagate();
+
+ if (outfile.is_open()) {
+ outfile << "<?xml version=\"1.0\"?>" << endl;
+ outfile << "<initialize name=\"reset00\">" << endl;
+ outfile << " <ubody unit=\"FT/SEC\"> " << Propagate->GetUVW(eX) << " </ubody> " << endl;
+ outfile << " <vbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eY) << " </vbody> " << endl;
+ outfile << " <wbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eZ) << " </wbody> " << endl;
+ outfile << " <phi unit=\"DEG\"> " << Propagate->GetEuler(ePhi) << " </phi>" << endl;
+ outfile << " <theta unit=\"DEG\"> " << Propagate->GetEuler(eTht) << " </theta>" << endl;
+ outfile << " <psi unit=\"DEG\"> " << Propagate->GetEuler(ePsi) << " </psi>" << endl;
+ outfile << " <longitude unit=\"DEG\"> " << Propagate->GetLongitudeDeg() << " </longitude>" << endl;
+ outfile << " <latitude unit=\"DEG\"> " << Propagate->GetLatitudeDeg() << " </latitude>" << endl;
+ outfile << " <altitude unit=\"FT\"> " << Propagate->GetAltitudeASL() << " </altitude>" << endl;
+ outfile << "</initialize>" << endl;
+ outfile.close();
+ } else {
+ cerr << "Could not open and/or write the state to the initial conditions file: " << filename << endl;
+ }
}
//******************************************************************************
lastSpeedSet=setuvw;
}
+
+//******************************************************************************
+
+void FGInitialCondition::SetVNorthFpsIC(double tt) {
+ double ua,va,wa;
+ double vxz;
+ vnorth = tt;
+ calcUVWfromNED();
+ ua = u + uw; va = v + vw; wa = w + ww;
+ vt = sqrt( ua*ua + va*va + wa*wa );
+ alpha = beta = 0;
+ vxz = sqrt( u*u + w*w );
+ if( w != 0 ) alpha = atan2( w, u );
+ if( vxz != 0 ) beta = atan2( v, vxz );
+ mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
+ vc=calcVcas(mach);
+ ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
+ lastSpeedSet=setned;
+}
+
+//******************************************************************************
+
+void FGInitialCondition::SetVEastFpsIC(double tt) {
+ double ua,va,wa;
+ double vxz;
+ veast = tt;
+ calcUVWfromNED();
+ ua = u + uw; va = v + vw; wa = w + ww;
+ vt = sqrt( ua*ua + va*va + wa*wa );
+ alpha = beta = 0;
+ vxz = sqrt( u*u + w*w );
+ if( w != 0 ) alpha = atan2( w, u );
+ if( vxz != 0 ) beta = atan2( v, vxz );
+ mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
+ vc=calcVcas(mach);
+ ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
+ lastSpeedSet=setned;
+}
+
+//******************************************************************************
+
+void FGInitialCondition::SetVDownFpsIC(double tt) {
+ double ua,va,wa;
+ double vxz;
+ vdown = tt;
+ calcUVWfromNED();
+ ua = u + uw; va = v + vw; wa = w + ww;
+ vt = sqrt( ua*ua + va*va + wa*wa );
+ alpha = beta = 0;
+ vxz = sqrt( u*u + w*w );
+ if( w != 0 ) alpha = atan2( w, u );
+ if( vxz != 0 ) beta = atan2( v, vxz );
+ mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
+ vc=calcVcas(mach);
+ ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
+ SetClimbRateFpsIC(-1*vdown);
+ lastSpeedSet=setned;
+}
+
//******************************************************************************
double FGInitialCondition::GetUBodyFpsIC(void) const {
- if(lastSpeedSet == setvg )
+ if (lastSpeedSet == setvg || lastSpeedSet == setned)
return u;
else
return vt*calpha*cbeta - uw;
//******************************************************************************
double FGInitialCondition::GetVBodyFpsIC(void) const {
- if( lastSpeedSet == setvg )
+ if (lastSpeedSet == setvg || lastSpeedSet == setned)
return v;
else {
return vt*sbeta - vw;
//******************************************************************************
double FGInitialCondition::GetWBodyFpsIC(void) const {
- if( lastSpeedSet == setvg )
+ if (lastSpeedSet == setvg || lastSpeedSet == setned)
return w;
else
return vt*salpha*cbeta -ww;
//******************************************************************************
-void FGInitialCondition::SetAltitudeFtIC(double tt) {
- altitude=tt;
- fdmex->GetPropagate()->Seth(altitude);
+void FGInitialCondition::SetAltitudeASLFtIC(double tt)
+{
+ altitudeASL=tt;
+ fdmex->GetPropagate()->SetAltitudeASL(altitudeASL);
fdmex->GetAtmosphere()->Run();
//lets try to make sure the user gets what they intended
//******************************************************************************
-void FGInitialCondition::SetAltitudeAGLFtIC(double tt) {
- SetAltitudeFtIC(terrain_altitude + tt);
+void FGInitialCondition::SetAltitudeAGLFtIC(double tt)
+{
+ SetAltitudeASLFtIC(terrain_elevation + tt);
}
//******************************************************************************
-void FGInitialCondition::SetSeaLevelRadiusFtIC(double tt) {
+void FGInitialCondition::SetSeaLevelRadiusFtIC(double tt)
+{
sea_level_radius = tt;
}
//******************************************************************************
-void FGInitialCondition::SetTerrainAltitudeFtIC(double tt) {
- terrain_altitude=tt;
+void FGInitialCondition::SetTerrainElevationFtIC(double tt)
+{
+ terrain_elevation=tt;
}
//******************************************************************************
-void FGInitialCondition::calcUVWfromNED(void) {
+void FGInitialCondition::calcUVWfromNED(void)
+{
u=vnorth*ctheta*cpsi +
veast*ctheta*spsi -
vdown*stheta;
//******************************************************************************
-void FGInitialCondition::SetVnorthFpsIC(double tt) {
- vnorth=tt;
- calcUVWfromNED();
- vt=sqrt(u*u + v*v + w*w);
- lastSpeedSet=setned;
-}
-
-//******************************************************************************
-
-void FGInitialCondition::SetVeastFpsIC(double tt) {
- veast=tt;
- calcUVWfromNED();
- vt=sqrt(u*u + v*v + w*w);
- lastSpeedSet=setned;
-}
-
-//******************************************************************************
-
-void FGInitialCondition::SetVdownFpsIC(double tt) {
- vdown=tt;
- calcUVWfromNED();
- vt=sqrt(u*u + v*v + w*w);
- SetClimbRateFpsIC(-1*vdown);
- lastSpeedSet=setned;
-}
-
-//******************************************************************************
-
bool FGInitialCondition::getMachFromVcas(double *Mach,double vcas) {
bool result=false;
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);
bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
{
- string resetDef;
- int n;
-
string sep = "/";
-# ifdef macintosh
- sep = ";";
-# endif
-
if( useStoredPath ) {
- resetDef = fdmex->GetFullAircraftPath() + sep + rstfile + ".xml";
+ init_file_name = fdmex->GetFullAircraftPath() + sep + rstfile + ".xml";
} else {
- resetDef = rstfile;
+ init_file_name = rstfile;
}
- document = LoadXMLDocument(resetDef);
+ document = LoadXMLDocument(init_file_name);
+
+ // Make sure that the document is valid
+ if (!document) {
+ cerr << "File: " << init_file_name << " could not be read." << endl;
+ exit(-1);
+ }
if (document->GetName() != string("initialize")) {
- cerr << "File: " << resetDef << " is not a reset file" << endl;
+ cerr << "File: " << init_file_name << " is not a reset file." << endl;
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"))
+ SetLongitudeDegIC(document->FindElementValueAsNumberConvertTo("longitude", "DEG"));
+ if (document->FindElement("elevation"))
+ SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT"));
+
+ if (document->FindElement("altitude")) // This is feet above ground level
+ SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT"));
+ else if (document->FindElement("altitudeAGL")) // This is feet above ground level
+ SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
+ else if (document->FindElement("altitudeMSL")) // This is feet above sea level
+ SetAltitudeASLFtIC(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
+
if (document->FindElement("ubody"))
SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo("ubody", "FT/SEC"));
if (document->FindElement("vbody"))
SetVBodyFpsIC(document->FindElementValueAsNumberConvertTo("vbody", "FT/SEC"));
if (document->FindElement("wbody"))
SetWBodyFpsIC(document->FindElementValueAsNumberConvertTo("wbody", "FT/SEC"));
- if (document->FindElement("latitude"))
- SetLatitudeDegIC(document->FindElementValueAsNumberConvertTo("latitude", "DEG"));
- if (document->FindElement("longitude"))
- SetLongitudeDegIC(document->FindElementValueAsNumberConvertTo("longitude", "DEG"));
- if (document->FindElement("altitude"))
- SetAltitudeFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT"));
+ if (document->FindElement("vnorth"))
+ SetVNorthFpsIC(document->FindElementValueAsNumberConvertTo("vnorth", "FT/SEC"));
+ if (document->FindElement("veast"))
+ SetVEastFpsIC(document->FindElementValueAsNumberConvertTo("veast", "FT/SEC"));
+ if (document->FindElement("vdown"))
+ SetVDownFpsIC(document->FindElementValueAsNumberConvertTo("vdown", "FT/SEC"));
if (document->FindElement("winddir"))
SetWindDirDegIC(document->FindElementValueAsNumberConvertTo("winddir", "DEG"));
if (document->FindElement("vwind"))
- SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo("vwind", "FT/SEC"));
+ SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo("vwind", "KTS"));
if (document->FindElement("hwind"))
SetHeadWindKtsIC(document->FindElementValueAsNumberConvertTo("hwind", "KTS"));
if (document->FindElement("xwind"))
SetCrossWindKtsIC(document->FindElementValueAsNumberConvertTo("xwind", "KTS"));
if (document->FindElement("vc"))
- SetVcalibratedKtsIC(document->FindElementValueAsNumberConvertTo("vc", "FT/SEC"));
+ SetVcalibratedKtsIC(document->FindElementValueAsNumberConvertTo("vc", "KTS"));
+ if (document->FindElement("vt"))
+ SetVtrueKtsIC(document->FindElementValueAsNumberConvertTo("vt", "KTS"));
if (document->FindElement("mach"))
SetMachIC(document->FindElementValueAsNumber("mach"));
if (document->FindElement("phi"))
if (document->FindElement("roc"))
SetClimbRateFpsIC(document->FindElementValueAsNumberConvertTo("roc", "FT/SEC"));
if (document->FindElement("vground"))
- SetVgroundKtsIC(document->FindElementValueAsNumberConvertTo("vground", "FT/SEC"));
- if (document->FindElement("running")) {
- n = int(document->FindElementValueAsNumber("running"));
- if (n != 0) {
- FGPropulsion* propulsion = fdmex->GetPropulsion();
- for(unsigned int i=0; i<propulsion->GetNumEngines(); i++) {
- propulsion->GetEngine(i)->SetRunning(true);
- }
- }
+ SetVgroundKtsIC(document->FindElementValueAsNumberConvertTo("vground", "KTS"));
+ if (document->FindElement("targetNlf"))
+ {
+ SetTargetNlfIC(document->FindElementValueAsNumber("targetNlf"));
+ }
+
+ // 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();
//******************************************************************************
+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,
&FGInitialCondition::SetLongitudeDegIC,
true);
PropertyManager->Tie("ic/h-sl-ft", this,
- &FGInitialCondition::GetAltitudeFtIC,
- &FGInitialCondition::SetAltitudeFtIC,
+ &FGInitialCondition::GetAltitudeASLFtIC,
+ &FGInitialCondition::SetAltitudeASLFtIC,
true);
PropertyManager->Tie("ic/h-agl-ft", this,
&FGInitialCondition::GetAltitudeAGLFtIC,
&FGInitialCondition::GetSeaLevelRadiusFtIC,
&FGInitialCondition::SetSeaLevelRadiusFtIC,
true);
- PropertyManager->Tie("ic/terrain-altitude-ft", this,
- &FGInitialCondition::GetTerrainAltitudeFtIC,
- &FGInitialCondition::SetTerrainAltitudeFtIC,
+ PropertyManager->Tie("ic/terrain-elevation-ft", this,
+ &FGInitialCondition::GetTerrainElevationFtIC,
+ &FGInitialCondition::SetTerrainElevationFtIC,
true);
PropertyManager->Tie("ic/vg-fps", this,
&FGInitialCondition::GetVgroundFpsIC,
&FGInitialCondition::GetWBodyFpsIC,
&FGInitialCondition::SetWBodyFpsIC,
true);
-
+ PropertyManager->Tie("ic/vn-fps", this,
+ &FGInitialCondition::GetVNorthFpsIC,
+ &FGInitialCondition::SetVNorthFpsIC,
+ true);
+ PropertyManager->Tie("ic/ve-fps", this,
+ &FGInitialCondition::GetVEastFpsIC,
+ &FGInitialCondition::SetVEastFpsIC,
+ true);
+ PropertyManager->Tie("ic/vd-fps", this,
+ &FGInitialCondition::GetVDownFpsIC,
+ &FGInitialCondition::SetVDownFpsIC,
+ true);
PropertyManager->Tie("ic/gamma-rad", this,
&FGInitialCondition::GetFlightPathAngleRadIC,
&FGInitialCondition::SetFlightPathAngleRadIC,
&FGInitialCondition::SetRRadpsIC,
true);
-}
-
-//******************************************************************************
-
-void FGInitialCondition::unbind(void)
-{
- PropertyManager->Untie("ic/vc-kts");
- PropertyManager->Untie("ic/ve-kts");
- PropertyManager->Untie("ic/vg-kts");
- PropertyManager->Untie("ic/vt-kts");
- PropertyManager->Untie("ic/mach");
- PropertyManager->Untie("ic/roc-fpm");
- PropertyManager->Untie("ic/gamma-deg");
- PropertyManager->Untie("ic/alpha-deg");
- PropertyManager->Untie("ic/beta-deg");
- PropertyManager->Untie("ic/theta-deg");
- PropertyManager->Untie("ic/phi-deg");
- PropertyManager->Untie("ic/psi-true-deg");
- PropertyManager->Untie("ic/lat-gc-deg");
- PropertyManager->Untie("ic/long-gc-deg");
- PropertyManager->Untie("ic/h-sl-ft");
- PropertyManager->Untie("ic/h-agl-ft");
- PropertyManager->Untie("ic/sea-level-radius-ft");
- PropertyManager->Untie("ic/terrain-altitude-ft");
- PropertyManager->Untie("ic/vg-fps");
- PropertyManager->Untie("ic/vt-fps");
- PropertyManager->Untie("ic/vw-bx-fps");
- PropertyManager->Untie("ic/vw-by-fps");
- PropertyManager->Untie("ic/vw-bz-fps");
- PropertyManager->Untie("ic/vw-north-fps");
- PropertyManager->Untie("ic/vw-east-fps");
- PropertyManager->Untie("ic/vw-down-fps");
- PropertyManager->Untie("ic/vw-mag-fps");
- PropertyManager->Untie("ic/vw-dir-deg");
-
- PropertyManager->Untie("ic/roc-fps");
-
- PropertyManager->Untie("ic/u-fps");
- PropertyManager->Untie("ic/v-fps");
- PropertyManager->Untie("ic/w-fps");
-
- PropertyManager->Untie("ic/gamma-rad");
- PropertyManager->Untie("ic/alpha-rad");
- PropertyManager->Untie("ic/theta-rad");
- PropertyManager->Untie("ic/beta-rad");
- PropertyManager->Untie("ic/phi-rad");
- PropertyManager->Untie("ic/psi-true-rad");
- PropertyManager->Untie("ic/lat-gc-rad");
- PropertyManager->Untie("ic/long-gc-rad");
- PropertyManager->Untie("ic/p-rad_sec");
- PropertyManager->Untie("ic/q-rad_sec");
- PropertyManager->Untie("ic/r-rad_sec");
+ typedef int (FGInitialCondition::*iPMF)(void) const;
+ PropertyManager->Tie("simulation/write-state-file",
+ this,
+ (iPMF)0,
+ &FGInitialCondition::WriteStateFile);
}
projects / flightgear.git / blobdiff