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 CAVEAT: This class makes use of alpha=theta-gamma. This means that setting
  42         any of the three with this class is only valid for steady state
  43         (all accels zero) and zero pitch rate.  One example where this
  44         would produce invalid results is setting up for a trim in a pull-up
  45         or pushover (both have nonzero pitch rate).  Maybe someday...
  46
  47 ********************************************************************************
  48 INCLUDES
  49 *******************************************************************************/
  50
  51 #include "FGInitialCondition.h"
  52 #include "FGFDMExec.h"
  53 #include "FGState.h"
  54 #include "FGAtmosphere.h"
  55 #include "FGFCS.h"
  56 #include "FGAircraft.h"
  57 #include "FGTranslation.h"
  58 #include "FGRotation.h"
  59 #include "FGPosition.h"
  60 #include "FGAuxiliary.h"
  61 #include "FGOutput.h"
  62 #include "FGDefs.h"
  63
  64 FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) {
  65   vt=vc=ve=0;
  66   mach=0;
  67   alpha=beta=gamma=0;
  68   theta=phi=psi=0;
  69   altitude=hdot=0;
  70   latitude=longitude=0;
  71   u=v=w=0;
  72   lastSpeedSet=setvt;
  73   if(FDMExec != NULL ) {
  74     fdmex=FDMExec;
  75     fdmex->GetPosition()->Seth(altitude);
  76     fdmex->GetAtmosphere()->Run();
  77   } else {
  78     cout << "FGInitialCondition: This class requires a pointer to an valid FGFDMExec object" << endl;
  79   }
  80
  81 }
  82
  83
  84 FGInitialCondition::~FGInitialCondition(void) {}
  85
  86
  87 void FGInitialCondition::SetVcalibratedKtsIC(float tt) {
  88
  89   if(getMachFromVcas(&mach,tt*KTSTOFPS)) {
  90     //cout << "Mach: " << mach << endl;
  91     lastSpeedSet=setvc;
  92     vc=tt*KTSTOFPS;
  93     vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
  94     ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
  95     //cout << "Vt: " << vt*FPSTOKTS << " Vc: " << vc*FPSTOKTS << endl;
  96   }
  97   else {
  98     cout << "Failed to get Mach number for given Vc and altitude, Vc unchanged." << endl;
  99     cout << "Please mail the set of initial conditions used to apeden@earthlink.net" << endl;
 100   }
 101 }
 102
 103 void FGInitialCondition::SetVequivalentKtsIC(float tt) {
 104   ve=tt*KTSTOFPS;
 105   lastSpeedSet=setve;
 106   vt=ve*1/sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 107   mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
 108   vc=calcVcas(mach);
 109 }
 110
 111 void FGInitialCondition::SetVtrueKtsIC(float tt) {
 112   vt=tt*KTSTOFPS;
 113   lastSpeedSet=setvt;
 114   mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
 115   vc=calcVcas(mach);
 116   ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 117 }
 118
 119 void FGInitialCondition::SetMachIC(float tt) {
 120   mach=tt;
 121   lastSpeedSet=setmach;
 122   vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
 123   vc=calcVcas(mach);
 124   ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
 125   //cout << "Vt: " << vt*FPSTOKTS << " Vc: " << vc*FPSTOKTS << endl;
 126 }
 127
 128
 129
 130 void FGInitialCondition::SetClimbRateFpmIC(float tt) {
 131
 132   if(vt > 0.1) {
 133     hdot=tt/60;
 134     gamma=asin(hdot/vt);
 135   }
 136 }
 137
 138 void FGInitialCondition::SetUBodyFpsIC(float tt) {
 139   u=tt;
 140   vt=sqrt(u*u+v*v+w*w);
 141   lastSpeedSet=setvt;
 142 }
 143
 144 void FGInitialCondition::SetVBodyFpsIC(float tt) {
 145   v=tt;
 146   vt=sqrt(u*u+v*v+w*w);
 147   lastSpeedSet=setvt;
 148 }
 149
 150 void FGInitialCondition::SetWBodyFpsIC(float tt) {
 151   w=tt;
 152   vt=sqrt(u*u+v*v+w*w);
 153   lastSpeedSet=setvt;
 154 }
 155
 156
 157 void FGInitialCondition::SetAltitudeFtIC(float tt) {
 158   altitude=tt;
 159   fdmex->GetPosition()->Seth(altitude);
 160   fdmex->GetAtmosphere()->Run();
 161
 162   //lets try to make sure the user gets what they intended
 163
 164   switch(lastSpeedSet) {
 165   case setvt:
 166     SetVtrueKtsIC(vt*FPSTOKTS);
 167     break;
 168   case setvc:
 169     SetVcalibratedKtsIC(vc*FPSTOKTS);
 170     break;
 171   case setve:
 172     SetVequivalentKtsIC(ve*FPSTOKTS);
 173     break;
 174   case setmach:
 175     SetMachIC(mach);
 176     break;
 177   }
 178 }
 179
 180 float FGInitialCondition::calcVcas(float Mach) {
 181
 182   float p=fdmex->GetAtmosphere()->GetPressure();
 183   float psl=fdmex->GetAtmosphere()->GetPressureSL();
 184   float rhosl=fdmex->GetAtmosphere()->GetDensitySL();
 185   float pt,A,B,D,vcas;
 186
 187   if(Mach < 1)    //calculate total pressure assuming isentropic flow
 188     pt=p*pow((1 + 0.2*Mach*Mach),3.5);
 189   else {
 190     // shock in front of pitot tube, we'll assume its normal and use
 191     // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
 192     // pressure behind the shock to the static pressure in front
 193
 194
 195     //the normal shock assumption should not be a bad one -- most supersonic
 196     //aircraft place the pitot probe out front so that it is the forward
 197     //most point on the aircraft.  The real shock would, of course, take
 198     //on something like the shape of a rounded-off cone but, here again,
 199     //the assumption should be good since the opening of the pitot probe
 200     //is very small and, therefore, the effects of the shock curvature
 201     //should be small as well. AFAIK, this approach is fairly well accepted
 202     //within the aerospace community
 203
 204     B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);
 205
 206     // The denominator above is zero for Mach ~ 0.38, for which
 207     // we'll never be here, so we're safe
 208
 209     D = (2.8*Mach*Mach-0.4)*0.4167;
 210     pt = p*pow(B,3.5)*D;
 211   }
 212
 213   A = pow(((pt-p)/psl+1),0.28571);
 214   vcas = sqrt(7*psl/rhosl*(A-1));
 215   //cout << "calcVcas: vcas= " << vcas*FPSTOKTS << " mach= " << Mach << " pressure: " << pt << endl;
 216   return vcas;
 217 }
 218
 219 bool FGInitialCondition::findMachInterval(float *mlo, float *mhi, float vcas) {
 220   //void find_interval(inter_params &ip,eqfunc f,float y,float constant, int &flag){
 221
 222   int i=0;
 223   bool found=false;
 224   float flo,fhi,fguess;
 225   float lo,hi,guess,step;
 226   step=0.1;
 227   guess=1.5;
 228   fguess=calcVcas(guess)-vcas;
 229   lo=hi=guess;
 230   do {
 231     step=2*step;
 232     lo-=step;
 233     if(lo < 0)
 234       lo=0;
 235     hi+=step;
 236     i++;
 237     flo=calcVcas(lo)-vcas;
 238     fhi=calcVcas(hi)-vcas;
 239     if(flo*fhi <=0) {  //found interval with root
 240       found=true;
 241       if(flo*fguess <= 0) {  //narrow interval down a bit
 242         hi=lo+step;    //to pass solver interval that is as
 243         //small as possible
 244       }
 245       else if(fhi*fguess <= 0) {
 246         lo=hi-step;
 247       }
 248     }
 249     //cout << "findMachInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
 250   }
 251   while((found == 0) && (i <= 100));
 252   *mlo=lo;
 253   *mhi=hi;
 254   return found;
 255 }
 256
 257
 258
 259 bool FGInitialCondition::getMachFromVcas(float *Mach,float vcas) {
 260
 261
 262   float x1,x2,x3,f1,f2,f3,d,d0;
 263   float eps=1E-3;
 264   float const relax =0.9;
 265   int i;
 266   bool success=false;
 267
 268   if(vcas < 0.1) {
 269     Mach=0;
 270     success=true;
 271     return success;
 272   }
 273   //initializations
 274   d=1;
 275   if(findMachInterval(&x1,&x3,vcas)) {
 276
 277
 278     f1=calcVcas(x1)-vcas;
 279     f3=calcVcas(x3)-vcas;
 280     d0=fabs(x3-x1);
 281
 282     //iterations
 283     i=0;
 284     while ((fabs(d) > eps) && (i < 100)) {
 285       //cout << "getMachFromVcas x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
 286       d=(x3-x1)/d0;
 287       x2=x1-d*d0*f1/(f3-f1);
 288
 289       f2=calcVcas(x2)-vcas;
 290       if(f1*f2 <= 0.0) {
 291         x3=x2;
 292         f3=f2;
 293         f1=relax*f1;
 294       } else if(f2*f3 <= 0) {
 295         x1=x2;
 296         f1=f2;
 297         f3=relax*f3;
 298       }
 299       //cout << i << endl;
 300       i++;
 301     }//end while
 302     if(i < 100) {
 303       success=true;
 304       *Mach=x2;
 305     }
 306
 307   }
 308   //cout << "Success= " << success << " Vcas: " << vcas*FPSTOKTS << " Mach: " << x2 << endl;
 309   return success;
 310 }