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