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