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.40 2010/06/02 04:05:13 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");
866 if (version == HUGE_VAL) {
867 return Load_v1(); // Default to the old version
868 } else if (version >= 3.0) {
869 cerr << "Only initialization file formats 1 and 2 are currently supported" << endl;
871 } else if (version >= 2.0) {
873 } else if (version >= 1.0) {
879 //******************************************************************************
881 bool FGInitialCondition::Load_v1(void)
885 if (document->FindElement("latitude"))
886 SetLatitudeDegIC(document->FindElementValueAsNumberConvertTo("latitude", "DEG"));
887 if (document->FindElement("longitude"))
888 SetLongitudeDegIC(document->FindElementValueAsNumberConvertTo("longitude", "DEG"));
889 if (document->FindElement("elevation"))
890 SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT"));
892 if (document->FindElement("altitude")) // This is feet above ground level
893 SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT"));
894 else if (document->FindElement("altitudeAGL")) // This is feet above ground level
895 SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
896 else if (document->FindElement("altitudeMSL")) // This is feet above sea level
897 SetAltitudeASLFtIC(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
899 if (document->FindElement("ubody"))
900 SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo("ubody", "FT/SEC"));
901 if (document->FindElement("vbody"))
902 SetVBodyFpsIC(document->FindElementValueAsNumberConvertTo("vbody", "FT/SEC"));
903 if (document->FindElement("wbody"))
904 SetWBodyFpsIC(document->FindElementValueAsNumberConvertTo("wbody", "FT/SEC"));
905 if (document->FindElement("vnorth"))
906 SetVNorthFpsIC(document->FindElementValueAsNumberConvertTo("vnorth", "FT/SEC"));
907 if (document->FindElement("veast"))
908 SetVEastFpsIC(document->FindElementValueAsNumberConvertTo("veast", "FT/SEC"));
909 if (document->FindElement("vdown"))
910 SetVDownFpsIC(document->FindElementValueAsNumberConvertTo("vdown", "FT/SEC"));
911 if (document->FindElement("winddir"))
912 SetWindDirDegIC(document->FindElementValueAsNumberConvertTo("winddir", "DEG"));
913 if (document->FindElement("vwind"))
914 SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo("vwind", "KTS"));
915 if (document->FindElement("hwind"))
916 SetHeadWindKtsIC(document->FindElementValueAsNumberConvertTo("hwind", "KTS"));
917 if (document->FindElement("xwind"))
918 SetCrossWindKtsIC(document->FindElementValueAsNumberConvertTo("xwind", "KTS"));
919 if (document->FindElement("vc"))
920 SetVcalibratedKtsIC(document->FindElementValueAsNumberConvertTo("vc", "KTS"));
921 if (document->FindElement("vt"))
922 SetVtrueKtsIC(document->FindElementValueAsNumberConvertTo("vt", "KTS"));
923 if (document->FindElement("mach"))
924 SetMachIC(document->FindElementValueAsNumber("mach"));
925 if (document->FindElement("phi"))
926 SetPhiDegIC(document->FindElementValueAsNumberConvertTo("phi", "DEG"));
927 if (document->FindElement("theta"))
928 SetThetaDegIC(document->FindElementValueAsNumberConvertTo("theta", "DEG"));
929 if (document->FindElement("psi"))
930 SetPsiDegIC(document->FindElementValueAsNumberConvertTo("psi", "DEG"));
931 if (document->FindElement("alpha"))
932 SetAlphaDegIC(document->FindElementValueAsNumberConvertTo("alpha", "DEG"));
933 if (document->FindElement("beta"))
934 SetBetaDegIC(document->FindElementValueAsNumberConvertTo("beta", "DEG"));
935 if (document->FindElement("gamma"))
936 SetFlightPathAngleDegIC(document->FindElementValueAsNumberConvertTo("gamma", "DEG"));
937 if (document->FindElement("roc"))
938 SetClimbRateFpsIC(document->FindElementValueAsNumberConvertTo("roc", "FT/SEC"));
939 if (document->FindElement("vground"))
940 SetVgroundKtsIC(document->FindElementValueAsNumberConvertTo("vground", "KTS"));
941 if (document->FindElement("targetNlf"))
943 SetTargetNlfIC(document->FindElementValueAsNumber("targetNlf"));
946 // Check to see if any engines are specified to be initialized in a running state
947 FGPropulsion* propulsion = fdmex->GetPropulsion();
948 Element* running_elements = document->FindElement("running");
949 while (running_elements) {
950 n = int(running_elements->GetDataAsNumber());
951 propulsion->InitRunning(n);
952 running_elements = document->FindNextElement("running");
960 //******************************************************************************
962 bool FGInitialCondition::Load_v2(void)
965 FGColumnVector3 vLoc, vOrient;
967 FGInertial* Inertial = fdmex->GetInertial();
968 FGPropagate* Propagate = fdmex->GetPropagate();
970 if (document->FindElement("earth_position_angle")) {
971 double epa = document->FindElementValueAsNumberConvertTo("earth_position_angle", "RAD");
972 Inertial->SetEarthPositionAngle(epa);
975 // Initialize vehicle position
978 // - ECI (Earth Centered Inertial)
979 // - ECEF (Earth Centered, Earth Fixed)
981 Element* position = document->FindElement("position");
983 vLoc = position->FindElementTripletConvertTo("FT");
984 string frame = position->GetAttributeValue("frame");
985 frame = to_lower(frame);
986 if (frame == "eci") {
987 // Need to transform vLoc to ECEF for storage and use in FGLocation.
988 vLoc = Propagate->GetTi2ec()*vLoc;
989 } else if (frame == "ecef") {
990 // Move vLoc query until after lat/lon/alt query to eliminate spurious warning msgs.
991 if (vLoc.Magnitude() == 0.0) {
992 Propagate->SetLatitudeDeg(position->FindElementValueAsNumberConvertTo("latitude", "DEG"));
993 Propagate->SetLongitudeDeg(position->FindElementValueAsNumberConvertTo("longitude", "DEG"));
994 if (position->FindElement("radius")) {
995 radius_to_vehicle = position->FindElementValueAsNumberConvertTo("radius", "FT");
996 SetAltitudeASLFtIC(radius_to_vehicle - sea_level_radius);
997 } else if (position->FindElement("altitudeAGL")) {
998 SetAltitudeAGLFtIC(position->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
999 } else if (position->FindElement("altitudeMSL")) {
1000 SetAltitudeASLFtIC(position->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
1002 cerr << endl << " No altitude or radius initial condition is given." << endl;
1007 cerr << endl << " Neither ECI nor ECEF frame is specified for initial position." << endl;
1011 cerr << endl << " Initial position not specified in this initialization file." << endl;
1015 // End of position initialization
1017 // Initialize vehicle orientation
1019 // - ECI (Earth Centered Inertial)
1020 // - ECEF (Earth Centered, Earth Fixed)
1023 // Need to convert the provided orientation to an ECI orientation, using
1024 // the given orientation and knowledge of the Earth position angle.
1025 // This could be done using matrices (where in the subscript "b/a",
1026 // it is meant "b with respect to a", and where b=body frame,
1027 // i=inertial frame, and e=ecef frame) as:
1029 // C_b/i = C_b/e * C_e/i
1031 // Using quaternions (note reverse ordering compared to matrix representation):
1033 // Q_b/i = Q_e/i * Q_b/e
1035 // Use the specific matrices as needed. The above example of course is for the whole
1036 // body to inertial orientation.
1037 // The new orientation angles can be extracted from the matrix or the quaternion.
1038 // ToDo: Do we need to deal with normalization of the quaternions here?
1040 Element* orientation_el = document->FindElement("orientation");
1041 if (orientation_el) {
1042 string frame = orientation_el->GetAttributeValue("frame");
1043 frame = to_lower(frame);
1044 vOrient = orientation_el->FindElementTripletConvertTo("RAD");
1045 FGQuaternion QuatI2Body;
1046 if (frame == "eci") {
1048 QuatI2Body = FGQuaternion(vOrient);
1050 } else if (frame == "ecef") {
1052 // In this case we are given the Euler angles representing the orientation of
1053 // the body with respect to the ECEF system, represented by the C_b/e Matrix.
1054 // We want the body orientation with respect to the inertial system:
1056 // C_b/i = C_b/e * C_e/i
1058 // Using quaternions (note reverse ordering compared to matrix representation):
1060 // Q_b/i = Q_e/i * Q_b/e
1062 FGQuaternion QuatEC2Body(vOrient); // Store relationship of Body frame wrt ECEF frame, Q_b/e
1063 FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix
1064 QuatI2Body = QuatI2EC * QuatEC2Body; // Q_b/i = Q_e/i * Q_b/e
1066 } else if (frame == "local") {
1068 // In this case, we are supplying the Euler angles for the vehicle with
1069 // respect to the local (NED frame), also called the navigation frame.
1070 // This is the most common way of initializing the orientation of
1071 // aircraft. The matrix representation is:
1073 // C_b/i = C_b/n * C_n/e * C_e/i
1075 // Or, using quaternions (note reverse ordering compared to matrix representation):
1077 // Q_b/i = Q_e/i * Q_n/e * Q_b/n
1079 FGQuaternion QuatLocal2Body = FGQuaternion(vOrient); // Store relationship of Body frame wrt local (NED) frame, Q_b/n
1080 FGQuaternion QuatEC2Local = Propagate->GetTec2l(); // Get Q_n/e from matrix
1081 FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix
1082 QuatI2Body = QuatI2EC * QuatEC2Local * QuatLocal2Body; // Q_b/i = Q_e/i * Q_n/e * Q_b/n
1086 cerr << endl << fgred << " Orientation frame type: \"" << frame
1087 << "\" is not supported!" << reset << endl << endl;
1092 Propagate->SetInertialOrientation(QuatI2Body);
1095 // Initialize vehicle velocity
1097 // - ECI (Earth Centered Inertial)
1098 // - ECEF (Earth Centered, Earth Fixed)
1101 // The vehicle will be defaulted to (0,0,0) in the Body frame if nothing is provided.
1103 Element* velocity_el = document->FindElement("velocity");
1104 FGColumnVector3 vInertialVelocity;
1105 FGColumnVector3 vInitVelocity = FGColumnVector3(0.0, 0.0, 0.0);
1108 string frame = velocity_el->GetAttributeValue("frame");
1109 frame = to_lower(frame);
1110 FGColumnVector3 vInitVelocity = velocity_el->FindElementTripletConvertTo("FT/SEC");
1111 FGColumnVector3 omega_cross_r = Inertial->omega() * Propagate->GetInertialPosition();
1113 if (frame == "eci") {
1114 vInertialVelocity = vInitVelocity;
1115 } else if (frame == "ecef") {
1116 FGMatrix33 mTec2i = Propagate->GetTec2i(); // Get C_i/e
1117 vInertialVelocity = mTec2i * vInitVelocity + omega_cross_r;
1118 } else if (frame == "local") {
1119 FGMatrix33 mTl2i = Propagate->GetTl2i();
1120 vInertialVelocity = mTl2i * vInitVelocity + omega_cross_r;
1121 } else if (frame == "body") {
1122 FGMatrix33 mTb2i = Propagate->GetTb2i();
1123 vInertialVelocity = mTb2i * vInitVelocity + omega_cross_r;
1126 cerr << endl << fgred << " Velocity frame type: \"" << frame
1127 << "\" is not supported!" << reset << endl << endl;
1134 FGMatrix33 mTb2i = Propagate->GetTb2i();
1135 vInertialVelocity = mTb2i * vInitVelocity + (Inertial->omega() * Propagate->GetInertialPosition());
1139 Propagate->SetInertialVelocity(vInertialVelocity);
1142 // - ECI (Earth Centered Inertial)
1143 // - ECEF (Earth Centered, Earth Fixed)
1146 FGColumnVector3 vInertialRate;
1147 Element* attrate_el = document->FindElement("attitude_rate");
1150 string frame = attrate_el->GetAttributeValue("frame");
1151 frame = to_lower(frame);
1152 FGColumnVector3 vAttRate = attrate_el->FindElementTripletConvertTo("RAD/SEC");
1154 if (frame == "eci") {
1155 vInertialRate = vAttRate;
1156 } else if (frame == "ecef") {
1157 // vInertialRate = vAttRate + Inertial->omega();
1158 } else if (frame == "body") {
1159 //Todo: determine local frame rate
1160 FGMatrix33 mTb2l = Propagate->GetTb2l();
1161 // vInertialRate = mTb2l*vAttRate + Inertial->omega();
1162 } else if (!frame.empty()) { // misspelling of frame
1164 cerr << endl << fgred << " Attitude rate frame type: \"" << frame
1165 << "\" is not supported!" << reset << endl << endl;
1168 } else if (frame.empty()) {
1172 } else { // Body frame attitude rate assumed 0 relative to local.
1174 //Todo: determine local frame rate
1176 FGMatrix33 mTi2l = Propagate->GetTi2l();
1177 vVel = mTi2l * vInertialVelocity;
1179 // Compute the local frame ECEF velocity
1180 vVel = Tb2l * VState.vUVW;
1182 FGColumnVector3 vOmegaLocal = FGColumnVector3(
1184 -radInv*vVel(eNorth),
1185 -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
1189 // Check to see if any engines are specified to be initialized in a running state
1190 FGPropulsion* propulsion = fdmex->GetPropulsion();
1191 Element* running_elements = document->FindElement("running");
1192 while (running_elements) {
1193 n = int(running_elements->GetDataAsNumber());
1194 propulsion->InitRunning(n);
1195 running_elements = document->FindNextElement("running");
1203 //******************************************************************************
1205 void FGInitialCondition::bind(void){
1206 PropertyManager->Tie("ic/vc-kts", this,
1207 &FGInitialCondition::GetVcalibratedKtsIC,
1208 &FGInitialCondition::SetVcalibratedKtsIC,
1210 PropertyManager->Tie("ic/ve-kts", this,
1211 &FGInitialCondition::GetVequivalentKtsIC,
1212 &FGInitialCondition::SetVequivalentKtsIC,
1214 PropertyManager->Tie("ic/vg-kts", this,
1215 &FGInitialCondition::GetVgroundKtsIC,
1216 &FGInitialCondition::SetVgroundKtsIC,
1218 PropertyManager->Tie("ic/vt-kts", this,
1219 &FGInitialCondition::GetVtrueKtsIC,
1220 &FGInitialCondition::SetVtrueKtsIC,
1222 PropertyManager->Tie("ic/mach", this,
1223 &FGInitialCondition::GetMachIC,
1224 &FGInitialCondition::SetMachIC,
1226 PropertyManager->Tie("ic/roc-fpm", this,
1227 &FGInitialCondition::GetClimbRateFpmIC,
1228 &FGInitialCondition::SetClimbRateFpmIC,
1230 PropertyManager->Tie("ic/gamma-deg", this,
1231 &FGInitialCondition::GetFlightPathAngleDegIC,
1232 &FGInitialCondition::SetFlightPathAngleDegIC,
1234 PropertyManager->Tie("ic/alpha-deg", this,
1235 &FGInitialCondition::GetAlphaDegIC,
1236 &FGInitialCondition::SetAlphaDegIC,
1238 PropertyManager->Tie("ic/beta-deg", this,
1239 &FGInitialCondition::GetBetaDegIC,
1240 &FGInitialCondition::SetBetaDegIC,
1242 PropertyManager->Tie("ic/theta-deg", this,
1243 &FGInitialCondition::GetThetaDegIC,
1244 &FGInitialCondition::SetThetaDegIC,
1246 PropertyManager->Tie("ic/phi-deg", this,
1247 &FGInitialCondition::GetPhiDegIC,
1248 &FGInitialCondition::SetPhiDegIC,
1250 PropertyManager->Tie("ic/psi-true-deg", this,
1251 &FGInitialCondition::GetPsiDegIC );
1252 PropertyManager->Tie("ic/lat-gc-deg", this,
1253 &FGInitialCondition::GetLatitudeDegIC,
1254 &FGInitialCondition::SetLatitudeDegIC,
1256 PropertyManager->Tie("ic/long-gc-deg", this,
1257 &FGInitialCondition::GetLongitudeDegIC,
1258 &FGInitialCondition::SetLongitudeDegIC,
1260 PropertyManager->Tie("ic/h-sl-ft", this,
1261 &FGInitialCondition::GetAltitudeASLFtIC,
1262 &FGInitialCondition::SetAltitudeASLFtIC,
1264 PropertyManager->Tie("ic/h-agl-ft", this,
1265 &FGInitialCondition::GetAltitudeAGLFtIC,
1266 &FGInitialCondition::SetAltitudeAGLFtIC,
1268 PropertyManager->Tie("ic/sea-level-radius-ft", this,
1269 &FGInitialCondition::GetSeaLevelRadiusFtIC,
1270 &FGInitialCondition::SetSeaLevelRadiusFtIC,
1272 PropertyManager->Tie("ic/terrain-elevation-ft", this,
1273 &FGInitialCondition::GetTerrainElevationFtIC,
1274 &FGInitialCondition::SetTerrainElevationFtIC,
1276 PropertyManager->Tie("ic/vg-fps", this,
1277 &FGInitialCondition::GetVgroundFpsIC,
1278 &FGInitialCondition::SetVgroundFpsIC,
1280 PropertyManager->Tie("ic/vt-fps", this,
1281 &FGInitialCondition::GetVtrueFpsIC,
1282 &FGInitialCondition::SetVtrueFpsIC,
1284 PropertyManager->Tie("ic/vw-bx-fps", this,
1285 &FGInitialCondition::GetWindUFpsIC);
1286 PropertyManager->Tie("ic/vw-by-fps", this,
1287 &FGInitialCondition::GetWindVFpsIC);
1288 PropertyManager->Tie("ic/vw-bz-fps", this,
1289 &FGInitialCondition::GetWindWFpsIC);
1290 PropertyManager->Tie("ic/vw-north-fps", this,
1291 &FGInitialCondition::GetWindNFpsIC);
1292 PropertyManager->Tie("ic/vw-east-fps", this,
1293 &FGInitialCondition::GetWindEFpsIC);
1294 PropertyManager->Tie("ic/vw-down-fps", this,
1295 &FGInitialCondition::GetWindDFpsIC);
1296 PropertyManager->Tie("ic/vw-mag-fps", this,
1297 &FGInitialCondition::GetWindFpsIC);
1298 PropertyManager->Tie("ic/vw-dir-deg", this,
1299 &FGInitialCondition::GetWindDirDegIC,
1300 &FGInitialCondition::SetWindDirDegIC,
1303 PropertyManager->Tie("ic/roc-fps", this,
1304 &FGInitialCondition::GetClimbRateFpsIC,
1305 &FGInitialCondition::SetClimbRateFpsIC,
1307 PropertyManager->Tie("ic/u-fps", this,
1308 &FGInitialCondition::GetUBodyFpsIC,
1309 &FGInitialCondition::SetUBodyFpsIC,
1311 PropertyManager->Tie("ic/v-fps", this,
1312 &FGInitialCondition::GetVBodyFpsIC,
1313 &FGInitialCondition::SetVBodyFpsIC,
1315 PropertyManager->Tie("ic/w-fps", this,
1316 &FGInitialCondition::GetWBodyFpsIC,
1317 &FGInitialCondition::SetWBodyFpsIC,
1319 PropertyManager->Tie("ic/vn-fps", this,
1320 &FGInitialCondition::GetVNorthFpsIC,
1321 &FGInitialCondition::SetVNorthFpsIC,
1323 PropertyManager->Tie("ic/ve-fps", this,
1324 &FGInitialCondition::GetVEastFpsIC,
1325 &FGInitialCondition::SetVEastFpsIC,
1327 PropertyManager->Tie("ic/vd-fps", this,
1328 &FGInitialCondition::GetVDownFpsIC,
1329 &FGInitialCondition::SetVDownFpsIC,
1331 PropertyManager->Tie("ic/gamma-rad", this,
1332 &FGInitialCondition::GetFlightPathAngleRadIC,
1333 &FGInitialCondition::SetFlightPathAngleRadIC,
1335 PropertyManager->Tie("ic/alpha-rad", this,
1336 &FGInitialCondition::GetAlphaRadIC,
1337 &FGInitialCondition::SetAlphaRadIC,
1339 PropertyManager->Tie("ic/theta-rad", this,
1340 &FGInitialCondition::GetThetaRadIC,
1341 &FGInitialCondition::SetThetaRadIC,
1343 PropertyManager->Tie("ic/beta-rad", this,
1344 &FGInitialCondition::GetBetaRadIC,
1345 &FGInitialCondition::SetBetaRadIC,
1347 PropertyManager->Tie("ic/phi-rad", this,
1348 &FGInitialCondition::GetPhiRadIC,
1349 &FGInitialCondition::SetPhiRadIC,
1351 PropertyManager->Tie("ic/psi-true-rad", this,
1352 &FGInitialCondition::GetPsiRadIC);
1353 PropertyManager->Tie("ic/lat-gc-rad", this,
1354 &FGInitialCondition::GetLatitudeRadIC,
1355 &FGInitialCondition::SetLatitudeRadIC,
1357 PropertyManager->Tie("ic/long-gc-rad", this,
1358 &FGInitialCondition::GetLongitudeRadIC,
1359 &FGInitialCondition::SetLongitudeRadIC,
1361 PropertyManager->Tie("ic/p-rad_sec", this,
1362 &FGInitialCondition::GetPRadpsIC,
1363 &FGInitialCondition::SetPRadpsIC,
1365 PropertyManager->Tie("ic/q-rad_sec", this,
1366 &FGInitialCondition::GetQRadpsIC,
1367 &FGInitialCondition::SetQRadpsIC,
1369 PropertyManager->Tie("ic/r-rad_sec", this,
1370 &FGInitialCondition::GetRRadpsIC,
1371 &FGInitialCondition::SetRRadpsIC,
1374 typedef int (FGInitialCondition::*iPMF)(void) const;
1375 PropertyManager->Tie("simulation/write-state-file",
1378 &FGInitialCondition::WriteStateFile);
1382 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
1383 // The bitmasked value choices are as follows:
1384 // unset: In this case (the default) JSBSim would only print
1385 // out the normally expected messages, essentially echoing
1386 // the config files as they are read. If the environment
1387 // variable is not set, debug_lvl is set to 1 internally
1388 // 0: This requests JSBSim not to output any messages
1390 // 1: This value explicity requests the normal JSBSim
1392 // 2: This value asks for a message to be printed out when
1393 // a class is instantiated
1394 // 4: When this value is set, a message is displayed when a
1395 // FGModel object executes its Run() method
1396 // 8: When this value is set, various runtime state variables
1397 // are printed out periodically
1398 // 16: When set various parameters are sanity checked and
1399 // a message is printed out when they go out of bounds
1401 void FGInitialCondition::Debug(int from)
1403 if (debug_lvl <= 0) return;
1405 if (debug_lvl & 1) { // Standard console startup message output
1407 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
1408 if (from == 0) cout << "Instantiated: FGInitialCondition" << endl;
1409 if (from == 1) cout << "Destroyed: FGInitialCondition" << endl;
1411 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
1413 if (debug_lvl & 8 ) { // Runtime state variables
1415 if (debug_lvl & 16) { // Sanity checking
1417 if (debug_lvl & 64) {
1418 if (from == 0) { // Constructor
1419 cout << IdSrc << endl;
1420 cout << IdHdr << endl;