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