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 <math/FGQuaternion.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;
//******************************************************************************
InitializeIC();
if(FDMExec != NULL ) {
- fdmex->GetPropagate()->Seth(altitude);
+ 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;
}
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 );
+// FGColumnVector3 _vUVWAero = _Tl2b * ( _vUVW_NED + _vWIND_NED );
uw=_vWIND_NED(1); vw=_vWIND_NED(2); ww=_vWIND_NED(3);
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;
lastSpeedSet=setvt;
lastWindSet=setwned;
radius_to_vehicle = sea_level_radius = fdmex->GetInertial()->GetRefRadius();
- terrain_altitude = 0;
+ terrain_elevation = 0;
targetNlfIC = 1.0;
void FGInitialCondition::WriteStateFile(int num)
{
- string filename = fdmex->GetFullAircraftPath() + "/" + "initfile.xml";
+ 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();
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->Geth() << " </altitude>" << 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." << endl;
+ cerr << "Could not open and/or write the state to the initial conditions file: " << filename << endl;
}
-
- outfile.close();
}
//******************************************************************************
//******************************************************************************
-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;
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)
{
- int n;
-
string sep = "/";
-# ifdef macintosh
- sep = ";";
-# endif
-
if( useStoredPath ) {
init_file_name = fdmex->GetFullAircraftPath() + sep + rstfile + ".xml";
} else {
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("altitude"))
- SetAltitudeFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT"));
+ 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"))
//******************************************************************************
+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,
projects / flightgear.git / blobdiff