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