src/FDM/JSBSim/FGInitialCondition.cpp

   1 /*******************************************************************************
   2
   3  Header:       FGInitialCondition.cpp
   4  Author:       Tony Peden
   5  Date started: 7/1/99
   6
   7  ------------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) -------------
   8
   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
  12  version.
  13
  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
  17  details.
  18
  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.
  22
  23  Further information about the GNU General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26
  27  HISTORY
  28 --------------------------------------------------------------------------------
  29 7/1/99   TP   Created
  30
  31
  32 FUNCTIONAL DESCRIPTION
  33 --------------------------------------------------------------------------------
  34
  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.
  40
  41 ********************************************************************************
  42 INCLUDES
  43 *******************************************************************************/
  44
  45 #include "FGInitialCondition.h"
  46 #include "FGFDMExec.h"
  47 #include "FGInertial.h"
  48 #include "FGAtmosphere.h"
  49 #include "FGAerodynamics.h"
  50 #include "FGPropagate.h"
  51 #include "FGConfigFile.h"
  52 #include "FGPropertyManager.h"
  53
  54 namespace JSBSim {
  55
  56 static const char *IdSrc = "$Id$";
  57 static const char *IdHdr = ID_INITIALCONDITION;
  58
  59 //******************************************************************************
  60
  61 FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec)
  62 {
  63   vt=vc=ve=vg=0;
  64   mach=0;
  65   alpha=beta=gamma=0;
  66   theta=phi=psi=0;
  67   altitude=hdot=0;
  68   latitude=longitude=0;
  69   u=v=w=0;
  70   p=q=r=0;
  71   uw=vw=ww=0;
  72   vnorth=veast=vdown=0;
  73   wnorth=weast=wdown=0;
  74   whead=wcross=0;
  75   wdir=wmag=0;
  76   lastSpeedSet=setvt;
  77   lastWindSet=setwned;
  78   sea_level_radius = FDMExec->GetInertial()->RefRadius();
  79   radius_to_vehicle = FDMExec->GetInertial()->RefRadius();
  80   terrain_altitude = 0;
  81
  82   salpha=sbeta=stheta=sphi=spsi=sgamma=0;
  83   calpha=cbeta=ctheta=cphi=cpsi=cgamma=1;
  84
  85   if(FDMExec != NULL ) {
  86     fdmex=FDMExec;
  87     fdmex->GetPropagate()->Seth(altitude);
  88     fdmex->GetAtmosphere()->Run();
  89     PropertyManager=fdmex->GetPropertyManager();
  90     bind();
  91   } else {
  92     cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
  93   }
  94
  95   Debug(0);
  96 }
  97
  98 //******************************************************************************
  99
 100 FGInitialCondition::~FGInitialCondition()
 101 {
 102   unbind();
 103   Debug(1);
 104 }
 105
 106 //******************************************************************************
 107
 108 void FGInitialCondition::SetVcalibratedKtsIC(double tt) {
 109
 110   if(getMachFromVcas(&mach,tt*ktstofps)) {
 111     //cout << "Mach: " << mach << endl;
 112     lastSpeedSet=setvc;
 113     vc=tt*ktstofps;
 114     vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
 115     ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 116     //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl;
 117   }
 118   else {
 119     cout << "Failed to get Mach number for given Vc and altitude, Vc unchanged." << endl;
 120     cout << "Please mail the set of initial conditions used to apeden@earthlink.net" << endl;
 121   }
 122 }
 123
 124 //******************************************************************************
 125
 126 void FGInitialCondition::SetVequivalentKtsIC(double tt) {
 127   ve=tt*ktstofps;
 128   lastSpeedSet=setve;
 129   vt=ve*1/sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 130   mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
 131   vc=calcVcas(mach);
 132 }
 133
 134 //******************************************************************************
 135
 136 void FGInitialCondition::SetVgroundFpsIC(double tt) {
 137   double ua,va,wa;
 138   double vxz;
 139
 140   vg=tt;
 141   lastSpeedSet=setvg;
 142   vnorth = vg*cos(psi); veast = vg*sin(psi); vdown = 0;
 143   calcUVWfromNED();
 144   ua = u + uw; va = v + vw; wa = w + ww;
 145   vt = sqrt( ua*ua + va*va + wa*wa );
 146   alpha = beta = 0;
 147   vxz = sqrt( u*u + w*w );
 148   if( w != 0 ) alpha = atan2( w, u );
 149   if( vxz != 0 ) beta = atan2( v, vxz );
 150   mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
 151   vc=calcVcas(mach);
 152   ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 153 }
 154
 155 //******************************************************************************
 156
 157 void FGInitialCondition::SetVtrueFpsIC(double tt) {
 158   vt=tt;
 159   lastSpeedSet=setvt;
 160   mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
 161   vc=calcVcas(mach);
 162   ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 163 }
 164
 165 //******************************************************************************
 166
 167 void FGInitialCondition::SetMachIC(double tt) {
 168   mach=tt;
 169   lastSpeedSet=setmach;
 170   vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
 171   vc=calcVcas(mach);
 172   ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 173   //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl;
 174 }
 175
 176 //******************************************************************************
 177
 178 void FGInitialCondition::SetClimbRateFpmIC(double tt) {
 179   SetClimbRateFpsIC(tt/60.0);
 180 }
 181
 182 //******************************************************************************
 183
 184 void FGInitialCondition::SetClimbRateFpsIC(double tt) {
 185
 186   if(vt > 0.1) {
 187     hdot=tt;
 188     gamma=asin(hdot/vt);
 189     sgamma=sin(gamma); cgamma=cos(gamma);
 190   }
 191 }
 192
 193 //******************************************************************************
 194
 195 void FGInitialCondition::SetFlightPathAngleRadIC(double tt) {
 196   gamma=tt;
 197   sgamma=sin(gamma); cgamma=cos(gamma);
 198   getTheta();
 199   hdot=vt*sgamma;
 200 }
 201
 202 //******************************************************************************
 203
 204 void FGInitialCondition::SetAlphaRadIC(double tt) {
 205   alpha=tt;
 206   salpha=sin(alpha); calpha=cos(alpha);
 207   getTheta();
 208 }
 209
 210 //******************************************************************************
 211
 212 void FGInitialCondition::SetPitchAngleRadIC(double tt) {
 213   theta=tt;
 214   stheta=sin(theta); ctheta=cos(theta);
 215   getAlpha();
 216 }
 217
 218 //******************************************************************************
 219
 220 void FGInitialCondition::SetBetaRadIC(double tt) {
 221   beta=tt;
 222   sbeta=sin(beta); cbeta=cos(beta);
 223   getTheta();
 224
 225 }
 226
 227 //******************************************************************************
 228
 229 void FGInitialCondition::SetRollAngleRadIC(double tt) {
 230   phi=tt;
 231   sphi=sin(phi); cphi=cos(phi);
 232   getTheta();
 233 }
 234
 235 //******************************************************************************
 236
 237 void FGInitialCondition::SetTrueHeadingRadIC(double tt) {
 238     psi=tt;
 239     spsi=sin(psi); cpsi=cos(psi);
 240     calcWindUVW();
 241 }
 242
 243 //******************************************************************************
 244
 245 void FGInitialCondition::SetUBodyFpsIC(double tt) {
 246   u=tt;
 247   vt=sqrt(u*u + v*v + w*w);
 248   lastSpeedSet=setuvw;
 249 }
 250
 251 //******************************************************************************
 252
 253 void FGInitialCondition::SetVBodyFpsIC(double tt) {
 254   v=tt;
 255   vt=sqrt(u*u + v*v + w*w);
 256   lastSpeedSet=setuvw;
 257 }
 258
 259 //******************************************************************************
 260
 261 void FGInitialCondition::SetWBodyFpsIC(double tt) {
 262   w=tt;
 263   vt=sqrt( u*u + v*v + w*w );
 264   lastSpeedSet=setuvw;
 265 }
 266
 267 //******************************************************************************
 268
 269 double FGInitialCondition::GetUBodyFpsIC(void) const {
 270     if(lastSpeedSet == setvg )
 271       return u;
 272     else
 273       return vt*calpha*cbeta - uw;
 274 }
 275
 276 //******************************************************************************
 277
 278 double FGInitialCondition::GetVBodyFpsIC(void) const {
 279     if( lastSpeedSet == setvg )
 280       return v;
 281     else {
 282       return vt*sbeta - vw;
 283     }
 284 }
 285
 286 //******************************************************************************
 287
 288 double FGInitialCondition::GetWBodyFpsIC(void) const {
 289     if( lastSpeedSet == setvg )
 290       return w;
 291     else
 292       return vt*salpha*cbeta -ww;
 293 }
 294
 295 //******************************************************************************
 296
 297 void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD ) {
 298   wnorth = wN; weast = wE; wdown = wD;
 299   lastWindSet = setwned;
 300   calcWindUVW();
 301   if(lastSpeedSet == setvg)
 302     SetVgroundFpsIC(vg);
 303 }
 304
 305 //******************************************************************************
 306
 307 // positive from left
 308 void FGInitialCondition::SetHeadWindKtsIC(double head){
 309     whead=head*ktstofps;
 310     lastWindSet=setwhc;
 311     calcWindUVW();
 312     if(lastSpeedSet == setvg)
 313       SetVgroundFpsIC(vg);
 314
 315 }
 316
 317 //******************************************************************************
 318
 319 void FGInitialCondition::SetCrossWindKtsIC(double cross){
 320     wcross=cross*ktstofps;
 321     lastWindSet=setwhc;
 322     calcWindUVW();
 323     if(lastSpeedSet == setvg)
 324       SetVgroundFpsIC(vg);
 325
 326 }
 327
 328 //******************************************************************************
 329
 330 void FGInitialCondition::SetWindDownKtsIC(double wD) {
 331     wdown=wD;
 332     calcWindUVW();
 333     if(lastSpeedSet == setvg)
 334       SetVgroundFpsIC(vg);
 335 }
 336
 337 //******************************************************************************
 338
 339 void FGInitialCondition::SetWindMagKtsIC(double mag) {
 340   wmag=mag*ktstofps;
 341   lastWindSet=setwmd;
 342   calcWindUVW();
 343   if(lastSpeedSet == setvg)
 344       SetVgroundFpsIC(vg);
 345 }
 346
 347 //******************************************************************************
 348
 349 void FGInitialCondition::SetWindDirDegIC(double dir) {
 350   wdir=dir*degtorad;
 351   lastWindSet=setwmd;
 352   calcWindUVW();
 353   if(lastSpeedSet == setvg)
 354       SetVgroundFpsIC(vg);
 355 }
 356
 357
 358 //******************************************************************************
 359
 360 void FGInitialCondition::calcWindUVW(void) {
 361
 362     switch(lastWindSet) {
 363       case setwmd:
 364         wnorth=wmag*cos(wdir);
 365         weast=wmag*sin(wdir);
 366       break;
 367       case setwhc:
 368         wnorth=whead*cos(psi) + wcross*cos(psi+M_PI/2);
 369         weast=whead*sin(psi) + wcross*sin(psi+M_PI/2);
 370       break;
 371       case setwned:
 372       break;
 373     }
 374     uw=wnorth*ctheta*cpsi +
 375        weast*ctheta*spsi -
 376        wdown*stheta;
 377     vw=wnorth*( sphi*stheta*cpsi - cphi*spsi ) +
 378         weast*( sphi*stheta*spsi + cphi*cpsi ) +
 379        wdown*sphi*ctheta;
 380     ww=wnorth*(cphi*stheta*cpsi + sphi*spsi) +
 381        weast*(cphi*stheta*spsi - sphi*cpsi) +
 382        wdown*cphi*ctheta;
 383
 384
 385     /* cout << "FGInitialCondition::calcWindUVW: wnorth, weast, wdown "
 386          << wnorth << ", " << weast << ", " << wdown << endl;
 387     cout << "FGInitialCondition::calcWindUVW: theta, phi, psi "
 388           << theta << ", " << phi << ", " << psi << endl;
 389     cout << "FGInitialCondition::calcWindUVW: uw, vw, ww "
 390           << uw << ", " << vw << ", " << ww << endl; */
 391
 392 }
 393
 394 //******************************************************************************
 395
 396 void FGInitialCondition::SetAltitudeFtIC(double tt) {
 397   altitude=tt;
 398   fdmex->GetPropagate()->Seth(altitude);
 399   fdmex->GetAtmosphere()->Run();
 400   //lets try to make sure the user gets what they intended
 401
 402   switch(lastSpeedSet) {
 403   case setned:
 404   case setuvw:
 405   case setvt:
 406     SetVtrueKtsIC(vt*fpstokts);
 407     break;
 408   case setvc:
 409     SetVcalibratedKtsIC(vc*fpstokts);
 410     break;
 411   case setve:
 412     SetVequivalentKtsIC(ve*fpstokts);
 413     break;
 414   case setmach:
 415     SetMachIC(mach);
 416     break;
 417   case setvg:
 418     SetVgroundFpsIC(vg);
 419     break;
 420   }
 421 }
 422
 423 //******************************************************************************
 424
 425 void FGInitialCondition::SetAltitudeAGLFtIC(double tt) {
 426   SetAltitudeFtIC(terrain_altitude + tt);
 427 }
 428
 429 //******************************************************************************
 430
 431 void FGInitialCondition::SetSeaLevelRadiusFtIC(double tt) {
 432   sea_level_radius = tt;
 433 }
 434
 435 //******************************************************************************
 436
 437 void FGInitialCondition::SetTerrainAltitudeFtIC(double tt) {
 438   terrain_altitude=tt;
 439 }
 440
 441 //******************************************************************************
 442
 443 void FGInitialCondition::calcUVWfromNED(void) {
 444   u=vnorth*ctheta*cpsi +
 445      veast*ctheta*spsi -
 446      vdown*stheta;
 447   v=vnorth*( sphi*stheta*cpsi - cphi*spsi ) +
 448      veast*( sphi*stheta*spsi + cphi*cpsi ) +
 449      vdown*sphi*ctheta;
 450   w=vnorth*( cphi*stheta*cpsi + sphi*spsi ) +
 451      veast*( cphi*stheta*spsi - sphi*cpsi ) +
 452      vdown*cphi*ctheta;
 453 }
 454
 455 //******************************************************************************
 456
 457 void FGInitialCondition::SetVnorthFpsIC(double tt) {
 458   vnorth=tt;
 459   calcUVWfromNED();
 460   vt=sqrt(u*u + v*v + w*w);
 461   lastSpeedSet=setned;
 462 }
 463
 464 //******************************************************************************
 465
 466 void FGInitialCondition::SetVeastFpsIC(double tt) {
 467   veast=tt;
 468   calcUVWfromNED();
 469   vt=sqrt(u*u + v*v + w*w);
 470   lastSpeedSet=setned;
 471 }
 472
 473 //******************************************************************************
 474
 475 void FGInitialCondition::SetVdownFpsIC(double tt) {
 476   vdown=tt;
 477   calcUVWfromNED();
 478   vt=sqrt(u*u + v*v + w*w);
 479   SetClimbRateFpsIC(-1*vdown);
 480   lastSpeedSet=setned;
 481 }
 482
 483 //******************************************************************************
 484
 485 bool FGInitialCondition::getMachFromVcas(double *Mach,double vcas) {
 486
 487   bool result=false;
 488   double guess=1.5;
 489   xlo=xhi=0;
 490   xmin=0;xmax=50;
 491   sfunc=&FGInitialCondition::calcVcas;
 492   if(findInterval(vcas,guess)) {
 493     if(solve(&mach,vcas))
 494       result=true;
 495   }
 496   return result;
 497 }
 498
 499 //******************************************************************************
 500
 501 bool FGInitialCondition::getAlpha(void) {
 502   bool result=false;
 503   double guess=theta-gamma;
 504
 505   if(vt < 0.01) return 0;
 506
 507   xlo=xhi=0;
 508   xmin=fdmex->GetAerodynamics()->GetAlphaCLMin();
 509   xmax=fdmex->GetAerodynamics()->GetAlphaCLMax();
 510   sfunc=&FGInitialCondition::GammaEqOfAlpha;
 511   if(findInterval(0,guess)){
 512     if(solve(&alpha,0)){
 513       result=true;
 514       salpha=sin(alpha);
 515       calpha=cos(alpha);
 516     }
 517   }
 518   calcWindUVW();
 519   return result;
 520 }
 521
 522 //******************************************************************************
 523
 524 bool FGInitialCondition::getTheta(void) {
 525   bool result=false;
 526   double guess=alpha+gamma;
 527
 528   if(vt < 0.01) return 0;
 529
 530   xlo=xhi=0;
 531   xmin=-89;xmax=89;
 532   sfunc=&FGInitialCondition::GammaEqOfTheta;
 533   if(findInterval(0,guess)){
 534     if(solve(&theta,0)){
 535       result=true;
 536       stheta=sin(theta);
 537       ctheta=cos(theta);
 538     }
 539   }
 540   calcWindUVW();
 541   return result;
 542 }
 543
 544 //******************************************************************************
 545
 546 double FGInitialCondition::GammaEqOfTheta(double Theta) {
 547   double a,b,c;
 548   double sTheta,cTheta;
 549
 550   //theta=Theta; stheta=sin(theta); ctheta=cos(theta);
 551   sTheta=sin(Theta); cTheta=cos(Theta);
 552   calcWindUVW();
 553   a=wdown + vt*calpha*cbeta + uw;
 554   b=vt*sphi*sbeta + vw*sphi;
 555   c=vt*cphi*salpha*cbeta + ww*cphi;
 556   return vt*sgamma - ( a*sTheta - (b+c)*cTheta);
 557 }
 558
 559 //******************************************************************************
 560
 561 double FGInitialCondition::GammaEqOfAlpha(double Alpha) {
 562   double a,b,c;
 563   double sAlpha,cAlpha;
 564   sAlpha=sin(Alpha); cAlpha=cos(Alpha);
 565   a=wdown + vt*cAlpha*cbeta + uw;
 566   b=vt*sphi*sbeta + vw*sphi;
 567   c=vt*cphi*sAlpha*cbeta + ww*cphi;
 568
 569   return vt*sgamma - ( a*stheta - (b+c)*ctheta );
 570 }
 571
 572 //******************************************************************************
 573
 574 double FGInitialCondition::calcVcas(double Mach) {
 575
 576   double p=fdmex->GetAtmosphere()->GetPressure();
 577   double psl=fdmex->GetAtmosphere()->GetPressureSL();
 578   double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
 579   double pt,A,B,D,vcas;
 580   if(Mach < 0) Mach=0;
 581   if(Mach < 1)    //calculate total pressure assuming isentropic flow
 582     pt=p*pow((1 + 0.2*Mach*Mach),3.5);
 583   else {
 584     // shock in front of pitot tube, we'll assume its normal and use
 585     // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
 586     // pressure behind the shock to the static pressure in front
 587
 588
 589     //the normal shock assumption should not be a bad one -- most supersonic
 590     //aircraft place the pitot probe out front so that it is the forward
 591     //most point on the aircraft.  The real shock would, of course, take
 592     //on something like the shape of a rounded-off cone but, here again,
 593     //the assumption should be good since the opening of the pitot probe
 594     //is very small and, therefore, the effects of the shock curvature
 595     //should be small as well. AFAIK, this approach is fairly well accepted
 596     //within the aerospace community
 597
 598     B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);
 599
 600     // The denominator above is zero for Mach ~ 0.38, for which
 601     // we'll never be here, so we're safe
 602
 603     D = (2.8*Mach*Mach-0.4)*0.4167;
 604     pt = p*pow(B,3.5)*D;
 605   }
 606
 607   A = pow(((pt-p)/psl+1),0.28571);
 608   vcas = sqrt(7*psl/rhosl*(A-1));
 609   //cout << "calcVcas: vcas= " << vcas*fpstokts << " mach= " << Mach << " pressure: " << pt << endl;
 610   return vcas;
 611 }
 612
 613 //******************************************************************************
 614
 615 bool FGInitialCondition::findInterval(double x,double guess) {
 616   //void find_interval(inter_params &ip,eqfunc f,double y,double constant, int &flag){
 617
 618   int i=0;
 619   bool found=false;
 620   double flo,fhi,fguess;
 621   double lo,hi,step;
 622   step=0.1;
 623   fguess=(this->*sfunc)(guess)-x;
 624   lo=hi=guess;
 625   do {
 626     step=2*step;
 627     lo-=step;
 628     hi+=step;
 629     if(lo < xmin) lo=xmin;
 630     if(hi > xmax) hi=xmax;
 631     i++;
 632     flo=(this->*sfunc)(lo)-x;
 633     fhi=(this->*sfunc)(hi)-x;
 634     if(flo*fhi <=0) {  //found interval with root
 635       found=true;
 636       if(flo*fguess <= 0) {  //narrow interval down a bit
 637         hi=lo+step;    //to pass solver interval that is as
 638         //small as possible
 639       }
 640       else if(fhi*fguess <= 0) {
 641         lo=hi-step;
 642       }
 643     }
 644     //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
 645   }
 646   while((found == 0) && (i <= 100));
 647   xlo=lo;
 648   xhi=hi;
 649   return found;
 650 }
 651
 652 //******************************************************************************
 653
 654 bool FGInitialCondition::solve(double *y,double x)
 655 {
 656   double x1,x2,x3,f1,f2,f3,d,d0;
 657   double eps=1E-5;
 658   double const relax =0.9;
 659   int i;
 660   bool success=false;
 661
 662   //initializations
 663   d=1;
 664   x2 = 0;
 665   x1=xlo;x3=xhi;
 666   f1=(this->*sfunc)(x1)-x;
 667   f3=(this->*sfunc)(x3)-x;
 668   d0=fabs(x3-x1);
 669
 670   //iterations
 671   i=0;
 672   while ((fabs(d) > eps) && (i < 100)) {
 673     d=(x3-x1)/d0;
 674     x2 = x1-d*d0*f1/(f3-f1);
 675
 676     f2=(this->*sfunc)(x2)-x;
 677     //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
 678     //cout << "                " << f1 << "," << f2 << "," << f3 << endl;
 679
 680     if(fabs(f2) <= 0.001) {
 681       x1=x3=x2;
 682     } else if(f1*f2 <= 0.0) {
 683       x3=x2;
 684       f3=f2;
 685       f1=relax*f1;
 686     } else if(f2*f3 <= 0) {
 687       x1=x2;
 688       f1=f2;
 689       f3=relax*f3;
 690     }
 691     //cout << i << endl;
 692     i++;
 693   }//end while
 694   if(i < 100) {
 695     success=true;
 696     *y=x2;
 697   }
 698
 699   //cout << "Success= " << success << " Vcas: " << vcas*fpstokts << " Mach: " << x2 << endl;
 700   return success;
 701 }
 702
 703 //******************************************************************************
 704
 705 double FGInitialCondition::GetWindDirDegIC(void) {
 706   if(weast != 0.0)
 707     return atan2(weast,wnorth)*radtodeg;
 708   else if(wnorth > 0)
 709     return 0.0;
 710   else
 711     return 180.0;
 712 }
 713
 714 //******************************************************************************
 715
 716 bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
 717 {
 718   string resetDef, acpath;
 719   string token="";
 720
 721   double temp;
 722   # ifndef macintosh
 723     string sep = "/";
 724   # else
 725     string sep = ";";
 726   #endif
 727
 728   if( useStoredPath ) {
 729     acpath = fdmex->GetAircraftPath() + sep + fdmex->GetModelName();
 730     resetDef = acpath + sep + rstfile + ".xml";
 731   } else {
 732     resetDef = rstfile;
 733   }
 734
 735   FGConfigFile resetfile(resetDef);
 736   if (!resetfile.IsOpen()) {
 737     cerr << "Failed to open reset file: " << resetDef << endl;
 738     return false;
 739   }
 740
 741   resetfile.GetNextConfigLine();
 742   token = resetfile.GetValue();
 743   if (token != string("initialize")) {
 744     cerr << "The reset file " << resetDef
 745          << " does not appear to be a reset file" << endl;
 746     return false;
 747   }
 748
 749   resetfile.GetNextConfigLine();
 750   resetfile >> token;
 751   while (token != string("/initialize") && token != string("EOF")) {
 752     if (token == "UBODY" ) { resetfile >> temp; SetUBodyFpsIC(temp); }
 753     if (token == "VBODY" ) { resetfile >> temp; SetVBodyFpsIC(temp); }
 754     if (token == "WBODY" ) { resetfile >> temp; SetWBodyFpsIC(temp); }
 755     if (token == "LATITUDE" ) { resetfile >> temp; SetLatitudeDegIC(temp); }
 756     if (token == "LONGITUDE" ) { resetfile >> temp; SetLongitudeDegIC(temp); }
 757     if (token == "PHI" ) { resetfile >> temp; SetRollAngleDegIC(temp); }
 758     if (token == "THETA" ) { resetfile >> temp; SetPitchAngleDegIC(temp); }
 759     if (token == "PSI" ) { resetfile >> temp; SetTrueHeadingDegIC(temp); }
 760     if (token == "ALPHA" ) { resetfile >> temp; SetAlphaDegIC(temp); }
 761     if (token == "BETA" ) { resetfile >> temp; SetBetaDegIC(temp); }
 762     if (token == "GAMMA" ) { resetfile >> temp; SetFlightPathAngleDegIC(temp); }
 763     if (token == "ROC" ) { resetfile >> temp; SetClimbRateFpmIC(temp); }
 764     if (token == "ALTITUDE" ) { resetfile >> temp; SetAltitudeFtIC(temp); }
 765     if (token == "WINDDIR" ) { resetfile >> temp; SetWindDirDegIC(temp); }
 766     if (token == "VWIND" ) { resetfile >> temp; SetWindMagKtsIC(temp); }
 767     if (token == "HWIND" ) { resetfile >> temp; SetHeadWindKtsIC(temp); }
 768     if (token == "XWIND" ) { resetfile >> temp; SetCrossWindKtsIC(temp); }
 769     if (token == "VC" ) { resetfile >> temp; SetVcalibratedKtsIC(temp); }
 770     if (token == "MACH" ) { resetfile >> temp; SetMachIC(temp); }
 771     if (token == "VGROUND" ) { resetfile >> temp; SetVgroundKtsIC(temp); }
 772     resetfile >> token;
 773   }
 774
 775   fdmex->RunIC();
 776
 777   return true;
 778 }
 779
 780 //******************************************************************************
 781
 782 void FGInitialCondition::bind(void){
 783   PropertyManager->Tie("ic/vc-kts", this,
 784                        &FGInitialCondition::GetVcalibratedKtsIC,
 785                        &FGInitialCondition::SetVcalibratedKtsIC,
 786                        true);
 787   PropertyManager->Tie("ic/ve-kts", this,
 788                        &FGInitialCondition::GetVequivalentKtsIC,
 789                        &FGInitialCondition::SetVequivalentKtsIC,
 790                        true);
 791   PropertyManager->Tie("ic/vg-kts", this,
 792                        &FGInitialCondition::GetVgroundKtsIC,
 793                        &FGInitialCondition::SetVgroundKtsIC,
 794                        true);
 795   PropertyManager->Tie("ic/vt-kts", this,
 796                        &FGInitialCondition::GetVtrueKtsIC,
 797                        &FGInitialCondition::SetVtrueKtsIC,
 798                        true);
 799   PropertyManager->Tie("ic/mach-norm", this,
 800                        &FGInitialCondition::GetMachIC,
 801                        &FGInitialCondition::SetMachIC,
 802                        true);
 803   PropertyManager->Tie("ic/roc-fpm", this,
 804                        &FGInitialCondition::GetClimbRateFpmIC,
 805                        &FGInitialCondition::SetClimbRateFpmIC,
 806                        true);
 807   PropertyManager->Tie("ic/gamma-deg", this,
 808                        &FGInitialCondition::GetFlightPathAngleDegIC,
 809                        &FGInitialCondition::SetFlightPathAngleDegIC,
 810                        true);
 811   PropertyManager->Tie("ic/alpha-deg", this,
 812                        &FGInitialCondition::GetAlphaDegIC,
 813                        &FGInitialCondition::SetAlphaDegIC,
 814                        true);
 815   PropertyManager->Tie("ic/beta-deg", this,
 816                        &FGInitialCondition::GetBetaDegIC,
 817                        &FGInitialCondition::SetBetaDegIC,
 818                        true);
 819   PropertyManager->Tie("ic/theta-deg", this,
 820                        &FGInitialCondition::GetPitchAngleDegIC,
 821                        &FGInitialCondition::SetPitchAngleDegIC,
 822                        true);
 823   PropertyManager->Tie("ic/phi-deg", this,
 824                        &FGInitialCondition::GetRollAngleDegIC,
 825                        &FGInitialCondition::SetRollAngleDegIC,
 826                        true);
 827   PropertyManager->Tie("ic/psi-true-deg", this,
 828                        &FGInitialCondition::GetHeadingDegIC );
 829   PropertyManager->Tie("ic/lat-gc-deg", this,
 830                        &FGInitialCondition::GetLatitudeDegIC,
 831                        &FGInitialCondition::SetLatitudeDegIC,
 832                        true);
 833   PropertyManager->Tie("ic/long-gc-deg", this,
 834                        &FGInitialCondition::GetLongitudeDegIC,
 835                        &FGInitialCondition::SetLongitudeDegIC,
 836                        true);
 837   PropertyManager->Tie("ic/h-sl-ft", this,
 838                        &FGInitialCondition::GetAltitudeFtIC,
 839                        &FGInitialCondition::SetAltitudeFtIC,
 840                        true);
 841   PropertyManager->Tie("ic/h-agl-ft", this,
 842                        &FGInitialCondition::GetAltitudeAGLFtIC,
 843                        &FGInitialCondition::SetAltitudeAGLFtIC,
 844                        true);
 845   PropertyManager->Tie("ic/sea-level-radius-ft", this,
 846                        &FGInitialCondition::GetSeaLevelRadiusFtIC,
 847                        &FGInitialCondition::SetSeaLevelRadiusFtIC,
 848                        true);
 849   PropertyManager->Tie("ic/terrain-altitude-ft", this,
 850                        &FGInitialCondition::GetTerrainAltitudeFtIC,
 851                        &FGInitialCondition::SetTerrainAltitudeFtIC,
 852                        true);
 853   PropertyManager->Tie("ic/vg-fps", this,
 854                        &FGInitialCondition::GetVgroundFpsIC,
 855                        &FGInitialCondition::SetVgroundFpsIC,
 856                        true);
 857   PropertyManager->Tie("ic/vt-fps", this,
 858                        &FGInitialCondition::GetVtrueFpsIC,
 859                        &FGInitialCondition::SetVtrueFpsIC,
 860                        true);
 861   PropertyManager->Tie("ic/vw-bx-fps", this,
 862                        &FGInitialCondition::GetWindUFpsIC);
 863   PropertyManager->Tie("ic/vw-by-fps", this,
 864                        &FGInitialCondition::GetWindVFpsIC);
 865   PropertyManager->Tie("ic/vw-bz-fps", this,
 866                        &FGInitialCondition::GetWindWFpsIC);
 867   PropertyManager->Tie("ic/vw-north-fps", this,
 868                        &FGInitialCondition::GetWindNFpsIC);
 869   PropertyManager->Tie("ic/vw-east-fps", this,
 870                        &FGInitialCondition::GetWindEFpsIC);
 871   PropertyManager->Tie("ic/vw-down-fps", this,
 872                        &FGInitialCondition::GetWindDFpsIC);
 873   PropertyManager->Tie("ic/vw-mag-fps", this,
 874                        &FGInitialCondition::GetWindFpsIC);
 875  /*  PropertyManager->Tie("ic/vw-dir-deg", this,
 876                        &FGInitialCondition::GetWindDirDegIC,
 877                        &FGInitialCondition::SetWindDirDegIC,
 878                        true); */
 879
 880   PropertyManager->Tie("ic/roc-fps", this,
 881                        &FGInitialCondition::GetClimbRateFpsIC,
 882                        &FGInitialCondition::SetClimbRateFpsIC,
 883                        true);
 884   /* PropertyManager->Tie("ic/u-fps", this,
 885                        &FGInitialCondition::GetUBodyFpsIC,
 886                        &FGInitialCondition::SetUBodyFpsIC,
 887                        true);
 888   PropertyManager->Tie("ic/v-fps", this,
 889                        &FGInitialCondition::GetVBodyFpsIC,
 890                        &FGInitialCondition::SetVBodyFpsIC,
 891                        true);
 892   PropertyManager->Tie("ic/w-fps", this,
 893                        &FGInitialCondition::GetWBodyFpsIC,
 894                        &FGInitialCondition::SetWBodyFpsIC,
 895                        true); */
 896
 897   PropertyManager->Tie("ic/gamma-rad", this,
 898                        &FGInitialCondition::GetFlightPathAngleRadIC,
 899                        &FGInitialCondition::SetFlightPathAngleRadIC,
 900                        true);
 901   PropertyManager->Tie("ic/alpha-rad", this,
 902                        &FGInitialCondition::GetAlphaRadIC,
 903                        &FGInitialCondition::SetAlphaRadIC,
 904                        true);
 905   PropertyManager->Tie("ic/theta-rad", this,
 906                        &FGInitialCondition::GetPitchAngleRadIC,
 907                        &FGInitialCondition::SetPitchAngleRadIC,
 908                        true);
 909   PropertyManager->Tie("ic/beta-rad", this,
 910                        &FGInitialCondition::GetBetaRadIC,
 911                        &FGInitialCondition::SetBetaRadIC,
 912                        true);
 913   PropertyManager->Tie("ic/phi-rad", this,
 914                        &FGInitialCondition::GetRollAngleRadIC,
 915                        &FGInitialCondition::SetRollAngleRadIC,
 916                        true);
 917   PropertyManager->Tie("ic/psi-true-rad", this,
 918                        &FGInitialCondition::GetHeadingRadIC);
 919   PropertyManager->Tie("ic/lat-gc-rad", this,
 920                        &FGInitialCondition::GetLatitudeRadIC,
 921                        &FGInitialCondition::SetLatitudeRadIC,
 922                        true);
 923   PropertyManager->Tie("ic/long-gc-rad", this,
 924                        &FGInitialCondition::GetLongitudeRadIC,
 925                        &FGInitialCondition::SetLongitudeRadIC,
 926                        true);
 927   PropertyManager->Tie("ic/p-rad_sec", this,
 928                        &FGInitialCondition::GetPRadpsIC,
 929                        &FGInitialCondition::SetPRadpsIC,
 930                        true);
 931   PropertyManager->Tie("ic/q-rad_sec", this,
 932                        &FGInitialCondition::GetQRadpsIC,
 933                        &FGInitialCondition::SetQRadpsIC,
 934                        true);
 935   PropertyManager->Tie("ic/r-rad_sec", this,
 936                        &FGInitialCondition::GetRRadpsIC,
 937                        &FGInitialCondition::SetRRadpsIC,
 938                        true);
 939
 940 }
 941
 942 //******************************************************************************
 943
 944 void FGInitialCondition::unbind(void){
 945   PropertyManager->Untie("ic/vc-kts");
 946   PropertyManager->Untie("ic/ve-kts");
 947   PropertyManager->Untie("ic/vg-kts");
 948   PropertyManager->Untie("ic/vt-kts");
 949   PropertyManager->Untie("ic/mach-norm");
 950   PropertyManager->Untie("ic/roc-fpm");
 951   PropertyManager->Untie("ic/gamma-deg");
 952   PropertyManager->Untie("ic/alpha-deg");
 953   PropertyManager->Untie("ic/beta-deg");
 954   PropertyManager->Untie("ic/theta-deg");
 955   PropertyManager->Untie("ic/phi-deg");
 956   PropertyManager->Untie("ic/psi-true-deg");
 957   PropertyManager->Untie("ic/lat-gc-deg");
 958   PropertyManager->Untie("ic/long-gc-deg");
 959   PropertyManager->Untie("ic/h-sl-ft");
 960   PropertyManager->Untie("ic/h-agl-ft");
 961   PropertyManager->Untie("ic/sea-level-radius-ft");
 962   PropertyManager->Untie("ic/terrain-altitude-ft");
 963   PropertyManager->Untie("ic/vg-fps");
 964   PropertyManager->Untie("ic/vt-fps");
 965   PropertyManager->Untie("ic/vw-bx-fps");
 966   PropertyManager->Untie("ic/vw-by-fps");
 967   PropertyManager->Untie("ic/vw-bz-fps");
 968   PropertyManager->Untie("ic/vw-north-fps");
 969   PropertyManager->Untie("ic/vw-east-fps");
 970   PropertyManager->Untie("ic/vw-down-fps");
 971   PropertyManager->Untie("ic/vw-mag-fps");
 972   /* PropertyManager->Untie("ic/vw-dir-deg"); */
 973
 974   PropertyManager->Untie("ic/roc-fps");
 975
 976   /*  PropertyManager->Untie("ic/u-fps");
 977   PropertyManager->Untie("ic/v-fps");
 978   PropertyManager->Untie("ic/w-fps"); */
 979
 980   PropertyManager->Untie("ic/gamma-rad");
 981   PropertyManager->Untie("ic/alpha-rad");
 982   PropertyManager->Untie("ic/theta-rad");
 983   PropertyManager->Untie("ic/beta-rad");
 984   PropertyManager->Untie("ic/phi-rad");
 985   PropertyManager->Untie("ic/psi-true-rad");
 986   PropertyManager->Untie("ic/lat-gc-rad");
 987   PropertyManager->Untie("ic/long-gc-rad");
 988   PropertyManager->Untie("ic/p-rad_sec");
 989   PropertyManager->Untie("ic/q-rad_sec");
 990   PropertyManager->Untie("ic/r-rad_sec");
 991
 992 }
 993
 994 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 995 //    The bitmasked value choices are as follows:
 996 //    unset: In this case (the default) JSBSim would only print
 997 //       out the normally expected messages, essentially echoing
 998 //       the config files as they are read. If the environment
 999 //       variable is not set, debug_lvl is set to 1 internally
1000 //    0: This requests JSBSim not to output any messages
1001 //       whatsoever.
1002 //    1: This value explicity requests the normal JSBSim
1003 //       startup messages
1004 //    2: This value asks for a message to be printed out when
1005 //       a class is instantiated
1006 //    4: When this value is set, a message is displayed when a
1007 //       FGModel object executes its Run() method
1008 //    8: When this value is set, various runtime state variables
1009 //       are printed out periodically
1010 //    16: When set various parameters are sanity checked and
1011 //       a message is printed out when they go out of bounds
1012
1013 void FGInitialCondition::Debug(int from)
1014 {
1015   if (debug_lvl <= 0) return;
1016
1017   if (debug_lvl & 1) { // Standard console startup message output
1018   }
1019   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
1020     if (from == 0) cout << "Instantiated: FGInitialCondition" << endl;
1021     if (from == 1) cout << "Destroyed:    FGInitialCondition" << endl;
1022   }
1023   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
1024   }
1025   if (debug_lvl & 8 ) { // Runtime state variables
1026   }
1027   if (debug_lvl & 16) { // Sanity checking
1028   }
1029   if (debug_lvl & 64) {
1030     if (from == 0) { // Constructor
1031       cout << IdSrc << endl;
1032       cout << IdHdr << endl;
1033     }
1034   }
1035 }
1036 }