1 /*******************************************************************************
3 Header: FGInitialCondition.cpp
7 ------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
9 This program is free software; you can redistribute it and/or modify it under
10 the terms of the GNU Lesser General Public License as published by the Free Software
11 Foundation; either version 2 of the License, or (at your option) any later
14 This program is distributed in the hope that it will be useful, but WITHOUT
15 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
19 You should have received a copy of the GNU Lesser General Public License along with
20 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21 Place - Suite 330, Boston, MA 02111-1307, USA.
23 Further information about the GNU Lesser General Public License can also be found on
24 the world wide web at http://www.gnu.org.
28 --------------------------------------------------------------------------------
32 FUNCTIONAL DESCRIPTION
33 --------------------------------------------------------------------------------
35 The purpose of this class is to take a set of initial conditions and provide
36 a kinematically consistent set of body axis velocity components, euler
37 angles, and altitude. This class does not attempt to trim the model i.e.
38 the sim will most likely start in a very dynamic state (unless, of course,
39 you have chosen your IC's wisely) even after setting it up with this class.
41 ********************************************************************************
43 *******************************************************************************/
45 #include "FGInitialCondition.h"
46 #include "FGFDMExec.h"
47 #include "models/FGInertial.h"
48 #include "models/FGAtmosphere.h"
49 #include "models/FGAerodynamics.h"
50 #include "models/FGPropagate.h"
51 #include "input_output/FGPropertyManager.h"
52 #include "input_output/FGXMLElement.h"
53 #include "models/FGPropulsion.h"
54 #include "input_output/FGXMLParse.h"
55 #include "math/FGQuaternion.h"
59 #include "input_output/string_utilities.h"
65 static const char *IdSrc = "$Id: FGInitialCondition.cpp,v 1.50 2010/11/20 16:38:43 bcoconni Exp $";
66 static const char *IdHdr = ID_INITIALCONDITION;
68 //******************************************************************************
70 FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) : fdmex(FDMExec)
74 if(FDMExec != NULL ) {
75 fdmex->GetPropagate()->SetAltitudeASL(altitudeASL);
76 fdmex->GetAtmosphere()->Run();
77 PropertyManager=fdmex->GetPropertyManager();
82 cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
88 //******************************************************************************
90 FGInitialCondition::~FGInitialCondition()
95 //******************************************************************************
97 void FGInitialCondition::ResetIC(double u0, double v0, double w0,
98 double p0, double q0, double r0,
99 double alpha0, double beta0,
100 double phi0, double theta0, double psi0,
101 double latRad0, double lonRad0, double altAGLFt0,
106 u = u0; v = v0; w = w0;
107 p = p0; q = q0; r = r0;
108 alpha = alpha0; beta = beta0;
109 phi = phi0; theta = theta0; psi = psi0;
114 SetAltitudeAGLFtIC(altAGLFt0);
116 cphi = cos(phi); sphi = sin(phi); // phi, rad
117 ctheta = cos(theta); stheta = sin(theta); // theta, rad
118 cpsi = cos(psi); spsi = sin(psi); // psi, rad
120 FGQuaternion Quat( phi, theta, psi );
123 // const FGMatrix33& _Tl2b = Quat.GetT(); // local to body frame
124 const FGMatrix33& _Tb2l = Quat.GetTInv(); // body to local
126 FGColumnVector3 _vUVW_BODY(u,v,w);
127 FGColumnVector3 _vUVW_NED = _Tb2l * _vUVW_BODY;
128 FGColumnVector3 _vWIND_NED(wnorth,weast,wdown);
129 // FGColumnVector3 _vUVWAero = _Tl2b * ( _vUVW_NED + _vWIND_NED );
131 uw=_vWIND_NED(1); vw=_vWIND_NED(2); ww=_vWIND_NED(3);
135 //******************************************************************************
137 void FGInitialCondition::InitializeIC(void)
144 latitude=longitude=0;
148 vnorth=veast=vdown=0;
149 wnorth=weast=wdown=0;
154 radius_to_vehicle = sea_level_radius = fdmex->GetInertial()->GetRefRadius();
155 terrain_elevation = 0;
159 salpha=sbeta=stheta=sphi=spsi=sgamma=0;
160 calpha=cbeta=ctheta=cphi=cpsi=cgamma=1;
163 //******************************************************************************
165 void FGInitialCondition::WriteStateFile(int num)
167 if (Constructing) return;
169 string filename = fdmex->GetFullAircraftPath();
171 if (filename.empty())
172 filename = "initfile.xml";
174 filename.append("/initfile.xml");
176 ofstream outfile(filename.c_str());
177 FGPropagate* Propagate = fdmex->GetPropagate();
179 if (outfile.is_open()) {
180 outfile << "<?xml version=\"1.0\"?>" << endl;
181 outfile << "<initialize name=\"reset00\">" << endl;
182 outfile << " <ubody unit=\"FT/SEC\"> " << Propagate->GetUVW(eX) << " </ubody> " << endl;
183 outfile << " <vbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eY) << " </vbody> " << endl;
184 outfile << " <wbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eZ) << " </wbody> " << endl;
185 outfile << " <phi unit=\"DEG\"> " << Propagate->GetEuler(ePhi) << " </phi>" << endl;
186 outfile << " <theta unit=\"DEG\"> " << Propagate->GetEuler(eTht) << " </theta>" << endl;
187 outfile << " <psi unit=\"DEG\"> " << Propagate->GetEuler(ePsi) << " </psi>" << endl;
188 outfile << " <longitude unit=\"DEG\"> " << Propagate->GetLongitudeDeg() << " </longitude>" << endl;
189 outfile << " <latitude unit=\"DEG\"> " << Propagate->GetLatitudeDeg() << " </latitude>" << endl;
190 outfile << " <altitude unit=\"FT\"> " << Propagate->GetDistanceAGL() << " </altitude>" << endl;
191 outfile << "</initialize>" << endl;
194 cerr << "Could not open and/or write the state to the initial conditions file: " << filename << endl;
198 //******************************************************************************
200 void FGInitialCondition::SetVcalibratedKtsIC(double tt) {
202 if(getMachFromVcas(&mach,tt*ktstofps)) {
203 //cout << "Mach: " << mach << endl;
206 vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
207 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
208 //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl;
211 cout << "Failed to get Mach number for given Vc and altitude, Vc unchanged." << endl;
212 cout << "Please mail the set of initial conditions used to apeden@earthlink.net" << endl;
216 //******************************************************************************
218 void FGInitialCondition::SetVequivalentKtsIC(double tt) {
221 vt=ve*1/sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
222 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
226 //******************************************************************************
228 void FGInitialCondition::calcAeroEuler(void)
233 vt = sqrt( ua*ua + va*va + wa*wa );
235 calpha = cbeta = 1.0;
236 salpha = sbeta = 0.0;
237 double vxz = sqrt( u*u + w*w );
238 if( w != 0 ) alpha = atan2( w, u );
240 beta = atan2( v, vxz );
244 double vn = sqrt(vxz*vxz + v*v);
251 //******************************************************************************
253 void FGInitialCondition::SetVgroundFpsIC(double tt) {
256 vnorth = vg*cos(psi); veast = vg*sin(psi); vdown = 0;
259 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
261 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
264 //******************************************************************************
266 void FGInitialCondition::SetVtrueFpsIC(double tt) {
269 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
271 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
274 //******************************************************************************
276 void FGInitialCondition::SetMachIC(double tt) {
278 lastSpeedSet=setmach;
279 vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
281 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
282 //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl;
285 //******************************************************************************
287 void FGInitialCondition::SetClimbRateFpmIC(double tt) {
288 SetClimbRateFpsIC(tt/60.0);
291 //******************************************************************************
293 void FGInitialCondition::SetClimbRateFpsIC(double tt) {
298 sgamma=sin(gamma); cgamma=cos(gamma);
302 //******************************************************************************
304 void FGInitialCondition::SetFlightPathAngleRadIC(double tt) {
306 sgamma=sin(gamma); cgamma=cos(gamma);
311 //******************************************************************************
313 void FGInitialCondition::SetAlphaRadIC(double tt) {
315 salpha=sin(alpha); calpha=cos(alpha);
319 //******************************************************************************
321 void FGInitialCondition::SetThetaRadIC(double tt) {
323 stheta=sin(theta); ctheta=cos(theta);
327 //******************************************************************************
329 void FGInitialCondition::SetBetaRadIC(double tt) {
331 sbeta=sin(beta); cbeta=cos(beta);
336 //******************************************************************************
338 void FGInitialCondition::SetPhiRadIC(double tt) {
340 sphi=sin(phi); cphi=cos(phi);
344 //******************************************************************************
346 void FGInitialCondition::SetPsiRadIC(double tt) {
348 spsi=sin(psi); cpsi=cos(psi);
352 //******************************************************************************
354 void FGInitialCondition::SetUBodyFpsIC(double tt) {
360 //******************************************************************************
362 void FGInitialCondition::SetVBodyFpsIC(double tt) {
368 //******************************************************************************
370 void FGInitialCondition::SetWBodyFpsIC(double tt) {
377 //******************************************************************************
379 void FGInitialCondition::SetVNorthFpsIC(double tt) {
383 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
385 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
389 //******************************************************************************
391 void FGInitialCondition::SetVEastFpsIC(double tt) {
395 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
397 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
401 //******************************************************************************
403 void FGInitialCondition::SetVDownFpsIC(double tt) {
407 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
409 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
410 SetClimbRateFpsIC(-1*vdown);
414 //******************************************************************************
416 double FGInitialCondition::GetUBodyFpsIC(void) const {
417 if (lastSpeedSet == setvg || lastSpeedSet == setned)
420 return vt*calpha*cbeta - uw;
423 //******************************************************************************
425 double FGInitialCondition::GetVBodyFpsIC(void) const {
426 if (lastSpeedSet == setvg || lastSpeedSet == setned)
429 return vt*sbeta - vw;
433 //******************************************************************************
435 double FGInitialCondition::GetWBodyFpsIC(void) const {
436 if (lastSpeedSet == setvg || lastSpeedSet == setned)
439 return vt*salpha*cbeta -ww;
442 //******************************************************************************
444 void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD ) {
445 wnorth = wN; weast = wE; wdown = wD;
446 lastWindSet = setwned;
448 if(lastSpeedSet == setvg)
452 //******************************************************************************
454 void FGInitialCondition::SetCrossWindKtsIC(double cross){
455 wcross=cross*ktstofps;
458 if(lastSpeedSet == setvg)
463 //******************************************************************************
465 // positive from left
466 void FGInitialCondition::SetHeadWindKtsIC(double head){
470 if(lastSpeedSet == setvg)
475 //******************************************************************************
477 void FGInitialCondition::SetWindDownKtsIC(double wD) {
480 if(lastSpeedSet == setvg)
484 //******************************************************************************
486 void FGInitialCondition::SetWindMagKtsIC(double mag) {
490 if(lastSpeedSet == setvg)
494 //******************************************************************************
496 void FGInitialCondition::SetWindDirDegIC(double dir) {
500 if(lastSpeedSet == setvg)
505 //******************************************************************************
507 void FGInitialCondition::calcWindUVW(void) {
509 switch(lastWindSet) {
511 wnorth=wmag*cos(wdir);
512 weast=wmag*sin(wdir);
515 wnorth=whead*cos(psi) + wcross*cos(psi+M_PI/2);
516 weast=whead*sin(psi) + wcross*sin(psi+M_PI/2);
521 uw=wnorth*ctheta*cpsi +
524 vw=wnorth*( sphi*stheta*cpsi - cphi*spsi ) +
525 weast*( sphi*stheta*spsi + cphi*cpsi ) +
527 ww=wnorth*(cphi*stheta*cpsi + sphi*spsi) +
528 weast*(cphi*stheta*spsi - sphi*cpsi) +
532 /* cout << "FGInitialCondition::calcWindUVW: wnorth, weast, wdown "
533 << wnorth << ", " << weast << ", " << wdown << endl;
534 cout << "FGInitialCondition::calcWindUVW: theta, phi, psi "
535 << theta << ", " << phi << ", " << psi << endl;
536 cout << "FGInitialCondition::calcWindUVW: uw, vw, ww "
537 << uw << ", " << vw << ", " << ww << endl; */
541 //******************************************************************************
543 void FGInitialCondition::SetAltitudeASLFtIC(double tt)
546 fdmex->GetPropagate()->SetAltitudeASL(altitudeASL);
547 fdmex->GetAtmosphere()->Run();
548 //lets try to make sure the user gets what they intended
550 switch(lastSpeedSet) {
554 SetVtrueKtsIC(vt*fpstokts);
557 SetVcalibratedKtsIC(vc*fpstokts);
560 SetVequivalentKtsIC(ve*fpstokts);
571 //******************************************************************************
573 void FGInitialCondition::SetAltitudeAGLFtIC(double tt)
575 SetAltitudeASLFtIC(terrain_elevation + tt);
578 //******************************************************************************
580 void FGInitialCondition::SetSeaLevelRadiusFtIC(double tt)
582 sea_level_radius = tt;
585 //******************************************************************************
587 void FGInitialCondition::SetTerrainElevationFtIC(double tt)
589 terrain_elevation=tt;
592 //******************************************************************************
594 void FGInitialCondition::calcUVWfromNED(void)
596 u=vnorth*ctheta*cpsi +
599 v=vnorth*( sphi*stheta*cpsi - cphi*spsi ) +
600 veast*( sphi*stheta*spsi + cphi*cpsi ) +
602 w=vnorth*( cphi*stheta*cpsi + sphi*spsi ) +
603 veast*( cphi*stheta*spsi - sphi*cpsi ) +
607 //******************************************************************************
609 bool FGInitialCondition::getMachFromVcas(double *Mach,double vcas) {
615 sfunc=&FGInitialCondition::calcVcas;
616 if(findInterval(vcas,guess)) {
617 if(solve(&mach,vcas))
623 //******************************************************************************
625 bool FGInitialCondition::getAlpha(void) {
627 double guess=theta-gamma;
629 if(vt < 0.01) return 0;
632 xmin=fdmex->GetAerodynamics()->GetAlphaCLMin();
633 xmax=fdmex->GetAerodynamics()->GetAlphaCLMax();
634 sfunc=&FGInitialCondition::GammaEqOfAlpha;
635 if(findInterval(0,guess)){
646 //******************************************************************************
648 bool FGInitialCondition::getTheta(void) {
650 double guess=alpha+gamma;
652 if(vt < 0.01) return 0;
656 sfunc=&FGInitialCondition::GammaEqOfTheta;
657 if(findInterval(0,guess)){
668 //******************************************************************************
670 double FGInitialCondition::GammaEqOfTheta(double Theta) {
672 double sTheta,cTheta;
674 //theta=Theta; stheta=sin(theta); ctheta=cos(theta);
675 sTheta=sin(Theta); cTheta=cos(Theta);
677 a=wdown + vt*calpha*cbeta + uw;
678 b=vt*sphi*sbeta + vw*sphi;
679 c=vt*cphi*salpha*cbeta + ww*cphi;
680 return vt*sgamma - ( a*sTheta - (b+c)*cTheta);
683 //******************************************************************************
685 double FGInitialCondition::GammaEqOfAlpha(double Alpha) {
687 double sAlpha,cAlpha;
688 sAlpha=sin(Alpha); cAlpha=cos(Alpha);
689 a=wdown + vt*cAlpha*cbeta + uw;
690 b=vt*sphi*sbeta + vw*sphi;
691 c=vt*cphi*sAlpha*cbeta + ww*cphi;
693 return vt*sgamma - ( a*stheta - (b+c)*ctheta );
696 //******************************************************************************
698 double FGInitialCondition::calcVcas(double Mach) {
700 double p=fdmex->GetAtmosphere()->GetPressure();
701 double psl=fdmex->GetAtmosphere()->GetPressureSL();
702 double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
703 double pt,A,B,D,vcas;
705 if (Mach < 0) Mach=0;
706 if (Mach < 1) //calculate total pressure assuming isentropic flow
707 pt=p*pow((1 + 0.2*Mach*Mach),3.5);
709 // shock in front of pitot tube, we'll assume its normal and use
710 // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
711 // pressure behind the shock to the static pressure in front of
712 // the normal shock assumption should not be a bad one -- most supersonic
713 // aircraft place the pitot probe out front so that it is the forward
714 // most point on the aircraft. The real shock would, of course, take
715 // on something like the shape of a rounded-off cone but, here again,
716 // the assumption should be good since the opening of the pitot probe
717 // is very small and, therefore, the effects of the shock curvature
718 // should be small as well. AFAIK, this approach is fairly well accepted
719 // within the aerospace community
721 B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);
723 // The denominator above is zero for Mach ~ 0.38, for which
724 // we'll never be here, so we're safe
726 D = (2.8*Mach*Mach-0.4)*0.4167;
730 A = pow(((pt-p)/psl+1),0.28571);
731 vcas = sqrt(7*psl/rhosl*(A-1));
732 //cout << "calcVcas: vcas= " << vcas*fpstokts << " mach= " << Mach << " pressure: " << pt << endl;
736 //******************************************************************************
738 bool FGInitialCondition::findInterval(double x,double guess) {
739 //void find_interval(inter_params &ip,eqfunc f,double y,double constant, int &flag){
743 double flo,fhi,fguess;
746 fguess=(this->*sfunc)(guess)-x;
752 if(lo < xmin) lo=xmin;
753 if(hi > xmax) hi=xmax;
755 flo=(this->*sfunc)(lo)-x;
756 fhi=(this->*sfunc)(hi)-x;
757 if(flo*fhi <=0) { //found interval with root
759 if(flo*fguess <= 0) { //narrow interval down a bit
760 hi=lo+step; //to pass solver interval that is as
763 else if(fhi*fguess <= 0) {
767 //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
769 while((found == 0) && (i <= 100));
775 //******************************************************************************
777 bool FGInitialCondition::solve(double *y,double x)
779 double x1,x2,x3,f1,f2,f3,d,d0;
781 double const relax =0.9;
789 f1=(this->*sfunc)(x1)-x;
790 f3=(this->*sfunc)(x3)-x;
795 while ((fabs(d) > eps) && (i < 100)) {
797 x2 = x1-d*d0*f1/(f3-f1);
799 f2=(this->*sfunc)(x2)-x;
800 //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
801 //cout << " " << f1 << "," << f2 << "," << f3 << endl;
803 if(fabs(f2) <= 0.001) {
805 } else if(f1*f2 <= 0.0) {
809 } else if(f2*f3 <= 0) {
822 //cout << "Success= " << success << " Vcas: " << vcas*fpstokts << " Mach: " << x2 << endl;
826 //******************************************************************************
828 double FGInitialCondition::GetWindDirDegIC(void) const {
830 return atan2(weast,wnorth)*radtodeg;
837 //******************************************************************************
839 bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
842 if( useStoredPath ) {
843 init_file_name = fdmex->GetFullAircraftPath() + sep + rstfile + ".xml";
845 init_file_name = rstfile;
848 document = LoadXMLDocument(init_file_name);
850 // Make sure that the document is valid
852 cerr << "File: " << init_file_name << " could not be read." << endl;
856 if (document->GetName() != string("initialize")) {
857 cerr << "File: " << init_file_name << " is not a reset file." << endl;
861 double version = document->GetAttributeValueAsNumber("version");
864 if (version == HUGE_VAL) {
865 result = Load_v1(); // Default to the old version
866 } else if (version >= 3.0) {
867 cerr << "Only initialization file formats 1 and 2 are currently supported" << endl;
869 } else if (version >= 2.0) {
871 } else if (version >= 1.0) {
875 fdmex->GetPropagate()->DumpState();
880 //******************************************************************************
882 bool FGInitialCondition::Load_v1(void)
887 if (document->FindElement("latitude"))
888 SetLatitudeDegIC(document->FindElementValueAsNumberConvertTo("latitude", "DEG"));
889 if (document->FindElement("longitude"))
890 SetLongitudeDegIC(document->FindElementValueAsNumberConvertTo("longitude", "DEG"));
891 if (document->FindElement("elevation"))
892 SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT"));
894 if (document->FindElement("altitude")) // This is feet above ground level
895 SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT"));
896 else if (document->FindElement("altitudeAGL")) // This is feet above ground level
897 SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
898 else if (document->FindElement("altitudeMSL")) // This is feet above sea level
899 SetAltitudeASLFtIC(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
901 if (document->FindElement("ubody"))
902 SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo("ubody", "FT/SEC"));
903 if (document->FindElement("vbody"))
904 SetVBodyFpsIC(document->FindElementValueAsNumberConvertTo("vbody", "FT/SEC"));
905 if (document->FindElement("wbody"))
906 SetWBodyFpsIC(document->FindElementValueAsNumberConvertTo("wbody", "FT/SEC"));
907 if (document->FindElement("vnorth"))
908 SetVNorthFpsIC(document->FindElementValueAsNumberConvertTo("vnorth", "FT/SEC"));
909 if (document->FindElement("veast"))
910 SetVEastFpsIC(document->FindElementValueAsNumberConvertTo("veast", "FT/SEC"));
911 if (document->FindElement("vdown"))
912 SetVDownFpsIC(document->FindElementValueAsNumberConvertTo("vdown", "FT/SEC"));
913 if (document->FindElement("winddir"))
914 SetWindDirDegIC(document->FindElementValueAsNumberConvertTo("winddir", "DEG"));
915 if (document->FindElement("vwind"))
916 SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo("vwind", "KTS"));
917 if (document->FindElement("hwind"))
918 SetHeadWindKtsIC(document->FindElementValueAsNumberConvertTo("hwind", "KTS"));
919 if (document->FindElement("xwind"))
920 SetCrossWindKtsIC(document->FindElementValueAsNumberConvertTo("xwind", "KTS"));
921 if (document->FindElement("vc"))
922 SetVcalibratedKtsIC(document->FindElementValueAsNumberConvertTo("vc", "KTS"));
923 if (document->FindElement("vt"))
924 SetVtrueKtsIC(document->FindElementValueAsNumberConvertTo("vt", "KTS"));
925 if (document->FindElement("mach"))
926 SetMachIC(document->FindElementValueAsNumber("mach"));
927 if (document->FindElement("phi"))
928 SetPhiDegIC(document->FindElementValueAsNumberConvertTo("phi", "DEG"));
929 if (document->FindElement("theta"))
930 SetThetaDegIC(document->FindElementValueAsNumberConvertTo("theta", "DEG"));
931 if (document->FindElement("psi"))
932 SetPsiDegIC(document->FindElementValueAsNumberConvertTo("psi", "DEG"));
933 if (document->FindElement("alpha"))
934 SetAlphaDegIC(document->FindElementValueAsNumberConvertTo("alpha", "DEG"));
935 if (document->FindElement("beta"))
936 SetBetaDegIC(document->FindElementValueAsNumberConvertTo("beta", "DEG"));
937 if (document->FindElement("gamma"))
938 SetFlightPathAngleDegIC(document->FindElementValueAsNumberConvertTo("gamma", "DEG"));
939 if (document->FindElement("roc"))
940 SetClimbRateFpsIC(document->FindElementValueAsNumberConvertTo("roc", "FT/SEC"));
941 if (document->FindElement("vground"))
942 SetVgroundKtsIC(document->FindElementValueAsNumberConvertTo("vground", "KTS"));
943 if (document->FindElement("targetNlf"))
945 SetTargetNlfIC(document->FindElementValueAsNumber("targetNlf"));
948 // Check to see if any engines are specified to be initialized in a running state
949 FGPropulsion* propulsion = fdmex->GetPropulsion();
950 Element* running_elements = document->FindElement("running");
951 while (running_elements) {
952 n = int(running_elements->GetDataAsNumber());
954 propulsion->InitRunning(n);
955 } catch (string str) {
959 running_elements = document->FindNextElement("running");
967 //******************************************************************************
969 bool FGInitialCondition::Load_v2(void)
973 FGColumnVector3 vLoc, vOrient;
975 FGInertial* Inertial = fdmex->GetInertial();
976 FGPropagate* Propagate = fdmex->GetPropagate();
977 FGColumnVector3 vOmegaEarth = FGColumnVector3(0.0, 0.0, Inertial->omega());
979 if (document->FindElement("earth_position_angle")) {
980 epa = document->FindElementValueAsNumberConvertTo("earth_position_angle", "RAD");
982 Inertial->SetEarthPositionAngle(epa);
983 Propagate->GetVState()->vLocation.SetEarthPositionAngle(epa);
985 Propagate->SetSeaLevelRadius(GetSeaLevelRadiusFtIC());
987 if (document->FindElement("elevation")) {
988 SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT"));
989 Propagate->SetTerrainElevation(terrain_elevation);
992 // Initialize vehicle position
995 // - ECI (Earth Centered Inertial)
996 // - ECEF (Earth Centered, Earth Fixed)
998 Element* position = document->FindElement("position");
1000 string frame = position->GetAttributeValue("frame");
1001 frame = to_lower(frame);
1002 if (frame == "eci") { // Need to transform vLoc to ECEF for storage and use in FGLocation.
1003 vLoc = position->FindElementTripletConvertTo("FT");
1004 vLoc = Propagate->GetTi2ec()*vLoc;
1005 Propagate->SetLocation(vLoc);
1006 } else if (frame == "ecef") {
1007 double AltitudeASL = 0.0;
1008 if (!position->FindElement("x") && !position->FindElement("y") && !position->FindElement("z")) {
1009 if (position->FindElement("radius")) {
1010 AltitudeASL = position->FindElementValueAsNumberConvertTo("radius", "FT") - sea_level_radius;
1011 } else if (position->FindElement("altitudeAGL")) {
1012 AltitudeASL = terrain_elevation + position->FindElementValueAsNumberConvertTo("altitudeAGL", "FT");
1013 } else if (position->FindElement("altitudeMSL")) {
1014 AltitudeASL = position->FindElementValueAsNumberConvertTo("altitudeMSL", "FT");
1016 cerr << endl << " No altitude or radius initial condition is given." << endl;
1020 double long_rad = 0.0;
1021 if (position->FindElement("longitude"))
1022 long_rad = position->FindElementValueAsNumberConvertTo("longitude", "RAD");
1023 if (position->FindElement("latitude"))
1024 lat_rad = position->FindElementValueAsNumberConvertTo("latitude", "RAD");
1025 Propagate->SetPosition(long_rad, lat_rad, AltitudeASL + GetSeaLevelRadiusFtIC());
1027 vLoc = position->FindElementTripletConvertTo("FT");
1028 Propagate->SetLocation(vLoc);
1031 cerr << endl << " Neither ECI nor ECEF frame is specified for initial position." << endl;
1035 cerr << endl << " Initial position not specified in this initialization file." << endl;
1039 // End of position initialization
1041 // Initialize vehicle orientation
1043 // - ECI (Earth Centered Inertial)
1044 // - ECEF (Earth Centered, Earth Fixed)
1047 // Need to convert the provided orientation to an ECI orientation, using
1048 // the given orientation and knowledge of the Earth position angle.
1049 // This could be done using matrices (where in the subscript "b/a",
1050 // it is meant "b with respect to a", and where b=body frame,
1051 // i=inertial frame, and e=ecef frame) as:
1053 // C_b/i = C_b/e * C_e/i
1055 // Using quaternions (note reverse ordering compared to matrix representation):
1057 // Q_b/i = Q_e/i * Q_b/e
1059 // Use the specific matrices as needed. The above example of course is for the whole
1060 // body to inertial orientation.
1061 // The new orientation angles can be extracted from the matrix or the quaternion.
1062 // ToDo: Do we need to deal with normalization of the quaternions here?
1064 Element* orientation_el = document->FindElement("orientation");
1065 FGQuaternion QuatI2Body;
1066 if (orientation_el) {
1067 string frame = orientation_el->GetAttributeValue("frame");
1068 frame = to_lower(frame);
1069 vOrient = orientation_el->FindElementTripletConvertTo("RAD");
1070 if (frame == "eci") {
1072 QuatI2Body = FGQuaternion(vOrient);
1074 } else if (frame == "ecef") {
1076 // In this case we are given the Euler angles representing the orientation of
1077 // the body with respect to the ECEF system, represented by the C_b/e Matrix.
1078 // We want the body orientation with respect to the inertial system:
1080 // C_b/i = C_b/e * C_e/i
1082 // Using quaternions (note reverse ordering compared to matrix representation):
1084 // Q_b/i = Q_e/i * Q_b/e
1086 FGQuaternion QuatEC2Body(vOrient); // Store relationship of Body frame wrt ECEF frame, Q_b/e
1087 QuatEC2Body.Normalize();
1088 FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix
1089 QuatI2EC.Normalize();
1090 QuatI2Body = QuatI2EC * QuatEC2Body; // Q_b/i = Q_e/i * Q_b/e
1092 } else if (frame == "local") {
1094 // In this case, we are supplying the Euler angles for the vehicle with
1095 // respect to the local (NED frame), also called the navigation frame.
1096 // This is the most common way of initializing the orientation of
1097 // aircraft. The matrix representation is:
1099 // C_b/i = C_b/n * C_n/e * C_e/i
1101 // Or, using quaternions (note reverse ordering compared to matrix representation):
1103 // Q_b/i = Q_e/i * Q_n/e * Q_b/n
1105 FGQuaternion QuatLocal2Body = FGQuaternion(vOrient); // Store relationship of Body frame wrt local (NED) frame, Q_b/n
1106 QuatLocal2Body.Normalize();
1107 FGQuaternion QuatEC2Local = Propagate->GetTec2l(); // Get Q_n/e from matrix
1108 QuatEC2Local.Normalize();
1109 FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix
1110 QuatI2EC.Normalize();
1111 QuatI2Body = QuatI2EC * QuatEC2Local * QuatLocal2Body; // Q_b/i = Q_e/i * Q_n/e * Q_b/n
1115 cerr << endl << fgred << " Orientation frame type: \"" << frame
1116 << "\" is not supported!" << reset << endl << endl;
1122 QuatI2Body.Normalize();
1123 Propagate->SetInertialOrientation(QuatI2Body);
1125 // Initialize vehicle velocity
1127 // - ECI (Earth Centered Inertial)
1128 // - ECEF (Earth Centered, Earth Fixed)
1131 // The vehicle will be defaulted to (0,0,0) in the Body frame if nothing is provided.
1133 Element* velocity_el = document->FindElement("velocity");
1134 FGColumnVector3 vInertialVelocity;
1135 FGColumnVector3 vInitVelocity = FGColumnVector3(0.0, 0.0, 0.0);
1136 FGColumnVector3 omega_cross_r = vOmegaEarth * Propagate->GetInertialPosition();
1137 FGMatrix33 mTl2i = Propagate->GetTl2i();
1138 FGMatrix33 mTec2i = Propagate->GetTec2i(); // Get C_i/e
1139 FGMatrix33 mTb2i = Propagate->GetTb2i();
1142 string frame = velocity_el->GetAttributeValue("frame");
1143 frame = to_lower(frame);
1144 FGColumnVector3 vInitVelocity = velocity_el->FindElementTripletConvertTo("FT/SEC");
1146 if (frame == "eci") {
1147 vInertialVelocity = vInitVelocity;
1148 } else if (frame == "ecef") {
1149 vInertialVelocity = mTec2i * vInitVelocity + omega_cross_r;
1150 } else if (frame == "local") {
1151 vInertialVelocity = mTl2i * vInitVelocity + omega_cross_r;
1152 } else if (frame == "body") {
1153 vInertialVelocity = mTb2i * vInitVelocity + omega_cross_r;
1156 cerr << endl << fgred << " Velocity frame type: \"" << frame
1157 << "\" is not supported!" << reset << endl << endl;
1164 vInertialVelocity = mTb2i * vInitVelocity + omega_cross_r;
1168 Propagate->SetInertialVelocity(vInertialVelocity);
1170 // Initialize vehicle body rates
1172 // - ECI (Earth Centered Inertial)
1173 // - ECEF (Earth Centered, Earth Fixed)
1176 FGColumnVector3 vInertialRate;
1177 Element* attrate_el = document->FindElement("attitude_rate");
1178 double radInv = 1.0/Propagate->GetRadius();
1179 FGColumnVector3 vVel = Propagate->GetVel();
1180 FGColumnVector3 vOmegaLocal = FGColumnVector3(
1182 -radInv*vVel(eNorth),
1183 -radInv*vVel(eEast)*Propagate->GetLocation().GetTanLatitude() );
1187 string frame = attrate_el->GetAttributeValue("frame");
1188 frame = to_lower(frame);
1189 FGColumnVector3 vAttRate = attrate_el->FindElementTripletConvertTo("RAD/SEC");
1191 if (frame == "eci") {
1192 vInertialRate = vAttRate;
1193 } else if (frame == "ecef") {
1194 vInertialRate = vAttRate + vOmegaEarth;
1195 } else if (frame == "local") {
1196 vInertialRate = vOmegaEarth + Propagate->GetTl2i() * vOmegaLocal + Propagate->GetTb2i() * vAttRate;
1197 } else if (!frame.empty()) { // misspelling of frame
1199 cerr << endl << fgred << " Attitude rate frame type: \"" << frame
1200 << "\" is not supported!" << reset << endl << endl;
1203 } else if (frame.empty()) {
1207 } else { // Body frame attitude rate assumed 0 relative to local.
1208 vInertialRate = vOmegaEarth + Propagate->GetTl2i() * vOmegaLocal;
1210 Propagate->SetInertialRates(vInertialRate);
1211 Propagate->InitializeDerivatives();
1213 // Check to see if any engines are specified to be initialized in a running state
1214 FGPropulsion* propulsion = fdmex->GetPropulsion();
1215 Element* running_elements = document->FindElement("running");
1216 while (running_elements) {
1217 n = int(running_elements->GetDataAsNumber());
1219 propulsion->InitRunning(n);
1220 } catch (string str) {
1221 cerr << str << endl;
1224 running_elements = document->FindNextElement("running");
1227 fdmex->SuspendIntegration(); // saves the integration rate, dt, then sets it to 0.0.
1229 fdmex->ResumeIntegration(); // Restores the integration rate to what it was.
1234 //******************************************************************************
1236 void FGInitialCondition::bind(void){
1237 PropertyManager->Tie("ic/vc-kts", this,
1238 &FGInitialCondition::GetVcalibratedKtsIC,
1239 &FGInitialCondition::SetVcalibratedKtsIC,
1241 PropertyManager->Tie("ic/ve-kts", this,
1242 &FGInitialCondition::GetVequivalentKtsIC,
1243 &FGInitialCondition::SetVequivalentKtsIC,
1245 PropertyManager->Tie("ic/vg-kts", this,
1246 &FGInitialCondition::GetVgroundKtsIC,
1247 &FGInitialCondition::SetVgroundKtsIC,
1249 PropertyManager->Tie("ic/vt-kts", this,
1250 &FGInitialCondition::GetVtrueKtsIC,
1251 &FGInitialCondition::SetVtrueKtsIC,
1253 PropertyManager->Tie("ic/mach", this,
1254 &FGInitialCondition::GetMachIC,
1255 &FGInitialCondition::SetMachIC,
1257 PropertyManager->Tie("ic/roc-fpm", this,
1258 &FGInitialCondition::GetClimbRateFpmIC,
1259 &FGInitialCondition::SetClimbRateFpmIC,
1261 PropertyManager->Tie("ic/gamma-deg", this,
1262 &FGInitialCondition::GetFlightPathAngleDegIC,
1263 &FGInitialCondition::SetFlightPathAngleDegIC,
1265 PropertyManager->Tie("ic/alpha-deg", this,
1266 &FGInitialCondition::GetAlphaDegIC,
1267 &FGInitialCondition::SetAlphaDegIC,
1269 PropertyManager->Tie("ic/beta-deg", this,
1270 &FGInitialCondition::GetBetaDegIC,
1271 &FGInitialCondition::SetBetaDegIC,
1273 PropertyManager->Tie("ic/theta-deg", this,
1274 &FGInitialCondition::GetThetaDegIC,
1275 &FGInitialCondition::SetThetaDegIC,
1277 PropertyManager->Tie("ic/phi-deg", this,
1278 &FGInitialCondition::GetPhiDegIC,
1279 &FGInitialCondition::SetPhiDegIC,
1281 PropertyManager->Tie("ic/psi-true-deg", this,
1282 &FGInitialCondition::GetPsiDegIC );
1283 PropertyManager->Tie("ic/lat-gc-deg", this,
1284 &FGInitialCondition::GetLatitudeDegIC,
1285 &FGInitialCondition::SetLatitudeDegIC,
1287 PropertyManager->Tie("ic/long-gc-deg", this,
1288 &FGInitialCondition::GetLongitudeDegIC,
1289 &FGInitialCondition::SetLongitudeDegIC,
1291 PropertyManager->Tie("ic/h-sl-ft", this,
1292 &FGInitialCondition::GetAltitudeASLFtIC,
1293 &FGInitialCondition::SetAltitudeASLFtIC,
1295 PropertyManager->Tie("ic/h-agl-ft", this,
1296 &FGInitialCondition::GetAltitudeAGLFtIC,
1297 &FGInitialCondition::SetAltitudeAGLFtIC,
1299 PropertyManager->Tie("ic/sea-level-radius-ft", this,
1300 &FGInitialCondition::GetSeaLevelRadiusFtIC,
1301 &FGInitialCondition::SetSeaLevelRadiusFtIC,
1303 PropertyManager->Tie("ic/terrain-elevation-ft", this,
1304 &FGInitialCondition::GetTerrainElevationFtIC,
1305 &FGInitialCondition::SetTerrainElevationFtIC,
1307 PropertyManager->Tie("ic/vg-fps", this,
1308 &FGInitialCondition::GetVgroundFpsIC,
1309 &FGInitialCondition::SetVgroundFpsIC,
1311 PropertyManager->Tie("ic/vt-fps", this,
1312 &FGInitialCondition::GetVtrueFpsIC,
1313 &FGInitialCondition::SetVtrueFpsIC,
1315 PropertyManager->Tie("ic/vw-bx-fps", this,
1316 &FGInitialCondition::GetWindUFpsIC);
1317 PropertyManager->Tie("ic/vw-by-fps", this,
1318 &FGInitialCondition::GetWindVFpsIC);
1319 PropertyManager->Tie("ic/vw-bz-fps", this,
1320 &FGInitialCondition::GetWindWFpsIC);
1321 PropertyManager->Tie("ic/vw-north-fps", this,
1322 &FGInitialCondition::GetWindNFpsIC);
1323 PropertyManager->Tie("ic/vw-east-fps", this,
1324 &FGInitialCondition::GetWindEFpsIC);
1325 PropertyManager->Tie("ic/vw-down-fps", this,
1326 &FGInitialCondition::GetWindDFpsIC);
1327 PropertyManager->Tie("ic/vw-mag-fps", this,
1328 &FGInitialCondition::GetWindFpsIC);
1329 PropertyManager->Tie("ic/vw-dir-deg", this,
1330 &FGInitialCondition::GetWindDirDegIC,
1331 &FGInitialCondition::SetWindDirDegIC,
1334 PropertyManager->Tie("ic/roc-fps", this,
1335 &FGInitialCondition::GetClimbRateFpsIC,
1336 &FGInitialCondition::SetClimbRateFpsIC,
1338 PropertyManager->Tie("ic/u-fps", this,
1339 &FGInitialCondition::GetUBodyFpsIC,
1340 &FGInitialCondition::SetUBodyFpsIC,
1342 PropertyManager->Tie("ic/v-fps", this,
1343 &FGInitialCondition::GetVBodyFpsIC,
1344 &FGInitialCondition::SetVBodyFpsIC,
1346 PropertyManager->Tie("ic/w-fps", this,
1347 &FGInitialCondition::GetWBodyFpsIC,
1348 &FGInitialCondition::SetWBodyFpsIC,
1350 PropertyManager->Tie("ic/vn-fps", this,
1351 &FGInitialCondition::GetVNorthFpsIC,
1352 &FGInitialCondition::SetVNorthFpsIC,
1354 PropertyManager->Tie("ic/ve-fps", this,
1355 &FGInitialCondition::GetVEastFpsIC,
1356 &FGInitialCondition::SetVEastFpsIC,
1358 PropertyManager->Tie("ic/vd-fps", this,
1359 &FGInitialCondition::GetVDownFpsIC,
1360 &FGInitialCondition::SetVDownFpsIC,
1362 PropertyManager->Tie("ic/gamma-rad", this,
1363 &FGInitialCondition::GetFlightPathAngleRadIC,
1364 &FGInitialCondition::SetFlightPathAngleRadIC,
1366 PropertyManager->Tie("ic/alpha-rad", this,
1367 &FGInitialCondition::GetAlphaRadIC,
1368 &FGInitialCondition::SetAlphaRadIC,
1370 PropertyManager->Tie("ic/theta-rad", this,
1371 &FGInitialCondition::GetThetaRadIC,
1372 &FGInitialCondition::SetThetaRadIC,
1374 PropertyManager->Tie("ic/beta-rad", this,
1375 &FGInitialCondition::GetBetaRadIC,
1376 &FGInitialCondition::SetBetaRadIC,
1378 PropertyManager->Tie("ic/phi-rad", this,
1379 &FGInitialCondition::GetPhiRadIC,
1380 &FGInitialCondition::SetPhiRadIC,
1382 PropertyManager->Tie("ic/psi-true-rad", this,
1383 &FGInitialCondition::GetPsiRadIC);
1384 PropertyManager->Tie("ic/lat-gc-rad", this,
1385 &FGInitialCondition::GetLatitudeRadIC,
1386 &FGInitialCondition::SetLatitudeRadIC,
1388 PropertyManager->Tie("ic/long-gc-rad", this,
1389 &FGInitialCondition::GetLongitudeRadIC,
1390 &FGInitialCondition::SetLongitudeRadIC,
1392 PropertyManager->Tie("ic/p-rad_sec", this,
1393 &FGInitialCondition::GetPRadpsIC,
1394 &FGInitialCondition::SetPRadpsIC,
1396 PropertyManager->Tie("ic/q-rad_sec", this,
1397 &FGInitialCondition::GetQRadpsIC,
1398 &FGInitialCondition::SetQRadpsIC,
1400 PropertyManager->Tie("ic/r-rad_sec", this,
1401 &FGInitialCondition::GetRRadpsIC,
1402 &FGInitialCondition::SetRRadpsIC,
1405 typedef int (FGInitialCondition::*iPMF)(void) const;
1406 PropertyManager->Tie("simulation/write-state-file",
1409 &FGInitialCondition::WriteStateFile);
1413 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
1414 // The bitmasked value choices are as follows:
1415 // unset: In this case (the default) JSBSim would only print
1416 // out the normally expected messages, essentially echoing
1417 // the config files as they are read. If the environment
1418 // variable is not set, debug_lvl is set to 1 internally
1419 // 0: This requests JSBSim not to output any messages
1421 // 1: This value explicity requests the normal JSBSim
1423 // 2: This value asks for a message to be printed out when
1424 // a class is instantiated
1425 // 4: When this value is set, a message is displayed when a
1426 // FGModel object executes its Run() method
1427 // 8: When this value is set, various runtime state variables
1428 // are printed out periodically
1429 // 16: When set various parameters are sanity checked and
1430 // a message is printed out when they go out of bounds
1432 void FGInitialCondition::Debug(int from)
1434 if (debug_lvl <= 0) return;
1436 if (debug_lvl & 1) { // Standard console startup message output
1438 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
1439 if (from == 0) cout << "Instantiated: FGInitialCondition" << endl;
1440 if (from == 1) cout << "Destroyed: FGInitialCondition" << endl;
1442 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
1444 if (debug_lvl & 8 ) { // Runtime state variables
1446 if (debug_lvl & 16) { // Sanity checking
1448 if (debug_lvl & 64) {
1449 if (from == 0) { // Constructor
1450 cout << IdSrc << endl;
1451 cout << IdHdr << endl;