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.46 2010/09/29 02:19:05 jberndt 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->GetAltitudeASL() << " </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::SetVgroundFpsIC(double tt) {
234 vnorth = vg*cos(psi); veast = vg*sin(psi); vdown = 0;
236 ua = u + uw; va = v + vw; wa = w + ww;
237 vt = sqrt( ua*ua + va*va + wa*wa );
239 vxz = sqrt( u*u + w*w );
240 if( w != 0 ) alpha = atan2( w, u );
241 if( vxz != 0 ) beta = atan2( v, vxz );
242 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
244 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
247 //******************************************************************************
249 void FGInitialCondition::SetVtrueFpsIC(double tt) {
252 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
254 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
257 //******************************************************************************
259 void FGInitialCondition::SetMachIC(double tt) {
261 lastSpeedSet=setmach;
262 vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
264 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
265 //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl;
268 //******************************************************************************
270 void FGInitialCondition::SetClimbRateFpmIC(double tt) {
271 SetClimbRateFpsIC(tt/60.0);
274 //******************************************************************************
276 void FGInitialCondition::SetClimbRateFpsIC(double tt) {
281 sgamma=sin(gamma); cgamma=cos(gamma);
285 //******************************************************************************
287 void FGInitialCondition::SetFlightPathAngleRadIC(double tt) {
289 sgamma=sin(gamma); cgamma=cos(gamma);
294 //******************************************************************************
296 void FGInitialCondition::SetAlphaRadIC(double tt) {
298 salpha=sin(alpha); calpha=cos(alpha);
302 //******************************************************************************
304 void FGInitialCondition::SetThetaRadIC(double tt) {
306 stheta=sin(theta); ctheta=cos(theta);
310 //******************************************************************************
312 void FGInitialCondition::SetBetaRadIC(double tt) {
314 sbeta=sin(beta); cbeta=cos(beta);
319 //******************************************************************************
321 void FGInitialCondition::SetPhiRadIC(double tt) {
323 sphi=sin(phi); cphi=cos(phi);
327 //******************************************************************************
329 void FGInitialCondition::SetPsiRadIC(double tt) {
331 spsi=sin(psi); cpsi=cos(psi);
335 //******************************************************************************
337 void FGInitialCondition::SetUBodyFpsIC(double tt) {
339 vt=sqrt(u*u + v*v + w*w);
343 //******************************************************************************
345 void FGInitialCondition::SetVBodyFpsIC(double tt) {
347 vt=sqrt(u*u + v*v + w*w);
351 //******************************************************************************
353 void FGInitialCondition::SetWBodyFpsIC(double tt) {
355 vt=sqrt( u*u + v*v + w*w );
360 //******************************************************************************
362 void FGInitialCondition::SetVNorthFpsIC(double tt) {
367 ua = u + uw; va = v + vw; wa = w + ww;
368 vt = sqrt( ua*ua + va*va + wa*wa );
370 vxz = sqrt( u*u + w*w );
371 if( w != 0 ) alpha = atan2( w, u );
372 if( vxz != 0 ) beta = atan2( v, vxz );
373 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
375 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
379 //******************************************************************************
381 void FGInitialCondition::SetVEastFpsIC(double tt) {
386 ua = u + uw; va = v + vw; wa = w + ww;
387 vt = sqrt( ua*ua + va*va + wa*wa );
389 vxz = sqrt( u*u + w*w );
390 if( w != 0 ) alpha = atan2( w, u );
391 if( vxz != 0 ) beta = atan2( v, vxz );
392 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
394 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
398 //******************************************************************************
400 void FGInitialCondition::SetVDownFpsIC(double tt) {
405 ua = u + uw; va = v + vw; wa = w + ww;
406 vt = sqrt( ua*ua + va*va + wa*wa );
408 vxz = sqrt( u*u + w*w );
409 if( w != 0 ) alpha = atan2( w, u );
410 if( vxz != 0 ) beta = atan2( v, vxz );
411 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
413 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
414 SetClimbRateFpsIC(-1*vdown);
418 //******************************************************************************
420 double FGInitialCondition::GetUBodyFpsIC(void) const {
421 if (lastSpeedSet == setvg || lastSpeedSet == setned)
424 return vt*calpha*cbeta - uw;
427 //******************************************************************************
429 double FGInitialCondition::GetVBodyFpsIC(void) const {
430 if (lastSpeedSet == setvg || lastSpeedSet == setned)
433 return vt*sbeta - vw;
437 //******************************************************************************
439 double FGInitialCondition::GetWBodyFpsIC(void) const {
440 if (lastSpeedSet == setvg || lastSpeedSet == setned)
443 return vt*salpha*cbeta -ww;
446 //******************************************************************************
448 void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD ) {
449 wnorth = wN; weast = wE; wdown = wD;
450 lastWindSet = setwned;
452 if(lastSpeedSet == setvg)
456 //******************************************************************************
458 void FGInitialCondition::SetCrossWindKtsIC(double cross){
459 wcross=cross*ktstofps;
462 if(lastSpeedSet == setvg)
467 //******************************************************************************
469 // positive from left
470 void FGInitialCondition::SetHeadWindKtsIC(double head){
474 if(lastSpeedSet == setvg)
479 //******************************************************************************
481 void FGInitialCondition::SetWindDownKtsIC(double wD) {
484 if(lastSpeedSet == setvg)
488 //******************************************************************************
490 void FGInitialCondition::SetWindMagKtsIC(double mag) {
494 if(lastSpeedSet == setvg)
498 //******************************************************************************
500 void FGInitialCondition::SetWindDirDegIC(double dir) {
504 if(lastSpeedSet == setvg)
509 //******************************************************************************
511 void FGInitialCondition::calcWindUVW(void) {
513 switch(lastWindSet) {
515 wnorth=wmag*cos(wdir);
516 weast=wmag*sin(wdir);
519 wnorth=whead*cos(psi) + wcross*cos(psi+M_PI/2);
520 weast=whead*sin(psi) + wcross*sin(psi+M_PI/2);
525 uw=wnorth*ctheta*cpsi +
528 vw=wnorth*( sphi*stheta*cpsi - cphi*spsi ) +
529 weast*( sphi*stheta*spsi + cphi*cpsi ) +
531 ww=wnorth*(cphi*stheta*cpsi + sphi*spsi) +
532 weast*(cphi*stheta*spsi - sphi*cpsi) +
536 /* cout << "FGInitialCondition::calcWindUVW: wnorth, weast, wdown "
537 << wnorth << ", " << weast << ", " << wdown << endl;
538 cout << "FGInitialCondition::calcWindUVW: theta, phi, psi "
539 << theta << ", " << phi << ", " << psi << endl;
540 cout << "FGInitialCondition::calcWindUVW: uw, vw, ww "
541 << uw << ", " << vw << ", " << ww << endl; */
545 //******************************************************************************
547 void FGInitialCondition::SetAltitudeASLFtIC(double tt)
550 fdmex->GetPropagate()->SetAltitudeASL(altitudeASL);
551 fdmex->GetAtmosphere()->Run();
552 //lets try to make sure the user gets what they intended
554 switch(lastSpeedSet) {
558 SetVtrueKtsIC(vt*fpstokts);
561 SetVcalibratedKtsIC(vc*fpstokts);
564 SetVequivalentKtsIC(ve*fpstokts);
575 //******************************************************************************
577 void FGInitialCondition::SetAltitudeAGLFtIC(double tt)
579 SetAltitudeASLFtIC(terrain_elevation + tt);
582 //******************************************************************************
584 void FGInitialCondition::SetSeaLevelRadiusFtIC(double tt)
586 sea_level_radius = tt;
589 //******************************************************************************
591 void FGInitialCondition::SetTerrainElevationFtIC(double tt)
593 terrain_elevation=tt;
596 //******************************************************************************
598 void FGInitialCondition::calcUVWfromNED(void)
600 u=vnorth*ctheta*cpsi +
603 v=vnorth*( sphi*stheta*cpsi - cphi*spsi ) +
604 veast*( sphi*stheta*spsi + cphi*cpsi ) +
606 w=vnorth*( cphi*stheta*cpsi + sphi*spsi ) +
607 veast*( cphi*stheta*spsi - sphi*cpsi ) +
611 //******************************************************************************
613 bool FGInitialCondition::getMachFromVcas(double *Mach,double vcas) {
619 sfunc=&FGInitialCondition::calcVcas;
620 if(findInterval(vcas,guess)) {
621 if(solve(&mach,vcas))
627 //******************************************************************************
629 bool FGInitialCondition::getAlpha(void) {
631 double guess=theta-gamma;
633 if(vt < 0.01) return 0;
636 xmin=fdmex->GetAerodynamics()->GetAlphaCLMin();
637 xmax=fdmex->GetAerodynamics()->GetAlphaCLMax();
638 sfunc=&FGInitialCondition::GammaEqOfAlpha;
639 if(findInterval(0,guess)){
650 //******************************************************************************
652 bool FGInitialCondition::getTheta(void) {
654 double guess=alpha+gamma;
656 if(vt < 0.01) return 0;
660 sfunc=&FGInitialCondition::GammaEqOfTheta;
661 if(findInterval(0,guess)){
672 //******************************************************************************
674 double FGInitialCondition::GammaEqOfTheta(double Theta) {
676 double sTheta,cTheta;
678 //theta=Theta; stheta=sin(theta); ctheta=cos(theta);
679 sTheta=sin(Theta); cTheta=cos(Theta);
681 a=wdown + vt*calpha*cbeta + uw;
682 b=vt*sphi*sbeta + vw*sphi;
683 c=vt*cphi*salpha*cbeta + ww*cphi;
684 return vt*sgamma - ( a*sTheta - (b+c)*cTheta);
687 //******************************************************************************
689 double FGInitialCondition::GammaEqOfAlpha(double Alpha) {
691 double sAlpha,cAlpha;
692 sAlpha=sin(Alpha); cAlpha=cos(Alpha);
693 a=wdown + vt*cAlpha*cbeta + uw;
694 b=vt*sphi*sbeta + vw*sphi;
695 c=vt*cphi*sAlpha*cbeta + ww*cphi;
697 return vt*sgamma - ( a*stheta - (b+c)*ctheta );
700 //******************************************************************************
702 double FGInitialCondition::calcVcas(double Mach) {
704 double p=fdmex->GetAtmosphere()->GetPressure();
705 double psl=fdmex->GetAtmosphere()->GetPressureSL();
706 double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
707 double pt,A,B,D,vcas;
709 if (Mach < 0) Mach=0;
710 if (Mach < 1) //calculate total pressure assuming isentropic flow
711 pt=p*pow((1 + 0.2*Mach*Mach),3.5);
713 // shock in front of pitot tube, we'll assume its normal and use
714 // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
715 // pressure behind the shock to the static pressure in front of
716 // the normal shock assumption should not be a bad one -- most supersonic
717 // aircraft place the pitot probe out front so that it is the forward
718 // most point on the aircraft. The real shock would, of course, take
719 // on something like the shape of a rounded-off cone but, here again,
720 // the assumption should be good since the opening of the pitot probe
721 // is very small and, therefore, the effects of the shock curvature
722 // should be small as well. AFAIK, this approach is fairly well accepted
723 // within the aerospace community
725 B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);
727 // The denominator above is zero for Mach ~ 0.38, for which
728 // we'll never be here, so we're safe
730 D = (2.8*Mach*Mach-0.4)*0.4167;
734 A = pow(((pt-p)/psl+1),0.28571);
735 vcas = sqrt(7*psl/rhosl*(A-1));
736 //cout << "calcVcas: vcas= " << vcas*fpstokts << " mach= " << Mach << " pressure: " << pt << endl;
740 //******************************************************************************
742 bool FGInitialCondition::findInterval(double x,double guess) {
743 //void find_interval(inter_params &ip,eqfunc f,double y,double constant, int &flag){
747 double flo,fhi,fguess;
750 fguess=(this->*sfunc)(guess)-x;
756 if(lo < xmin) lo=xmin;
757 if(hi > xmax) hi=xmax;
759 flo=(this->*sfunc)(lo)-x;
760 fhi=(this->*sfunc)(hi)-x;
761 if(flo*fhi <=0) { //found interval with root
763 if(flo*fguess <= 0) { //narrow interval down a bit
764 hi=lo+step; //to pass solver interval that is as
767 else if(fhi*fguess <= 0) {
771 //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
773 while((found == 0) && (i <= 100));
779 //******************************************************************************
781 bool FGInitialCondition::solve(double *y,double x)
783 double x1,x2,x3,f1,f2,f3,d,d0;
785 double const relax =0.9;
793 f1=(this->*sfunc)(x1)-x;
794 f3=(this->*sfunc)(x3)-x;
799 while ((fabs(d) > eps) && (i < 100)) {
801 x2 = x1-d*d0*f1/(f3-f1);
803 f2=(this->*sfunc)(x2)-x;
804 //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
805 //cout << " " << f1 << "," << f2 << "," << f3 << endl;
807 if(fabs(f2) <= 0.001) {
809 } else if(f1*f2 <= 0.0) {
813 } else if(f2*f3 <= 0) {
826 //cout << "Success= " << success << " Vcas: " << vcas*fpstokts << " Mach: " << x2 << endl;
830 //******************************************************************************
832 double FGInitialCondition::GetWindDirDegIC(void) const {
834 return atan2(weast,wnorth)*radtodeg;
841 //******************************************************************************
843 bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
846 if( useStoredPath ) {
847 init_file_name = fdmex->GetFullAircraftPath() + sep + rstfile + ".xml";
849 init_file_name = rstfile;
852 document = LoadXMLDocument(init_file_name);
854 // Make sure that the document is valid
856 cerr << "File: " << init_file_name << " could not be read." << endl;
860 if (document->GetName() != string("initialize")) {
861 cerr << "File: " << init_file_name << " is not a reset file." << endl;
865 double version = document->GetAttributeValueAsNumber("version");
868 if (version == HUGE_VAL) {
869 result = Load_v1(); // Default to the old version
870 } else if (version >= 3.0) {
871 cerr << "Only initialization file formats 1 and 2 are currently supported" << endl;
873 } else if (version >= 2.0) {
875 } else if (version >= 1.0) {
879 fdmex->GetPropagate()->DumpState();
884 //******************************************************************************
886 bool FGInitialCondition::Load_v1(void)
890 if (document->FindElement("latitude"))
891 SetLatitudeDegIC(document->FindElementValueAsNumberConvertTo("latitude", "DEG"));
892 if (document->FindElement("longitude"))
893 SetLongitudeDegIC(document->FindElementValueAsNumberConvertTo("longitude", "DEG"));
894 if (document->FindElement("elevation"))
895 SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT"));
897 if (document->FindElement("altitude")) // This is feet above ground level
898 SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT"));
899 else if (document->FindElement("altitudeAGL")) // This is feet above ground level
900 SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
901 else if (document->FindElement("altitudeMSL")) // This is feet above sea level
902 SetAltitudeASLFtIC(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
904 if (document->FindElement("ubody"))
905 SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo("ubody", "FT/SEC"));
906 if (document->FindElement("vbody"))
907 SetVBodyFpsIC(document->FindElementValueAsNumberConvertTo("vbody", "FT/SEC"));
908 if (document->FindElement("wbody"))
909 SetWBodyFpsIC(document->FindElementValueAsNumberConvertTo("wbody", "FT/SEC"));
910 if (document->FindElement("vnorth"))
911 SetVNorthFpsIC(document->FindElementValueAsNumberConvertTo("vnorth", "FT/SEC"));
912 if (document->FindElement("veast"))
913 SetVEastFpsIC(document->FindElementValueAsNumberConvertTo("veast", "FT/SEC"));
914 if (document->FindElement("vdown"))
915 SetVDownFpsIC(document->FindElementValueAsNumberConvertTo("vdown", "FT/SEC"));
916 if (document->FindElement("winddir"))
917 SetWindDirDegIC(document->FindElementValueAsNumberConvertTo("winddir", "DEG"));
918 if (document->FindElement("vwind"))
919 SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo("vwind", "KTS"));
920 if (document->FindElement("hwind"))
921 SetHeadWindKtsIC(document->FindElementValueAsNumberConvertTo("hwind", "KTS"));
922 if (document->FindElement("xwind"))
923 SetCrossWindKtsIC(document->FindElementValueAsNumberConvertTo("xwind", "KTS"));
924 if (document->FindElement("vc"))
925 SetVcalibratedKtsIC(document->FindElementValueAsNumberConvertTo("vc", "KTS"));
926 if (document->FindElement("vt"))
927 SetVtrueKtsIC(document->FindElementValueAsNumberConvertTo("vt", "KTS"));
928 if (document->FindElement("mach"))
929 SetMachIC(document->FindElementValueAsNumber("mach"));
930 if (document->FindElement("phi"))
931 SetPhiDegIC(document->FindElementValueAsNumberConvertTo("phi", "DEG"));
932 if (document->FindElement("theta"))
933 SetThetaDegIC(document->FindElementValueAsNumberConvertTo("theta", "DEG"));
934 if (document->FindElement("psi"))
935 SetPsiDegIC(document->FindElementValueAsNumberConvertTo("psi", "DEG"));
936 if (document->FindElement("alpha"))
937 SetAlphaDegIC(document->FindElementValueAsNumberConvertTo("alpha", "DEG"));
938 if (document->FindElement("beta"))
939 SetBetaDegIC(document->FindElementValueAsNumberConvertTo("beta", "DEG"));
940 if (document->FindElement("gamma"))
941 SetFlightPathAngleDegIC(document->FindElementValueAsNumberConvertTo("gamma", "DEG"));
942 if (document->FindElement("roc"))
943 SetClimbRateFpsIC(document->FindElementValueAsNumberConvertTo("roc", "FT/SEC"));
944 if (document->FindElement("vground"))
945 SetVgroundKtsIC(document->FindElementValueAsNumberConvertTo("vground", "KTS"));
946 if (document->FindElement("targetNlf"))
948 SetTargetNlfIC(document->FindElementValueAsNumber("targetNlf"));
951 // Check to see if any engines are specified to be initialized in a running state
952 FGPropulsion* propulsion = fdmex->GetPropulsion();
953 Element* running_elements = document->FindElement("running");
954 while (running_elements) {
955 n = int(running_elements->GetDataAsNumber());
956 propulsion->InitRunning(n);
957 running_elements = document->FindNextElement("running");
965 //******************************************************************************
967 bool FGInitialCondition::Load_v2(void)
971 FGColumnVector3 vLoc, vOrient;
973 FGInertial* Inertial = fdmex->GetInertial();
974 FGPropagate* Propagate = fdmex->GetPropagate();
975 FGColumnVector3 vOmegaEarth = FGColumnVector3(0.0, 0.0, Inertial->omega());
977 if (document->FindElement("earth_position_angle")) {
978 epa = document->FindElementValueAsNumberConvertTo("earth_position_angle", "RAD");
980 Inertial->SetEarthPositionAngle(epa);
981 Propagate->GetVState()->vLocation.SetEarthPositionAngle(epa);
983 Propagate->SetSeaLevelRadius(GetSeaLevelRadiusFtIC());
985 if (document->FindElement("elevation")) {
986 SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT"));
987 Propagate->SetTerrainElevation(terrain_elevation);
990 // Initialize vehicle position
993 // - ECI (Earth Centered Inertial)
994 // - ECEF (Earth Centered, Earth Fixed)
996 Element* position = document->FindElement("position");
998 string frame = position->GetAttributeValue("frame");
999 frame = to_lower(frame);
1000 if (frame == "eci") { // Need to transform vLoc to ECEF for storage and use in FGLocation.
1001 vLoc = position->FindElementTripletConvertTo("FT");
1002 vLoc = Propagate->GetTi2ec()*vLoc;
1003 Propagate->SetLocation(vLoc);
1004 } else if (frame == "ecef") {
1005 double AltitudeASL = 0.0;
1006 if (!position->FindElement("x") && !position->FindElement("y") && !position->FindElement("z")) {
1007 if (position->FindElement("radius")) {
1008 AltitudeASL = position->FindElementValueAsNumberConvertTo("radius", "FT") - sea_level_radius;
1009 } else if (position->FindElement("altitudeAGL")) {
1010 AltitudeASL = terrain_elevation + position->FindElementValueAsNumberConvertTo("altitudeAGL", "FT");
1011 } else if (position->FindElement("altitudeMSL")) {
1012 AltitudeASL = position->FindElementValueAsNumberConvertTo("altitudeMSL", "FT");
1014 cerr << endl << " No altitude or radius initial condition is given." << endl;
1018 double long_rad = 0.0;
1019 if (position->FindElement("longitude"))
1020 long_rad = position->FindElementValueAsNumberConvertTo("longitude", "RAD");
1021 if (position->FindElement("latitude"))
1022 lat_rad = position->FindElementValueAsNumberConvertTo("latitude", "RAD");
1023 Propagate->SetPosition(long_rad, lat_rad, AltitudeASL + GetSeaLevelRadiusFtIC());
1025 vLoc = position->FindElementTripletConvertTo("FT");
1026 Propagate->SetLocation(vLoc);
1029 cerr << endl << " Neither ECI nor ECEF frame is specified for initial position." << endl;
1033 cerr << endl << " Initial position not specified in this initialization file." << endl;
1037 // End of position initialization
1039 // Initialize vehicle orientation
1041 // - ECI (Earth Centered Inertial)
1042 // - ECEF (Earth Centered, Earth Fixed)
1045 // Need to convert the provided orientation to an ECI orientation, using
1046 // the given orientation and knowledge of the Earth position angle.
1047 // This could be done using matrices (where in the subscript "b/a",
1048 // it is meant "b with respect to a", and where b=body frame,
1049 // i=inertial frame, and e=ecef frame) as:
1051 // C_b/i = C_b/e * C_e/i
1053 // Using quaternions (note reverse ordering compared to matrix representation):
1055 // Q_b/i = Q_e/i * Q_b/e
1057 // Use the specific matrices as needed. The above example of course is for the whole
1058 // body to inertial orientation.
1059 // The new orientation angles can be extracted from the matrix or the quaternion.
1060 // ToDo: Do we need to deal with normalization of the quaternions here?
1062 Element* orientation_el = document->FindElement("orientation");
1063 FGQuaternion QuatI2Body;
1064 if (orientation_el) {
1065 string frame = orientation_el->GetAttributeValue("frame");
1066 frame = to_lower(frame);
1067 vOrient = orientation_el->FindElementTripletConvertTo("RAD");
1068 if (frame == "eci") {
1070 QuatI2Body = FGQuaternion(vOrient);
1072 } else if (frame == "ecef") {
1074 // In this case we are given the Euler angles representing the orientation of
1075 // the body with respect to the ECEF system, represented by the C_b/e Matrix.
1076 // We want the body orientation with respect to the inertial system:
1078 // C_b/i = C_b/e * C_e/i
1080 // Using quaternions (note reverse ordering compared to matrix representation):
1082 // Q_b/i = Q_e/i * Q_b/e
1084 FGQuaternion QuatEC2Body(vOrient); // Store relationship of Body frame wrt ECEF frame, Q_b/e
1085 QuatEC2Body.Normalize();
1086 FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix
1087 QuatI2EC.Normalize();
1088 QuatI2Body = QuatI2EC * QuatEC2Body; // Q_b/i = Q_e/i * Q_b/e
1090 } else if (frame == "local") {
1092 // In this case, we are supplying the Euler angles for the vehicle with
1093 // respect to the local (NED frame), also called the navigation frame.
1094 // This is the most common way of initializing the orientation of
1095 // aircraft. The matrix representation is:
1097 // C_b/i = C_b/n * C_n/e * C_e/i
1099 // Or, using quaternions (note reverse ordering compared to matrix representation):
1101 // Q_b/i = Q_e/i * Q_n/e * Q_b/n
1103 FGQuaternion QuatLocal2Body = FGQuaternion(vOrient); // Store relationship of Body frame wrt local (NED) frame, Q_b/n
1104 QuatLocal2Body.Normalize();
1105 FGQuaternion QuatEC2Local = Propagate->GetTec2l(); // Get Q_n/e from matrix
1106 QuatEC2Local.Normalize();
1107 FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix
1108 QuatI2EC.Normalize();
1109 QuatI2Body = QuatI2EC * QuatEC2Local * QuatLocal2Body; // Q_b/i = Q_e/i * Q_n/e * Q_b/n
1113 cerr << endl << fgred << " Orientation frame type: \"" << frame
1114 << "\" is not supported!" << reset << endl << endl;
1120 QuatI2Body.Normalize();
1121 Propagate->SetInertialOrientation(QuatI2Body);
1123 // Initialize vehicle velocity
1125 // - ECI (Earth Centered Inertial)
1126 // - ECEF (Earth Centered, Earth Fixed)
1129 // The vehicle will be defaulted to (0,0,0) in the Body frame if nothing is provided.
1131 Element* velocity_el = document->FindElement("velocity");
1132 FGColumnVector3 vInertialVelocity;
1133 FGColumnVector3 vInitVelocity = FGColumnVector3(0.0, 0.0, 0.0);
1134 FGColumnVector3 omega_cross_r = vOmegaEarth * Propagate->GetInertialPosition();
1135 FGMatrix33 mTl2i = Propagate->GetTl2i();
1136 FGMatrix33 mTec2i = Propagate->GetTec2i(); // Get C_i/e
1137 FGMatrix33 mTb2i = Propagate->GetTb2i();
1140 string frame = velocity_el->GetAttributeValue("frame");
1141 frame = to_lower(frame);
1142 FGColumnVector3 vInitVelocity = velocity_el->FindElementTripletConvertTo("FT/SEC");
1144 if (frame == "eci") {
1145 vInertialVelocity = vInitVelocity;
1146 } else if (frame == "ecef") {
1147 vInertialVelocity = mTec2i * vInitVelocity + omega_cross_r;
1148 } else if (frame == "local") {
1149 vInertialVelocity = mTl2i * vInitVelocity + omega_cross_r;
1150 } else if (frame == "body") {
1151 vInertialVelocity = mTb2i * vInitVelocity + omega_cross_r;
1154 cerr << endl << fgred << " Velocity frame type: \"" << frame
1155 << "\" is not supported!" << reset << endl << endl;
1162 vInertialVelocity = mTb2i * vInitVelocity + omega_cross_r;
1166 Propagate->SetInertialVelocity(vInertialVelocity);
1168 // Initialize vehicle body rates
1170 // - ECI (Earth Centered Inertial)
1171 // - ECEF (Earth Centered, Earth Fixed)
1174 FGColumnVector3 vInertialRate;
1175 Element* attrate_el = document->FindElement("attitude_rate");
1176 double radInv = 1.0/Propagate->GetRadius();
1177 FGColumnVector3 vVel = Propagate->GetVel();
1178 FGColumnVector3 vOmegaLocal = FGColumnVector3(
1180 -radInv*vVel(eNorth),
1181 -radInv*vVel(eEast)*Propagate->GetLocation().GetTanLatitude() );
1185 string frame = attrate_el->GetAttributeValue("frame");
1186 frame = to_lower(frame);
1187 FGColumnVector3 vAttRate = attrate_el->FindElementTripletConvertTo("RAD/SEC");
1189 if (frame == "eci") {
1190 vInertialRate = vAttRate;
1191 } else if (frame == "ecef") {
1192 vInertialRate = vAttRate + vOmegaEarth;
1193 } else if (frame == "local") {
1194 vInertialRate = vOmegaEarth + Propagate->GetTl2i() * vOmegaLocal + Propagate->GetTb2i() * vAttRate;
1195 } else if (!frame.empty()) { // misspelling of frame
1197 cerr << endl << fgred << " Attitude rate frame type: \"" << frame
1198 << "\" is not supported!" << reset << endl << endl;
1201 } else if (frame.empty()) {
1205 } else { // Body frame attitude rate assumed 0 relative to local.
1206 vInertialRate = vOmegaEarth + Propagate->GetTl2i() * vOmegaLocal;
1208 Propagate->SetInertialRates(vInertialRate);
1209 Propagate->InitializeDerivatives();
1211 // Check to see if any engines are specified to be initialized in a running state
1212 FGPropulsion* propulsion = fdmex->GetPropulsion();
1213 Element* running_elements = document->FindElement("running");
1214 while (running_elements) {
1215 n = int(running_elements->GetDataAsNumber());
1216 propulsion->InitRunning(n);
1217 running_elements = document->FindNextElement("running");
1220 fdmex->SuspendIntegration(); // saves the integration rate, dt, then sets it to 0.0.
1222 fdmex->ResumeIntegration(); // Restores the integration rate to what it was.
1227 //******************************************************************************
1229 void FGInitialCondition::bind(void){
1230 PropertyManager->Tie("ic/vc-kts", this,
1231 &FGInitialCondition::GetVcalibratedKtsIC,
1232 &FGInitialCondition::SetVcalibratedKtsIC,
1234 PropertyManager->Tie("ic/ve-kts", this,
1235 &FGInitialCondition::GetVequivalentKtsIC,
1236 &FGInitialCondition::SetVequivalentKtsIC,
1238 PropertyManager->Tie("ic/vg-kts", this,
1239 &FGInitialCondition::GetVgroundKtsIC,
1240 &FGInitialCondition::SetVgroundKtsIC,
1242 PropertyManager->Tie("ic/vt-kts", this,
1243 &FGInitialCondition::GetVtrueKtsIC,
1244 &FGInitialCondition::SetVtrueKtsIC,
1246 PropertyManager->Tie("ic/mach", this,
1247 &FGInitialCondition::GetMachIC,
1248 &FGInitialCondition::SetMachIC,
1250 PropertyManager->Tie("ic/roc-fpm", this,
1251 &FGInitialCondition::GetClimbRateFpmIC,
1252 &FGInitialCondition::SetClimbRateFpmIC,
1254 PropertyManager->Tie("ic/gamma-deg", this,
1255 &FGInitialCondition::GetFlightPathAngleDegIC,
1256 &FGInitialCondition::SetFlightPathAngleDegIC,
1258 PropertyManager->Tie("ic/alpha-deg", this,
1259 &FGInitialCondition::GetAlphaDegIC,
1260 &FGInitialCondition::SetAlphaDegIC,
1262 PropertyManager->Tie("ic/beta-deg", this,
1263 &FGInitialCondition::GetBetaDegIC,
1264 &FGInitialCondition::SetBetaDegIC,
1266 PropertyManager->Tie("ic/theta-deg", this,
1267 &FGInitialCondition::GetThetaDegIC,
1268 &FGInitialCondition::SetThetaDegIC,
1270 PropertyManager->Tie("ic/phi-deg", this,
1271 &FGInitialCondition::GetPhiDegIC,
1272 &FGInitialCondition::SetPhiDegIC,
1274 PropertyManager->Tie("ic/psi-true-deg", this,
1275 &FGInitialCondition::GetPsiDegIC );
1276 PropertyManager->Tie("ic/lat-gc-deg", this,
1277 &FGInitialCondition::GetLatitudeDegIC,
1278 &FGInitialCondition::SetLatitudeDegIC,
1280 PropertyManager->Tie("ic/long-gc-deg", this,
1281 &FGInitialCondition::GetLongitudeDegIC,
1282 &FGInitialCondition::SetLongitudeDegIC,
1284 PropertyManager->Tie("ic/h-sl-ft", this,
1285 &FGInitialCondition::GetAltitudeASLFtIC,
1286 &FGInitialCondition::SetAltitudeASLFtIC,
1288 PropertyManager->Tie("ic/h-agl-ft", this,
1289 &FGInitialCondition::GetAltitudeAGLFtIC,
1290 &FGInitialCondition::SetAltitudeAGLFtIC,
1292 PropertyManager->Tie("ic/sea-level-radius-ft", this,
1293 &FGInitialCondition::GetSeaLevelRadiusFtIC,
1294 &FGInitialCondition::SetSeaLevelRadiusFtIC,
1296 PropertyManager->Tie("ic/terrain-elevation-ft", this,
1297 &FGInitialCondition::GetTerrainElevationFtIC,
1298 &FGInitialCondition::SetTerrainElevationFtIC,
1300 PropertyManager->Tie("ic/vg-fps", this,
1301 &FGInitialCondition::GetVgroundFpsIC,
1302 &FGInitialCondition::SetVgroundFpsIC,
1304 PropertyManager->Tie("ic/vt-fps", this,
1305 &FGInitialCondition::GetVtrueFpsIC,
1306 &FGInitialCondition::SetVtrueFpsIC,
1308 PropertyManager->Tie("ic/vw-bx-fps", this,
1309 &FGInitialCondition::GetWindUFpsIC);
1310 PropertyManager->Tie("ic/vw-by-fps", this,
1311 &FGInitialCondition::GetWindVFpsIC);
1312 PropertyManager->Tie("ic/vw-bz-fps", this,
1313 &FGInitialCondition::GetWindWFpsIC);
1314 PropertyManager->Tie("ic/vw-north-fps", this,
1315 &FGInitialCondition::GetWindNFpsIC);
1316 PropertyManager->Tie("ic/vw-east-fps", this,
1317 &FGInitialCondition::GetWindEFpsIC);
1318 PropertyManager->Tie("ic/vw-down-fps", this,
1319 &FGInitialCondition::GetWindDFpsIC);
1320 PropertyManager->Tie("ic/vw-mag-fps", this,
1321 &FGInitialCondition::GetWindFpsIC);
1322 PropertyManager->Tie("ic/vw-dir-deg", this,
1323 &FGInitialCondition::GetWindDirDegIC,
1324 &FGInitialCondition::SetWindDirDegIC,
1327 PropertyManager->Tie("ic/roc-fps", this,
1328 &FGInitialCondition::GetClimbRateFpsIC,
1329 &FGInitialCondition::SetClimbRateFpsIC,
1331 PropertyManager->Tie("ic/u-fps", this,
1332 &FGInitialCondition::GetUBodyFpsIC,
1333 &FGInitialCondition::SetUBodyFpsIC,
1335 PropertyManager->Tie("ic/v-fps", this,
1336 &FGInitialCondition::GetVBodyFpsIC,
1337 &FGInitialCondition::SetVBodyFpsIC,
1339 PropertyManager->Tie("ic/w-fps", this,
1340 &FGInitialCondition::GetWBodyFpsIC,
1341 &FGInitialCondition::SetWBodyFpsIC,
1343 PropertyManager->Tie("ic/vn-fps", this,
1344 &FGInitialCondition::GetVNorthFpsIC,
1345 &FGInitialCondition::SetVNorthFpsIC,
1347 PropertyManager->Tie("ic/ve-fps", this,
1348 &FGInitialCondition::GetVEastFpsIC,
1349 &FGInitialCondition::SetVEastFpsIC,
1351 PropertyManager->Tie("ic/vd-fps", this,
1352 &FGInitialCondition::GetVDownFpsIC,
1353 &FGInitialCondition::SetVDownFpsIC,
1355 PropertyManager->Tie("ic/gamma-rad", this,
1356 &FGInitialCondition::GetFlightPathAngleRadIC,
1357 &FGInitialCondition::SetFlightPathAngleRadIC,
1359 PropertyManager->Tie("ic/alpha-rad", this,
1360 &FGInitialCondition::GetAlphaRadIC,
1361 &FGInitialCondition::SetAlphaRadIC,
1363 PropertyManager->Tie("ic/theta-rad", this,
1364 &FGInitialCondition::GetThetaRadIC,
1365 &FGInitialCondition::SetThetaRadIC,
1367 PropertyManager->Tie("ic/beta-rad", this,
1368 &FGInitialCondition::GetBetaRadIC,
1369 &FGInitialCondition::SetBetaRadIC,
1371 PropertyManager->Tie("ic/phi-rad", this,
1372 &FGInitialCondition::GetPhiRadIC,
1373 &FGInitialCondition::SetPhiRadIC,
1375 PropertyManager->Tie("ic/psi-true-rad", this,
1376 &FGInitialCondition::GetPsiRadIC);
1377 PropertyManager->Tie("ic/lat-gc-rad", this,
1378 &FGInitialCondition::GetLatitudeRadIC,
1379 &FGInitialCondition::SetLatitudeRadIC,
1381 PropertyManager->Tie("ic/long-gc-rad", this,
1382 &FGInitialCondition::GetLongitudeRadIC,
1383 &FGInitialCondition::SetLongitudeRadIC,
1385 PropertyManager->Tie("ic/p-rad_sec", this,
1386 &FGInitialCondition::GetPRadpsIC,
1387 &FGInitialCondition::SetPRadpsIC,
1389 PropertyManager->Tie("ic/q-rad_sec", this,
1390 &FGInitialCondition::GetQRadpsIC,
1391 &FGInitialCondition::SetQRadpsIC,
1393 PropertyManager->Tie("ic/r-rad_sec", this,
1394 &FGInitialCondition::GetRRadpsIC,
1395 &FGInitialCondition::SetRRadpsIC,
1398 typedef int (FGInitialCondition::*iPMF)(void) const;
1399 PropertyManager->Tie("simulation/write-state-file",
1402 &FGInitialCondition::WriteStateFile);
1406 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
1407 // The bitmasked value choices are as follows:
1408 // unset: In this case (the default) JSBSim would only print
1409 // out the normally expected messages, essentially echoing
1410 // the config files as they are read. If the environment
1411 // variable is not set, debug_lvl is set to 1 internally
1412 // 0: This requests JSBSim not to output any messages
1414 // 1: This value explicity requests the normal JSBSim
1416 // 2: This value asks for a message to be printed out when
1417 // a class is instantiated
1418 // 4: When this value is set, a message is displayed when a
1419 // FGModel object executes its Run() method
1420 // 8: When this value is set, various runtime state variables
1421 // are printed out periodically
1422 // 16: When set various parameters are sanity checked and
1423 // a message is printed out when they go out of bounds
1425 void FGInitialCondition::Debug(int from)
1427 if (debug_lvl <= 0) return;
1429 if (debug_lvl & 1) { // Standard console startup message output
1431 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
1432 if (from == 0) cout << "Instantiated: FGInitialCondition" << endl;
1433 if (from == 1) cout << "Destroyed: FGInitialCondition" << endl;
1435 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
1437 if (debug_lvl & 8 ) { // Runtime state variables
1439 if (debug_lvl & 16) { // Sanity checking
1441 if (debug_lvl & 64) {
1442 if (from == 0) { // Constructor
1443 cout << IdSrc << endl;
1444 cout << IdHdr << endl;