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