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