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 = "$Header$";
  59 static const char *IdHdr = ID_INITIALCONDITION;
  60
  61
  62 FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) {
  63   vt=vc=ve=0;
  64   mach=0;
  65   alpha=beta=gamma=0;
  66   theta=phi=psi=0;
  67   altitude=hdot=0;
  68   latitude=longitude=0;
  69   u=v=w=0;
  70   vnorth=veast=vdown=0;
  71   lastSpeedSet=setvt;
  72   sea_level_radius = EARTHRAD;
  73   radius_to_vehicle = EARTHRAD;
  74   terrain_altitude = 0;
  75   if(FDMExec != NULL ) {
  76     fdmex=FDMExec;
  77     fdmex->GetPosition()->Seth(altitude);
  78     fdmex->GetAtmosphere()->Run();
  79   } else {
  80     cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
  81   }
  82
  83 }
  84
  85
  86 FGInitialCondition::~FGInitialCondition(void) {}
  87
  88
  89 void FGInitialCondition::SetVcalibratedKtsIC(float tt) {
  90
  91   if(getMachFromVcas(&mach,tt*jsbKTSTOFPS)) {
  92     //cout << "Mach: " << mach << endl;
  93     lastSpeedSet=setvc;
  94     vc=tt*jsbKTSTOFPS;
  95     vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
  96     ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
  97     //cout << "Vt: " << vt*jsbFPSTOKTS << " Vc: " << vc*jsbFPSTOKTS << endl;
  98   }
  99   else {
 100     cout << "Failed to get Mach number for given Vc and altitude, Vc unchanged." << endl;
 101     cout << "Please mail the set of initial conditions used to apeden@earthlink.net" << endl;
 102   }
 103 }
 104
 105 void FGInitialCondition::SetVequivalentKtsIC(float tt) {
 106   ve=tt*jsbKTSTOFPS;
 107   lastSpeedSet=setve;
 108   vt=ve*1/sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 109   mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
 110   vc=calcVcas(mach);
 111 }
 112
 113 void FGInitialCondition::SetVtrueKtsIC(float tt) {
 114   vt=tt*jsbKTSTOFPS;
 115   lastSpeedSet=setvt;
 116   mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
 117   vc=calcVcas(mach);
 118   ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 119 }
 120
 121 void FGInitialCondition::SetMachIC(float tt) {
 122   mach=tt;
 123   lastSpeedSet=setmach;
 124   vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
 125   vc=calcVcas(mach);
 126   ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 127   //cout << "Vt: " << vt*jsbFPSTOKTS << " Vc: " << vc*jsbFPSTOKTS << endl;
 128 }
 129
 130 void FGInitialCondition::SetClimbRateFpmIC(float tt) {
 131   SetClimbRateFpsIC(tt/60.0);
 132 }
 133
 134 void FGInitialCondition::SetClimbRateFpsIC(float tt) {
 135
 136   if(vt > 0.1) {
 137     hdot=tt;
 138     gamma=asin(hdot/vt);
 139   }
 140 }
 141
 142 void FGInitialCondition::SetFlightPathAngleRadIC(float tt) {
 143   gamma=tt;
 144   getTheta();
 145   hdot=vt*sin(tt);
 146 }
 147
 148
 149 void FGInitialCondition::SetUBodyFpsIC(float tt) {
 150   u=tt;
 151   vt=sqrt(u*u+v*v+w*w);
 152   lastSpeedSet=setuvw;
 153 }
 154
 155
 156 void FGInitialCondition::SetVBodyFpsIC(float tt) {
 157   v=tt;
 158   vt=sqrt(u*u+v*v+w*w);
 159   lastSpeedSet=setuvw;
 160 }
 161
 162 void FGInitialCondition::SetWBodyFpsIC(float tt) {
 163   w=tt;
 164   vt=sqrt(u*u+v*v+w*w);
 165   lastSpeedSet=setuvw;
 166 }
 167
 168
 169 void FGInitialCondition::SetAltitudeFtIC(float tt) {
 170   altitude=tt;
 171   fdmex->GetPosition()->Seth(altitude);
 172   fdmex->GetAtmosphere()->Run();
 173
 174   //lets try to make sure the user gets what they intended
 175
 176   switch(lastSpeedSet) {
 177   case setned:
 178   case setuvw:
 179   case setvt:
 180     SetVtrueKtsIC(vt*jsbFPSTOKTS);
 181     break;
 182   case setvc:
 183     SetVcalibratedKtsIC(vc*jsbFPSTOKTS);
 184     break;
 185   case setve:
 186     SetVequivalentKtsIC(ve*jsbFPSTOKTS);
 187     break;
 188   case setmach:
 189     SetMachIC(mach);
 190     break;
 191   }
 192 }
 193
 194 void FGInitialCondition::SetAltitudeAGLFtIC(float tt) {
 195   fdmex->GetPosition()->SetDistanceAGL(tt);
 196   altitude=fdmex->GetPosition()->Geth();
 197   SetAltitudeFtIC(altitude);
 198 }
 199 void FGInitialCondition::SetSeaLevelRadiusFtIC(double tt) {
 200   sea_level_radius = tt;
 201 }
 202
 203 void FGInitialCondition::SetTerrainAltitudeFtIC(double tt) {
 204   terrain_altitude=tt;
 205   fdmex->GetPosition()->SetDistanceAGL(altitude-terrain_altitude);
 206   fdmex->GetPosition()->SetRunwayRadius(sea_level_radius + terrain_altitude);
 207 }
 208
 209 void FGInitialCondition::calcUVWfromNED(void) {
 210   u=vnorth*cos(theta)*cos(psi) +
 211      veast*cos(theta)*sin(psi) -
 212      vdown*sin(theta);
 213   v=vnorth*(sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi)) +
 214      veast*(sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi)) +
 215      vdown*sin(phi)*cos(theta);
 216   w=vnorth*(cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi)) +
 217      veast*(cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) +
 218      vdown*cos(phi)*cos(theta);
 219 }
 220
 221 void FGInitialCondition::SetVnorthFpsIC(float tt) {
 222   vnorth=tt;
 223   calcUVWfromNED();
 224   vt=sqrt(u*u + v*v + w*w);
 225   lastSpeedSet=setned;
 226 }
 227
 228 void FGInitialCondition::SetVeastFpsIC(float tt) {
 229   veast=tt;
 230   calcUVWfromNED();
 231   vt=sqrt(u*u + v*v + w*w);
 232   lastSpeedSet=setned;
 233 }
 234
 235 void FGInitialCondition::SetVdownFpsIC(float tt) {
 236   vdown=tt;
 237   calcUVWfromNED();
 238   vt=sqrt(u*u + v*v + w*w);
 239   SetClimbRateFpsIC(-1*vdown);
 240   lastSpeedSet=setned;
 241 }
 242
 243 bool FGInitialCondition::getMachFromVcas(float *Mach,float vcas) {
 244
 245   bool result=false;
 246   float guess=1.5;
 247   xlo=xhi=0;
 248   xmin=0;xmax=50;
 249   sfunc=&FGInitialCondition::calcVcas;
 250   if(findInterval(vcas,guess)) {
 251     if(solve(&mach,vcas))
 252       result=true;
 253   }
 254   return result;
 255 }
 256
 257 bool FGInitialCondition::getAlpha(void) {
 258   bool result=false;
 259   float guess=theta-gamma;
 260   xlo=xhi=0;
 261   xmin=fdmex->GetAircraft()->GetAlphaCLMin();
 262   xmax=fdmex->GetAircraft()->GetAlphaCLMax();
 263   sfunc=&FGInitialCondition::GammaEqOfAlpha;
 264   if(findInterval(0,guess)){
 265     if(solve(&alpha,0)){
 266       result=true;
 267     }
 268   }
 269   return result;
 270 }
 271
 272 bool FGInitialCondition::getTheta(void) {
 273   bool result=false;
 274   float guess=alpha+gamma;
 275   xlo=xhi=0;
 276   xmin=-89;xmax=89;
 277   sfunc=&FGInitialCondition::GammaEqOfTheta;
 278   if(findInterval(0,guess)){
 279     if(solve(&theta,0)){
 280       result=true;
 281     }
 282   }
 283   return result;
 284 }
 285
 286
 287
 288 float FGInitialCondition::GammaEqOfTheta(float Theta) {
 289   float a,b,c;
 290
 291   a=cos(alpha)*cos(beta)*sin(Theta);
 292   b=sin(beta)*sin(phi);
 293   c=sin(alpha)*cos(beta)*cos(phi);
 294   return sin(gamma)-a+(b+c)*cos(Theta);
 295 }
 296
 297 float FGInitialCondition::GammaEqOfAlpha(float Alpha) {
 298   float a,b,c;
 299
 300   a=cos(Alpha)*cos(beta)*sin(theta);
 301   b=sin(beta)*sin(phi);
 302   c=sin(Alpha)*cos(beta)*cos(phi);
 303   return sin(gamma)-a+(b+c)*cos(theta);
 304 }
 305
 306
 307 float FGInitialCondition::calcVcas(float Mach) {
 308
 309   float p=fdmex->GetAtmosphere()->GetPressure();
 310   float psl=fdmex->GetAtmosphere()->GetPressureSL();
 311   float rhosl=fdmex->GetAtmosphere()->GetDensitySL();
 312   float pt,A,B,D,vcas;
 313   if(Mach < 0) Mach=0;
 314   if(Mach < 1)    //calculate total pressure assuming isentropic flow
 315     pt=p*pow((1 + 0.2*Mach*Mach),3.5);
 316   else {
 317     // shock in front of pitot tube, we'll assume its normal and use
 318     // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
 319     // pressure behind the shock to the static pressure in front
 320
 321
 322     //the normal shock assumption should not be a bad one -- most supersonic
 323     //aircraft place the pitot probe out front so that it is the forward
 324     //most point on the aircraft.  The real shock would, of course, take
 325     //on something like the shape of a rounded-off cone but, here again,
 326     //the assumption should be good since the opening of the pitot probe
 327     //is very small and, therefore, the effects of the shock curvature
 328     //should be small as well. AFAIK, this approach is fairly well accepted
 329     //within the aerospace community
 330
 331     B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);
 332
 333     // The denominator above is zero for Mach ~ 0.38, for which
 334     // we'll never be here, so we're safe
 335
 336     D = (2.8*Mach*Mach-0.4)*0.4167;
 337     pt = p*pow(B,3.5)*D;
 338   }
 339
 340   A = pow(((pt-p)/psl+1),0.28571);
 341   vcas = sqrt(7*psl/rhosl*(A-1));
 342   //cout << "calcVcas: vcas= " << vcas*jsbFPSTOKTS << " mach= " << Mach << " pressure: " << pt << endl;
 343   return vcas;
 344 }
 345
 346 bool FGInitialCondition::findInterval(float x,float guess) {
 347   //void find_interval(inter_params &ip,eqfunc f,float y,float constant, int &flag){
 348
 349   int i=0;
 350   bool found=false;
 351   float flo,fhi,fguess;
 352   float lo,hi,step;
 353   step=0.1;
 354   fguess=(this->*sfunc)(guess)-x;
 355   lo=hi=guess;
 356   do {
 357     step=2*step;
 358     lo-=step;
 359     hi+=step;
 360     if(lo < xmin) lo=xmin;
 361     if(hi > xmax) hi=xmax;
 362     i++;
 363     flo=(this->*sfunc)(lo)-x;
 364     fhi=(this->*sfunc)(hi)-x;
 365     if(flo*fhi <=0) {  //found interval with root
 366       found=true;
 367       if(flo*fguess <= 0) {  //narrow interval down a bit
 368         hi=lo+step;    //to pass solver interval that is as
 369         //small as possible
 370       }
 371       else if(fhi*fguess <= 0) {
 372         lo=hi-step;
 373       }
 374     }
 375     //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
 376   }
 377   while((found == 0) && (i <= 100));
 378   xlo=lo;
 379   xhi=hi;
 380   return found;
 381 }
 382
 383
 384
 385
 386 bool FGInitialCondition::solve(float *y,float x) {
 387   float x1,x2,x3,f1,f2,f3,d,d0;
 388   float eps=1E-5;
 389   float const relax =0.9;
 390   int i;
 391   bool success=false;
 392
 393    //initializations
 394   d=1;
 395
 396     x1=xlo;x3=xhi;
 397     f1=(this->*sfunc)(x1)-x;
 398     f3=(this->*sfunc)(x3)-x;
 399     d0=fabs(x3-x1);
 400
 401     //iterations
 402     i=0;
 403     while ((fabs(d) > eps) && (i < 100)) {
 404       d=(x3-x1)/d0;
 405       x2=x1-d*d0*f1/(f3-f1);
 406
 407       f2=(this->*sfunc)(x2)-x;
 408       //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
 409       //cout << "                " << f1 << "," << f2 << "," << f3 << endl;
 410
 411       if(fabs(f2) <= 0.001) {
 412         x1=x3=x2;
 413       } else if(f1*f2 <= 0.0) {
 414         x3=x2;
 415         f3=f2;
 416         f1=relax*f1;
 417       } else if(f2*f3 <= 0) {
 418         x1=x2;
 419         f1=f2;
 420         f3=relax*f3;
 421       }
 422       //cout << i << endl;
 423       i++;
 424     }//end while
 425     if(i < 100) {
 426       success=true;
 427       *y=x2;
 428     }
 429
 430   //cout << "Success= " << success << " Vcas: " << vcas*jsbFPSTOKTS << " Mach: " << x2 << endl;
 431   return success;
 432 }