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 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 General Public License for more
19 You should have received a copy of the GNU 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 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"
48 #include "FGAtmosphere.h"
50 #include "FGAircraft.h"
51 #include "FGTranslation.h"
52 #include "FGRotation.h"
53 #include "FGPosition.h"
54 #include "FGAuxiliary.h"
56 #include "FGConfigFile.h"
58 static const char *IdSrc = "$Id$";
59 static const char *IdHdr = ID_INITIALCONDITION;
61 //******************************************************************************
63 FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec)
79 sea_level_radius = FDMExec->GetInertial()->RefRadius();
80 radius_to_vehicle = FDMExec->GetInertial()->RefRadius();
83 salpha=sbeta=stheta=sphi=spsi=sgamma=0;
84 calpha=cbeta=ctheta=cphi=cpsi=cgamma=1;
86 if(FDMExec != NULL ) {
88 fdmex->GetPosition()->Seth(altitude);
89 fdmex->GetAtmosphere()->Run();
91 cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
94 if (debug_lvl & 2) cout << "Instantiated: FGInitialCondition" << endl;
97 //******************************************************************************
99 FGInitialCondition::~FGInitialCondition()
101 if (debug_lvl & 2) cout << "Destroyed: FGInitialCondition" << endl;
104 //******************************************************************************
106 void FGInitialCondition::SetVcalibratedKtsIC(double tt) {
108 if(getMachFromVcas(&mach,tt*ktstofps)) {
109 //cout << "Mach: " << mach << endl;
112 vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
113 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
114 //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl;
117 cout << "Failed to get Mach number for given Vc and altitude, Vc unchanged." << endl;
118 cout << "Please mail the set of initial conditions used to apeden@earthlink.net" << endl;
122 //******************************************************************************
124 void FGInitialCondition::SetVequivalentKtsIC(double tt) {
127 vt=ve*1/sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
128 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
132 //******************************************************************************
134 void FGInitialCondition::SetVgroundFpsIC(double tt) {
140 vnorth = vg*cos(psi); veast = vg*sin(psi); vdown = 0;
142 ua = u + uw; va = v + vw; wa = w + ww;
143 vt = sqrt( ua*ua + va*va + wa*wa );
145 vxz = sqrt( u*u + w*w );
146 if( w != 0 ) alpha = atan2( w, u );
147 if( vxz != 0 ) beta = atan2( v, vxz );
148 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
150 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
153 //******************************************************************************
155 void FGInitialCondition::SetVtrueFpsIC(double tt) {
158 mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
160 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
163 //******************************************************************************
165 void FGInitialCondition::SetMachIC(double tt) {
167 lastSpeedSet=setmach;
168 vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
170 ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
171 //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl;
174 //******************************************************************************
176 void FGInitialCondition::SetClimbRateFpmIC(double tt) {
177 SetClimbRateFpsIC(tt/60.0);
180 //******************************************************************************
182 void FGInitialCondition::SetClimbRateFpsIC(double tt) {
187 sgamma=sin(gamma); cgamma=cos(gamma);
191 //******************************************************************************
193 void FGInitialCondition::SetFlightPathAngleRadIC(double tt) {
195 sgamma=sin(gamma); cgamma=cos(gamma);
200 //******************************************************************************
202 void FGInitialCondition::SetAlphaRadIC(double tt) {
204 salpha=sin(alpha); calpha=cos(alpha);
208 //******************************************************************************
210 void FGInitialCondition::SetPitchAngleRadIC(double tt) {
212 stheta=sin(theta); ctheta=cos(theta);
216 //******************************************************************************
218 void FGInitialCondition::SetBetaRadIC(double tt) {
220 sbeta=sin(beta); cbeta=cos(beta);
225 //******************************************************************************
227 void FGInitialCondition::SetRollAngleRadIC(double tt) {
229 sphi=sin(phi); cphi=cos(phi);
233 //******************************************************************************
235 void FGInitialCondition::SetTrueHeadingRadIC(double tt) {
237 spsi=sin(psi); cpsi=cos(psi);
241 //******************************************************************************
243 void FGInitialCondition::SetUBodyFpsIC(double tt) {
245 vt=sqrt(u*u + v*v + w*w);
249 //******************************************************************************
251 void FGInitialCondition::SetVBodyFpsIC(double tt) {
253 vt=sqrt(u*u + v*v + w*w);
257 //******************************************************************************
259 void FGInitialCondition::SetWBodyFpsIC(double tt) {
261 vt=sqrt( u*u + v*v + w*w );
265 //******************************************************************************
267 double FGInitialCondition::GetUBodyFpsIC(void) {
268 if(lastSpeedSet == setvg )
271 return vt*calpha*cbeta - uw;
274 //******************************************************************************
276 double FGInitialCondition::GetVBodyFpsIC(void) {
277 if( lastSpeedSet == setvg )
280 return vt*sbeta - vw;
284 //******************************************************************************
286 double FGInitialCondition::GetWBodyFpsIC(void) {
287 if( lastSpeedSet == setvg )
290 return vt*salpha*cbeta -ww;
293 //******************************************************************************
295 void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD ) {
296 wnorth = wN; weast = wE; wdown = wD;
297 lastWindSet = setwned;
299 if(lastSpeedSet == setvg)
303 //******************************************************************************
305 // positive from left
306 void FGInitialCondition::SetHeadWindKtsIC(double head){
310 if(lastSpeedSet == setvg)
315 //******************************************************************************
317 void FGInitialCondition::SetCrossWindKtsIC(double cross){
318 wcross=cross*ktstofps;
321 if(lastSpeedSet == setvg)
326 //******************************************************************************
328 void FGInitialCondition::SetWindDownKtsIC(double wD) {
331 if(lastSpeedSet == setvg)
335 //******************************************************************************
337 void FGInitialCondition::SetWindMagKtsIC(double mag) {
341 if(lastSpeedSet == setvg)
345 //******************************************************************************
347 void FGInitialCondition::SetWindDirDegIC(double dir) {
351 if(lastSpeedSet == setvg)
356 //******************************************************************************
358 void FGInitialCondition::calcWindUVW(void) {
360 switch(lastWindSet) {
362 wnorth=wmag*cos(wdir);
363 weast=wmag*sin(wdir);
366 wnorth=whead*cos(psi) + wcross*cos(psi+M_PI/2);
367 weast=whead*sin(psi) + wcross*sin(psi+M_PI/2);
372 uw=wnorth*ctheta*cpsi +
375 vw=wnorth*( sphi*stheta*cpsi - cphi*spsi ) +
376 weast*( sphi*stheta*spsi + cphi*cpsi ) +
378 ww=wnorth*(cphi*stheta*cpsi + sphi*spsi) +
379 weast*(cphi*stheta*spsi - sphi*cpsi) +
383 /* cout << "FGInitialCondition::calcWindUVW: wnorth, weast, wdown "
384 << wnorth << ", " << weast << ", " << wdown << endl;
385 cout << "FGInitialCondition::calcWindUVW: theta, phi, psi "
386 << theta << ", " << phi << ", " << psi << endl;
387 cout << "FGInitialCondition::calcWindUVW: uw, vw, ww "
388 << uw << ", " << vw << ", " << ww << endl; */
392 //******************************************************************************
394 void FGInitialCondition::SetAltitudeFtIC(double tt) {
396 fdmex->GetPosition()->Seth(altitude);
397 fdmex->GetAtmosphere()->Run();
398 //lets try to make sure the user gets what they intended
400 switch(lastSpeedSet) {
404 SetVtrueKtsIC(vt*fpstokts);
407 SetVcalibratedKtsIC(vc*fpstokts);
410 SetVequivalentKtsIC(ve*fpstokts);
421 //******************************************************************************
423 void FGInitialCondition::SetAltitudeAGLFtIC(double tt) {
424 fdmex->GetPosition()->SetDistanceAGL(tt);
425 altitude=fdmex->GetPosition()->Geth();
426 SetAltitudeFtIC(altitude);
429 //******************************************************************************
431 void FGInitialCondition::SetSeaLevelRadiusFtIC(double tt) {
432 sea_level_radius = tt;
435 //******************************************************************************
437 void FGInitialCondition::SetTerrainAltitudeFtIC(double tt) {
441 //******************************************************************************
443 void FGInitialCondition::calcUVWfromNED(void) {
444 u=vnorth*ctheta*cpsi +
447 v=vnorth*( sphi*stheta*cpsi - cphi*spsi ) +
448 veast*( sphi*stheta*spsi + cphi*cpsi ) +
450 w=vnorth*( cphi*stheta*cpsi + sphi*spsi ) +
451 veast*( cphi*stheta*spsi - sphi*cpsi ) +
455 //******************************************************************************
457 void FGInitialCondition::SetVnorthFpsIC(double tt) {
460 vt=sqrt(u*u + v*v + w*w);
464 //******************************************************************************
466 void FGInitialCondition::SetVeastFpsIC(double tt) {
469 vt=sqrt(u*u + v*v + w*w);
473 //******************************************************************************
475 void FGInitialCondition::SetVdownFpsIC(double tt) {
478 vt=sqrt(u*u + v*v + w*w);
479 SetClimbRateFpsIC(-1*vdown);
483 //******************************************************************************
485 bool FGInitialCondition::getMachFromVcas(double *Mach,double vcas) {
491 sfunc=&FGInitialCondition::calcVcas;
492 if(findInterval(vcas,guess)) {
493 if(solve(&mach,vcas))
499 //******************************************************************************
501 bool FGInitialCondition::getAlpha(void) {
503 double guess=theta-gamma;
505 xmin=fdmex->GetAircraft()->GetAlphaCLMin();
506 xmax=fdmex->GetAircraft()->GetAlphaCLMax();
507 sfunc=&FGInitialCondition::GammaEqOfAlpha;
508 if(findInterval(0,guess)){
519 //******************************************************************************
521 bool FGInitialCondition::getTheta(void) {
523 double guess=alpha+gamma;
526 sfunc=&FGInitialCondition::GammaEqOfTheta;
527 if(findInterval(0,guess)){
538 //******************************************************************************
540 double FGInitialCondition::GammaEqOfTheta(double Theta) {
542 double sTheta,cTheta;
544 //theta=Theta; stheta=sin(theta); ctheta=cos(theta);
545 sTheta=sin(Theta); cTheta=cos(Theta);
547 a=wdown + vt*calpha*cbeta + uw;
548 b=vt*sphi*sbeta + vw*sphi;
549 c=vt*cphi*salpha*cbeta + ww*cphi;
550 return vt*sgamma - ( a*sTheta - (b+c)*cTheta);
553 //******************************************************************************
555 double FGInitialCondition::GammaEqOfAlpha(double Alpha) {
557 double sAlpha,cAlpha;
559 sAlpha=sin(Alpha); cAlpha=cos(Alpha);
560 a=wdown + vt*cAlpha*cbeta + uw;
561 b=vt*sphi*sbeta + vw*sphi;
562 c=vt*cphi*sAlpha*cbeta + ww*cphi;
564 return vt*sgamma - ( a*stheta - (b+c)*ctheta );
567 //******************************************************************************
569 double FGInitialCondition::calcVcas(double Mach) {
571 double p=fdmex->GetAtmosphere()->GetPressure();
572 double psl=fdmex->GetAtmosphere()->GetPressureSL();
573 double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
574 double pt,A,B,D,vcas;
576 if(Mach < 1) //calculate total pressure assuming isentropic flow
577 pt=p*pow((1 + 0.2*Mach*Mach),3.5);
579 // shock in front of pitot tube, we'll assume its normal and use
580 // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
581 // pressure behind the shock to the static pressure in front
584 //the normal shock assumption should not be a bad one -- most supersonic
585 //aircraft place the pitot probe out front so that it is the forward
586 //most point on the aircraft. The real shock would, of course, take
587 //on something like the shape of a rounded-off cone but, here again,
588 //the assumption should be good since the opening of the pitot probe
589 //is very small and, therefore, the effects of the shock curvature
590 //should be small as well. AFAIK, this approach is fairly well accepted
591 //within the aerospace community
593 B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);
595 // The denominator above is zero for Mach ~ 0.38, for which
596 // we'll never be here, so we're safe
598 D = (2.8*Mach*Mach-0.4)*0.4167;
602 A = pow(((pt-p)/psl+1),0.28571);
603 vcas = sqrt(7*psl/rhosl*(A-1));
604 //cout << "calcVcas: vcas= " << vcas*fpstokts << " mach= " << Mach << " pressure: " << pt << endl;
608 //******************************************************************************
610 bool FGInitialCondition::findInterval(double x,double guess) {
611 //void find_interval(inter_params &ip,eqfunc f,double y,double constant, int &flag){
615 double flo,fhi,fguess;
618 fguess=(this->*sfunc)(guess)-x;
624 if(lo < xmin) lo=xmin;
625 if(hi > xmax) hi=xmax;
627 flo=(this->*sfunc)(lo)-x;
628 fhi=(this->*sfunc)(hi)-x;
629 if(flo*fhi <=0) { //found interval with root
631 if(flo*fguess <= 0) { //narrow interval down a bit
632 hi=lo+step; //to pass solver interval that is as
635 else if(fhi*fguess <= 0) {
639 //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
641 while((found == 0) && (i <= 100));
647 //******************************************************************************
649 bool FGInitialCondition::solve(double *y,double x)
651 double x1,x2,x3,f1,f2,f3,d,d0;
653 double const relax =0.9;
661 f1=(this->*sfunc)(x1)-x;
662 f3=(this->*sfunc)(x3)-x;
667 while ((fabs(d) > eps) && (i < 100)) {
669 x2 = x1-d*d0*f1/(f3-f1);
671 f2=(this->*sfunc)(x2)-x;
672 //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
673 //cout << " " << f1 << "," << f2 << "," << f3 << endl;
675 if(fabs(f2) <= 0.001) {
677 } else if(f1*f2 <= 0.0) {
681 } else if(f2*f3 <= 0) {
694 //cout << "Success= " << success << " Vcas: " << vcas*fpstokts << " Mach: " << x2 << endl;
698 //******************************************************************************
700 double FGInitialCondition::GetWindDirDegIC(void) {
702 return atan2(weast,wnorth)*radtodeg;
709 //******************************************************************************
711 bool FGInitialCondition::Load(string acpath, string acname, string rstfile)
719 resetDef = acpath + "/" + acname + "/" + rstfile + ".xml";
721 resetDef = acpath + ";" + acname + ";" + rstfile + ".xml";
724 FGConfigFile resetfile(resetDef);
725 if (!resetfile.IsOpen()) {
726 cerr << "Failed to open reset file: " << resetDef << endl;
730 resetfile.GetNextConfigLine();
731 token = resetfile.GetValue();
732 if (token != string("initialize")) {
733 cerr << "The reset file " << resetDef
734 << " does not appear to be a reset file" << endl;
738 resetfile.GetNextConfigLine();
740 while (token != string("/initialize") && token != string("EOF")) {
741 if (token == "UBODY" ) { resetfile >> temp; SetUBodyFpsIC(temp); }
742 if (token == "VBODY" ) { resetfile >> temp; SetVBodyFpsIC(temp); }
743 if (token == "WBODY" ) { resetfile >> temp; SetWBodyFpsIC(temp); }
744 if (token == "LATITUDE" ) { resetfile >> temp; SetLatitudeDegIC(temp); }
745 if (token == "LONGITUDE" ) { resetfile >> temp; SetLongitudeDegIC(temp); }
746 if (token == "PHI" ) { resetfile >> temp; SetRollAngleDegIC(temp); }
747 if (token == "THETA" ) { resetfile >> temp; SetPitchAngleDegIC(temp); }
748 if (token == "PSI" ) { resetfile >> temp; SetTrueHeadingDegIC(temp); }
749 if (token == "ALPHA" ) { resetfile >> temp; SetAlphaDegIC(temp); }
750 if (token == "BETA" ) { resetfile >> temp; SetBetaDegIC(temp); }
751 if (token == "GAMMA" ) { resetfile >> temp; SetFlightPathAngleDegIC(temp); }
752 if (token == "ROC" ) { resetfile >> temp; SetClimbRateFpmIC(temp); }
753 if (token == "ALTITUDE" ) { resetfile >> temp; SetAltitudeFtIC(temp); }
754 if (token == "WINDDIR" ) { resetfile >> temp; SetWindDirDegIC(temp); }
755 if (token == "VWIND" ) { resetfile >> temp; SetWindMagKtsIC(temp); }
756 if (token == "HWIND" ) { resetfile >> temp; SetHeadWindKtsIC(temp); }
757 if (token == "XWIND" ) { resetfile >> temp; SetCrossWindKtsIC(temp); }
758 if (token == "VC" ) { resetfile >> temp; SetVcalibratedKtsIC(temp); }
759 if (token == "MACH" ) { resetfile >> temp; SetMachIC(temp); }
760 if (token == "VGROUND" ) { resetfile >> temp; SetVgroundKtsIC(temp); }