src/FDM/JSBSim/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 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 General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU 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 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 "FGState.h"
  48 #include "FGAtmosphere.h"
  49 #include "FGFCS.h"
  50 #include "FGAircraft.h"
  51 #include "FGTranslation.h"
  52 #include "FGRotation.h"
  53 #include "FGPosition.h"
  54 #include "FGAuxiliary.h"
  55 #include "FGOutput.h"
  56 #include "FGDefs.h"
  57
  58 static const char *IdSrc = "$Id$";
  59 static const char *IdHdr = ID_INITIALCONDITION;
  60
  61 extern short debug_lvl;
  62
  63 //******************************************************************************
  64
  65 FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec){
  66
  67   vt=vc=ve=vg=0;
  68   mach=0;
  69   alpha=beta=gamma=0;
  70   theta=phi=psi=0;
  71   altitude=hdot=0;
  72   latitude=longitude=0;
  73   u=v=w=0;
  74   vw=vw=ww=0;
  75   vnorth=veast=vdown=0;
  76   lastSpeedSet=setvt;
  77   sea_level_radius = EARTHRAD;
  78   radius_to_vehicle = EARTHRAD;
  79   terrain_altitude = 0;
  80
  81   salpha=sbeta=stheta=sphi=spsi=sgamma=0;
  82   calpha=cbeta=ctheta=cphi=cpsi=cgamma=1;
  83
  84   if(FDMExec != NULL ) {
  85     fdmex=FDMExec;
  86     fdmex->GetPosition()->Seth(altitude);
  87     fdmex->GetAtmosphere()->Run();
  88   } else {
  89     cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
  90   }
  91
  92   if (debug_lvl & 2) cout << "Instantiated: FGInitialCondition" << endl;
  93 }
  94
  95 //******************************************************************************
  96
  97 FGInitialCondition::~FGInitialCondition()
  98 {
  99   if (debug_lvl & 2) cout << "Destroyed:    FGInitialCondition" << endl;
 100 }
 101
 102 //******************************************************************************
 103
 104 void FGInitialCondition::SetVcalibratedKtsIC(float tt) {
 105
 106   if(getMachFromVcas(&mach,tt*jsbKTSTOFPS)) {
 107     //cout << "Mach: " << mach << endl;
 108     lastSpeedSet=setvc;
 109     vc=tt*jsbKTSTOFPS;
 110     vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
 111     ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 112     //cout << "Vt: " << vt*jsbFPSTOKTS << " Vc: " << vc*jsbFPSTOKTS << endl;
 113   }
 114   else {
 115     cout << "Failed to get Mach number for given Vc and altitude, Vc unchanged." << endl;
 116     cout << "Please mail the set of initial conditions used to apeden@earthlink.net" << endl;
 117   }
 118 }
 119
 120 //******************************************************************************
 121
 122 void FGInitialCondition::SetVequivalentKtsIC(float tt) {
 123   ve=tt*jsbKTSTOFPS;
 124   lastSpeedSet=setve;
 125   vt=ve*1/sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 126   mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
 127   vc=calcVcas(mach);
 128 }
 129
 130 //******************************************************************************
 131
 132 void FGInitialCondition::SetVgroundFpsIC(float tt) {
 133   //float ua,va,wa;
 134   float vxz;
 135
 136   //cout << "FGInitialCondition::SetVgroundFpsIC" << endl;
 137   vg=tt;
 138   lastSpeedSet=setvg;
 139   vnorth = vg*cos(psi); veast = vg*sin(psi); vdown = 0;
 140   calcUVWfromNED();
 141   //cout << "\tu,v,w: " << u << ", " << v << ", " << w << endl;
 142   calcWindUVW();
 143   //cout << "\tuw,vw,ww: " << uw << ", " << vw << ", " << ww << endl;
 144   u = -uw; v = -vw; w = -ww;
 145   //ua = u - uw; va = v - vw; wa = w - ww;
 146   //cout << "\tua,va,wa: " << ua << ", " << va << ", " << wa << endl;
 147   vt = sqrt( u*u + v*v + w*w );
 148   alpha = beta = 0;
 149   vxz = sqrt( u*u + w*w );
 150   if( w != 0 ) alpha = atan2( w, u );
 151   if( vxz != 0 ) beta = atan2( v, vxz );
 152   //cout << "\tvt,alpha,beta: " << vt << ", " << alpha*RADTODEG << ", "
 153   //          << beta*RADTODEG << endl;
 154   mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
 155   vc=calcVcas(mach);
 156   ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 157 }
 158
 159 //******************************************************************************
 160
 161 void FGInitialCondition::SetVtrueFpsIC(float tt) {
 162   vt=tt;
 163   lastSpeedSet=setvt;
 164   mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
 165   vc=calcVcas(mach);
 166   ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 167 }
 168
 169 //******************************************************************************
 170
 171 void FGInitialCondition::SetMachIC(float tt) {
 172   mach=tt;
 173   lastSpeedSet=setmach;
 174   vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
 175   vc=calcVcas(mach);
 176   ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 177   //cout << "Vt: " << vt*jsbFPSTOKTS << " Vc: " << vc*jsbFPSTOKTS << endl;
 178 }
 179
 180 //******************************************************************************
 181
 182 void FGInitialCondition::SetClimbRateFpmIC(float tt) {
 183   SetClimbRateFpsIC(tt/60.0);
 184 }
 185
 186 //******************************************************************************
 187
 188 void FGInitialCondition::SetClimbRateFpsIC(float tt) {
 189
 190   if(vt > 0.1) {
 191     hdot=tt;
 192     gamma=asin(hdot/vt);
 193     sgamma=sin(gamma); cgamma=cos(gamma);
 194   }
 195 }
 196
 197 //******************************************************************************
 198
 199 void FGInitialCondition::SetFlightPathAngleRadIC(float tt) {
 200   gamma=tt;
 201   sgamma=sin(gamma); cgamma=cos(gamma);
 202   getTheta();
 203   hdot=vt*sgamma;
 204 }
 205
 206 //******************************************************************************
 207
 208 void FGInitialCondition::SetAlphaRadIC(float tt) {
 209   alpha=tt;
 210   salpha=sin(alpha); calpha=cos(alpha);
 211   getTheta();
 212 }
 213
 214 //******************************************************************************
 215
 216 void FGInitialCondition::SetPitchAngleRadIC(float tt) {
 217   theta=tt;
 218   stheta=sin(theta); ctheta=cos(theta);
 219   calcWindUVW();
 220   getAlpha();
 221 }
 222
 223 //******************************************************************************
 224
 225 void FGInitialCondition::SetBetaRadIC(float tt) {
 226   beta=tt;
 227   sbeta=sin(beta); cbeta=cos(beta);
 228   getTheta();
 229 }
 230
 231 //******************************************************************************
 232
 233 void FGInitialCondition::SetRollAngleRadIC(float tt) {
 234   phi=tt;
 235   sphi=sin(phi); cphi=cos(phi);
 236   getTheta();
 237 }
 238
 239 //******************************************************************************
 240
 241 void FGInitialCondition::SetTrueHeadingRadIC(float tt) {
 242     psi=tt;
 243     spsi=sin(psi); cpsi=cos(psi);
 244     calcWindUVW();
 245 }
 246
 247 //******************************************************************************
 248
 249 void FGInitialCondition::SetUBodyFpsIC(float tt) {
 250   u=tt;
 251   vt=sqrt(u*u + v*v + w*w);
 252   lastSpeedSet=setuvw;
 253 }
 254
 255 //******************************************************************************
 256
 257 void FGInitialCondition::SetVBodyFpsIC(float tt) {
 258   v=tt;
 259   vt=sqrt(u*u + v*v + w*w);
 260   lastSpeedSet=setuvw;
 261 }
 262
 263 //******************************************************************************
 264
 265 void FGInitialCondition::SetWBodyFpsIC(float tt) {
 266   w=tt;
 267   vt=sqrt( u*u + v*v + w*w );
 268   lastSpeedSet=setuvw;
 269 }
 270
 271 //******************************************************************************
 272
 273 float FGInitialCondition::GetUBodyFpsIC(void) {
 274     if(lastSpeedSet == setvg )
 275       return u;
 276     else
 277       return vt*calpha*cbeta;
 278 }
 279
 280 //******************************************************************************
 281
 282 float FGInitialCondition::GetVBodyFpsIC(void) {
 283     if( lastSpeedSet == setvg )
 284       return v;
 285     else
 286       return vt*sbeta;
 287 }
 288
 289 //******************************************************************************
 290
 291 float FGInitialCondition::GetWBodyFpsIC(void) {
 292     if( lastSpeedSet == setvg )
 293       return w;
 294     else {
 295       return vt*salpha*cbeta;
 296    }
 297 }
 298
 299 //******************************************************************************
 300
 301 void FGInitialCondition::SetWindNEDFpsIC(float wN, float wE, float wD ) {
 302   wnorth = wN; weast = wE; wdown = wD;
 303   if(lastSpeedSet == setvg)
 304     SetVgroundFpsIC(vg);
 305
 306
 307 }
 308
 309 //******************************************************************************
 310
 311 void FGInitialCondition::calcWindUVW(void) {
 312   if(lastSpeedSet == setvg ) {
 313
 314     uw=wnorth*ctheta*cpsi +
 315        weast*ctheta*spsi -
 316        wdown*stheta;
 317     vw=wnorth*( sphi*stheta*cpsi - cphi*spsi ) +
 318         weast*( sphi*stheta*spsi + cphi*cpsi ) +
 319        wdown*sphi*ctheta;
 320     ww=wnorth*(cphi*stheta*cpsi + sphi*spsi) +
 321        weast*(cphi*stheta*spsi - sphi*cpsi) +
 322        wdown*cphi*ctheta;
 323     /* cout << "FGInitialCondition::calcWindUVW: wnorth, weast, wdown "
 324          << wnorth << ", " << weast << ", " << wdown << endl;
 325     cout << "FGInitialCondition::calcWindUVW: theta, phi, psi "
 326           << theta << ", " << phi << ", " << psi << endl;
 327     cout << "FGInitialCondition::calcWindUVW: uw, vw, ww "
 328           << uw << ", " << vw << ", " << ww << endl;   */
 329
 330   } else {
 331     uw=vw=ww=0;
 332   }
 333 }
 334
 335 //******************************************************************************
 336
 337 void FGInitialCondition::SetAltitudeFtIC(float tt) {
 338   altitude=tt;
 339   fdmex->GetPosition()->Seth(altitude);
 340   fdmex->GetAtmosphere()->Run();
 341
 342   //lets try to make sure the user gets what they intended
 343
 344   switch(lastSpeedSet) {
 345   case setned:
 346   case setuvw:
 347   case setvt:
 348     SetVtrueKtsIC(vt*jsbFPSTOKTS);
 349     break;
 350   case setvc:
 351     SetVcalibratedKtsIC(vc*jsbFPSTOKTS);
 352     break;
 353   case setve:
 354     SetVequivalentKtsIC(ve*jsbFPSTOKTS);
 355     break;
 356   case setmach:
 357     SetMachIC(mach);
 358     break;
 359   case setvg:
 360     SetVgroundFpsIC(vg);
 361     break;
 362   }
 363 }
 364
 365 //******************************************************************************
 366
 367 void FGInitialCondition::SetAltitudeAGLFtIC(float tt) {
 368   fdmex->GetPosition()->SetDistanceAGL(tt);
 369   altitude=fdmex->GetPosition()->Geth();
 370   SetAltitudeFtIC(altitude);
 371 }
 372
 373 //******************************************************************************
 374
 375 void FGInitialCondition::SetSeaLevelRadiusFtIC(double tt) {
 376   sea_level_radius = tt;
 377 }
 378
 379 //******************************************************************************
 380
 381 void FGInitialCondition::SetTerrainAltitudeFtIC(double tt) {
 382   terrain_altitude=tt;
 383   fdmex->GetPosition()->SetDistanceAGL(altitude-terrain_altitude);
 384   fdmex->GetPosition()->SetRunwayRadius(sea_level_radius + terrain_altitude);
 385 }
 386
 387 //******************************************************************************
 388
 389 void FGInitialCondition::calcUVWfromNED(void) {
 390   u=vnorth*ctheta*cpsi +
 391      veast*ctheta*spsi -
 392      vdown*stheta;
 393   v=vnorth*( sphi*stheta*cpsi - cphi*spsi ) +
 394      veast*( sphi*stheta*spsi + cphi*cpsi ) +
 395      vdown*sphi*ctheta;
 396   w=vnorth*( cphi*stheta*cpsi + sphi*spsi ) +
 397      veast*( cphi*stheta*spsi - sphi*cpsi ) +
 398      vdown*cphi*ctheta;
 399 }
 400
 401 //******************************************************************************
 402
 403 void FGInitialCondition::SetVnorthFpsIC(float tt) {
 404   vnorth=tt;
 405   calcUVWfromNED();
 406   vt=sqrt(u*u + v*v + w*w);
 407   lastSpeedSet=setned;
 408 }
 409
 410 //******************************************************************************
 411
 412 void FGInitialCondition::SetVeastFpsIC(float tt) {
 413   veast=tt;
 414   calcUVWfromNED();
 415   vt=sqrt(u*u + v*v + w*w);
 416   lastSpeedSet=setned;
 417 }
 418
 419 //******************************************************************************
 420
 421 void FGInitialCondition::SetVdownFpsIC(float tt) {
 422   vdown=tt;
 423   calcUVWfromNED();
 424   vt=sqrt(u*u + v*v + w*w);
 425   SetClimbRateFpsIC(-1*vdown);
 426   lastSpeedSet=setned;
 427 }
 428
 429 //******************************************************************************
 430
 431 bool FGInitialCondition::getMachFromVcas(float *Mach,float vcas) {
 432
 433   bool result=false;
 434   float guess=1.5;
 435   xlo=xhi=0;
 436   xmin=0;xmax=50;
 437   sfunc=&FGInitialCondition::calcVcas;
 438   if(findInterval(vcas,guess)) {
 439     if(solve(&mach,vcas))
 440       result=true;
 441   }
 442   return result;
 443 }
 444
 445 //******************************************************************************
 446
 447 bool FGInitialCondition::getAlpha(void) {
 448   bool result=false;
 449   float guess=theta-gamma;
 450   xlo=xhi=0;
 451   xmin=fdmex->GetAircraft()->GetAlphaCLMin();
 452   xmax=fdmex->GetAircraft()->GetAlphaCLMax();
 453   sfunc=&FGInitialCondition::GammaEqOfAlpha;
 454   if(findInterval(0,guess)){
 455     if(solve(&alpha,0)){
 456       result=true;
 457       salpha=sin(alpha);
 458       calpha=cos(alpha);
 459     }
 460   }
 461   return result;
 462 }
 463
 464 //******************************************************************************
 465
 466 bool FGInitialCondition::getTheta(void) {
 467   bool result=false;
 468   float guess=alpha+gamma;
 469   xlo=xhi=0;
 470   xmin=-89;xmax=89;
 471   sfunc=&FGInitialCondition::GammaEqOfTheta;
 472   if(findInterval(0,guess)){
 473     if(solve(&theta,0)){
 474       result=true;
 475       stheta=sin(theta);
 476       ctheta=cos(theta);
 477     }
 478   }
 479   return result;
 480 }
 481
 482 //******************************************************************************
 483
 484 float FGInitialCondition::GammaEqOfTheta(float Theta) {
 485   float a,b,c,d;
 486   float sTheta,cTheta;
 487
 488   //theta=Theta; stheta=sin(theta); ctheta=cos(theta);
 489   sTheta=sin(Theta); cTheta=cos(Theta);
 490   calcWindUVW();
 491   a=wdown + vt*calpha*cbeta + uw;
 492   b=vt*sphi*sbeta + vw*sphi;
 493   c=vt*cphi*salpha*cbeta + ww*cphi;
 494   return vt*sgamma - ( a*sTheta - (b+c)*cTheta);
 495 }
 496
 497 //******************************************************************************
 498
 499 float FGInitialCondition::GammaEqOfAlpha(float Alpha) {
 500   float a,b,c,d;
 501   float sAlpha,cAlpha;
 502
 503   sAlpha=sin(Alpha); cAlpha=cos(Alpha);
 504   a=wdown + vt*cAlpha*cbeta + uw;
 505   b=vt*sphi*sbeta + vw*sphi;
 506   c=vt*cphi*sAlpha*cbeta + ww*cphi;
 507
 508   return vt*sgamma - ( a*stheta - (b+c)*ctheta );
 509 }
 510
 511 //******************************************************************************
 512
 513 float FGInitialCondition::calcVcas(float Mach) {
 514
 515   float p=fdmex->GetAtmosphere()->GetPressure();
 516   float psl=fdmex->GetAtmosphere()->GetPressureSL();
 517   float rhosl=fdmex->GetAtmosphere()->GetDensitySL();
 518   float pt,A,B,D,vcas;
 519   if(Mach < 0) Mach=0;
 520   if(Mach < 1)    //calculate total pressure assuming isentropic flow
 521     pt=p*pow((1 + 0.2*Mach*Mach),3.5);
 522   else {
 523     // shock in front of pitot tube, we'll assume its normal and use
 524     // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
 525     // pressure behind the shock to the static pressure in front
 526
 527
 528     //the normal shock assumption should not be a bad one -- most supersonic
 529     //aircraft place the pitot probe out front so that it is the forward
 530     //most point on the aircraft.  The real shock would, of course, take
 531     //on something like the shape of a rounded-off cone but, here again,
 532     //the assumption should be good since the opening of the pitot probe
 533     //is very small and, therefore, the effects of the shock curvature
 534     //should be small as well. AFAIK, this approach is fairly well accepted
 535     //within the aerospace community
 536
 537     B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);
 538
 539     // The denominator above is zero for Mach ~ 0.38, for which
 540     // we'll never be here, so we're safe
 541
 542     D = (2.8*Mach*Mach-0.4)*0.4167;
 543     pt = p*pow(B,3.5)*D;
 544   }
 545
 546   A = pow(((pt-p)/psl+1),0.28571);
 547   vcas = sqrt(7*psl/rhosl*(A-1));
 548   //cout << "calcVcas: vcas= " << vcas*jsbFPSTOKTS << " mach= " << Mach << " pressure: " << pt << endl;
 549   return vcas;
 550 }
 551
 552 //******************************************************************************
 553
 554 bool FGInitialCondition::findInterval(float x,float guess) {
 555   //void find_interval(inter_params &ip,eqfunc f,float y,float constant, int &flag){
 556
 557   int i=0;
 558   bool found=false;
 559   float flo,fhi,fguess;
 560   float lo,hi,step;
 561   step=0.1;
 562   fguess=(this->*sfunc)(guess)-x;
 563   lo=hi=guess;
 564   do {
 565     step=2*step;
 566     lo-=step;
 567     hi+=step;
 568     if(lo < xmin) lo=xmin;
 569     if(hi > xmax) hi=xmax;
 570     i++;
 571     flo=(this->*sfunc)(lo)-x;
 572     fhi=(this->*sfunc)(hi)-x;
 573     if(flo*fhi <=0) {  //found interval with root
 574       found=true;
 575       if(flo*fguess <= 0) {  //narrow interval down a bit
 576         hi=lo+step;    //to pass solver interval that is as
 577         //small as possible
 578       }
 579       else if(fhi*fguess <= 0) {
 580         lo=hi-step;
 581       }
 582     }
 583     //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
 584   }
 585   while((found == 0) && (i <= 100));
 586   xlo=lo;
 587   xhi=hi;
 588   return found;
 589 }
 590
 591 //******************************************************************************
 592
 593 bool FGInitialCondition::solve(float *y,float x) {
 594   float x1,x2,x3,f1,f2,f3,d,d0;
 595   float eps=1E-5;
 596   float const relax =0.9;
 597   int i;
 598   bool success=false;
 599
 600    //initializations
 601   d=1;
 602
 603     x1=xlo;x3=xhi;
 604     f1=(this->*sfunc)(x1)-x;
 605     f3=(this->*sfunc)(x3)-x;
 606     d0=fabs(x3-x1);
 607
 608     //iterations
 609     i=0;
 610     while ((fabs(d) > eps) && (i < 100)) {
 611       d=(x3-x1)/d0;
 612       x2=x1-d*d0*f1/(f3-f1);
 613
 614       f2=(this->*sfunc)(x2)-x;
 615       //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
 616       //cout << "                " << f1 << "," << f2 << "," << f3 << endl;
 617
 618       if(fabs(f2) <= 0.001) {
 619         x1=x3=x2;
 620       } else if(f1*f2 <= 0.0) {
 621         x3=x2;
 622         f3=f2;
 623         f1=relax*f1;
 624       } else if(f2*f3 <= 0) {
 625         x1=x2;
 626         f1=f2;
 627         f3=relax*f3;
 628       }
 629       //cout << i << endl;
 630       i++;
 631     }//end while
 632     if(i < 100) {
 633       success=true;
 634       *y=x2;
 635     }
 636
 637   //cout << "Success= " << success << " Vcas: " << vcas*jsbFPSTOKTS << " Mach: " << x2 << endl;
 638   return success;
 639 }
 640
 641 //******************************************************************************
 642
 643 void FGInitialCondition::Debug(void)
 644 {
 645     //TODO: Add your source code here
 646 }
 647