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