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