fginitialcondition.cpp

6 DOF Missle Simulation

  lastSpeedSet=setned;
}

//******************************************************************************

void FGInitialCondition::SetVDownFpsIC(double tt) {
  double ua,va,wa;
  double vxz;
  vdown = tt;
  calcUVWfromNED();
  ua = u + uw; va = v + vw; wa = w + ww;
  vt = sqrt( ua*ua + va*va + wa*wa );
  alpha = beta = 0;
  vxz = sqrt( u*u + w*w );
  if( w != 0 ) alpha = atan2( w, u );
  if( vxz != 0 ) beta = atan2( v, vxz );
  mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
  vc=calcVcas(mach);
  ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
  SetClimbRateFpsIC(-1*vdown);
  lastSpeedSet=setned;
}

//******************************************************************************

double FGInitialCondition::GetUBodyFpsIC(void) const {
    if (lastSpeedSet == setvg || lastSpeedSet == setned)
      return u;
    else
      return vt*calpha*cbeta - uw;
}

//******************************************************************************

double FGInitialCondition::GetVBodyFpsIC(void) const {
    if (lastSpeedSet == setvg || lastSpeedSet == setned)
      return v;
    else {
      return vt*sbeta - vw;
    }
}

//******************************************************************************

double FGInitialCondition::GetWBodyFpsIC(void) const {
    if (lastSpeedSet == setvg || lastSpeedSet == setned)
      return w;
    else
      return vt*salpha*cbeta -ww;
}

//******************************************************************************

void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD ) {
  wnorth = wN; weast = wE; wdown = wD;
  lastWindSet = setwned;
  calcWindUVW();
  if(lastSpeedSet == setvg)
    SetVgroundFpsIC(vg);
}

//******************************************************************************

void FGInitialCondition::SetCrossWindKtsIC(double cross){
    wcross=cross*ktstofps;
    lastWindSet=setwhc;
    calcWindUVW();
    if(lastSpeedSet == setvg)
      SetVgroundFpsIC(vg);

}

//******************************************************************************

// positive from left
void FGInitialCondition::SetHeadWindKtsIC(double head){
    whead=head*ktstofps;
    lastWindSet=setwhc;
    calcWindUVW();
    if(lastSpeedSet == setvg)
      SetVgroundFpsIC(vg);

}

//******************************************************************************

void FGInitialCondition::SetWindDownKtsIC(double wD) {
    wdown=wD;
    calcWindUVW();
    if(lastSpeedSet == setvg)
      SetVgroundFpsIC(vg);
}

//******************************************************************************

void FGInitialCondition::SetWindMagKtsIC(double mag) {
  wmag=mag*ktstofps;
  lastWindSet=setwmd;
  calcWindUVW();
  if(lastSpeedSet == setvg)
      SetVgroundFpsIC(vg);
}

//******************************************************************************

void FGInitialCondition::SetWindDirDegIC(double dir) {
  wdir=dir*degtorad;
  lastWindSet=setwmd;
  calcWindUVW();
  if(lastSpeedSet == setvg)
      SetVgroundFpsIC(vg);
}


//******************************************************************************

void FGInitialCondition::calcWindUVW(void) {

    switch(lastWindSet) {
      case setwmd:
        wnorth=wmag*cos(wdir);
        weast=wmag*sin(wdir);
      break;
      case setwhc:
        wnorth=whead*cos(psi) + wcross*cos(psi+M_PI/2);
        weast=whead*sin(psi) + wcross*sin(psi+M_PI/2);
      break;
      case setwned:
      break;
    }
    uw=wnorth*ctheta*cpsi +
       weast*ctheta*spsi -
       wdown*stheta;
    vw=wnorth*( sphi*stheta*cpsi - cphi*spsi ) +
        weast*( sphi*stheta*spsi + cphi*cpsi ) +
       wdown*sphi*ctheta;
    ww=wnorth*(cphi*stheta*cpsi + sphi*spsi) +
       weast*(cphi*stheta*spsi - sphi*cpsi) +
       wdown*cphi*ctheta;


    /* cout << "FGInitialCondition::calcWindUVW: wnorth, weast, wdown "
         << wnorth << ", " << weast << ", " << wdown << endl;
    cout << "FGInitialCondition::calcWindUVW: theta, phi, psi "
          << theta << ", " << phi << ", " << psi << endl;
    cout << "FGInitialCondition::calcWindUVW: uw, vw, ww "
          << uw << ", " << vw << ", " << ww << endl; */

}

//******************************************************************************

void FGInitialCondition::SetAltitudeFtIC(double tt) {
  altitude=tt;
  fdmex->GetPropagate()->Seth(altitude);
  fdmex->GetAtmosphere()->Run();
  //lets try to make sure the user gets what they intended

  switch(lastSpeedSet) {
  case setned:
  case setuvw:
  case setvt:
    SetVtrueKtsIC(vt*fpstokts);
    break;
  case setvc:
    SetVcalibratedKtsIC(vc*fpstokts);
    break;
  case setve:
    SetVequivalentKtsIC(ve*fpstokts);
    break;
  case setmach:
    SetMachIC(mach);
    break;
  case setvg:
    SetVgroundFpsIC(vg);
    break;
  }
}

//******************************************************************************

void FGInitialCondition::SetAltitudeAGLFtIC(double tt) {
  SetAltitudeFtIC(terrain_altitude + tt);
}

//******************************************************************************

void FGInitialCondition::SetSeaLevelRadiusFtIC(double tt) {
  sea_level_radius = tt;
}

//******************************************************************************

void FGInitialCondition::SetTerrainAltitudeFtIC(double tt) {
  terrain_altitude=tt;
}

//******************************************************************************

void FGInitialCondition::calcUVWfromNED(void) {
  u=vnorth*ctheta*cpsi +
     veast*ctheta*spsi -
     vdown*stheta;
  v=vnorth*( sphi*stheta*cpsi - cphi*spsi ) +
     veast*( sphi*stheta*spsi + cphi*cpsi ) +
     vdown*sphi*ctheta;
  w=vnorth*( cphi*stheta*cpsi + sphi*spsi ) +
     veast*( cphi*stheta*spsi - sphi*cpsi ) +
     vdown*cphi*ctheta;
}

//******************************************************************************

bool FGInitialCondition::getMachFromVcas(double *Mach,double vcas) {

  bool result=false;
  double guess=1.5;
  xlo=xhi=0;
  xmin=0;xmax=50;
  sfunc=&FGInitialCondition::calcVcas;
  if(findInterval(vcas,guess)) {
    if(solve(&mach,vcas))
      result=true;
  }
  return result;
}

//******************************************************************************

bool FGInitialCondition::getAlpha(void) {
  bool result=false;
  double guess=theta-gamma;

  if(vt < 0.01) return 0;

  xlo=xhi=0;
  xmin=fdmex->GetAerodynamics()->GetAlphaCLMin();
  xmax=fdmex->GetAerodynamics()->GetAlphaCLMax();
  sfunc=&FGInitialCondition::GammaEqOfAlpha;
  if(findInterval(0,guess)){
    if(solve(&alpha,0)){
      result=true;
      salpha=sin(alpha);
      calpha=cos(alpha);
    }
  }
  calcWindUVW();
  return result;
}

//******************************************************************************

bool FGInitialCondition::getTheta(void) {
  bool result=false;
  double guess=alpha+gamma;

  if(vt < 0.01) return 0;

  xlo=xhi=0;
  xmin=-89;xmax=89;
  sfunc=&FGInitialCondition::GammaEqOfTheta;
  if(findInterval(0,guess)){
    if(solve(&theta,0)){
      result=true;
      stheta=sin(theta);
      ctheta=cos(theta);
    }
  }
  calcWindUVW();
  return result;
}

//******************************************************************************

double FGInitialCondition::GammaEqOfTheta(double Theta) {
  double a,b,c;
  double sTheta,cTheta;

  //theta=Theta; stheta=sin(theta); ctheta=cos(theta);
  sTheta=sin(Theta); cTheta=cos(Theta);
  calcWindUVW();
  a=wdown + vt*calpha*cbeta + uw;
  b=vt*sphi*sbeta + vw*sphi;
  c=vt*cphi*salpha*cbeta + ww*cphi;
  return vt*sgamma - ( a*sTheta - (b+c)*cTheta);
}

//******************************************************************************

double FGInitialCondition::GammaEqOfAlpha(double Alpha) {
  double a,b,c;
  double sAlpha,cAlpha;
  sAlpha=sin(Alpha); cAlpha=cos(Alpha);
  a=wdown + vt*cAlpha*cbeta + uw;
  b=vt*sphi*sbeta + vw*sphi;
  c=vt*cphi*sAlpha*cbeta + ww*cphi;

  return vt*sgamma - ( a*stheta - (b+c)*ctheta );
}

//******************************************************************************

double FGInitialCondition::calcVcas(double Mach) {

  double p=fdmex->GetAtmosphere()->GetPressure();
  double psl=fdmex->GetAtmosphere()->GetPressureSL();
  double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
  double pt,A,B,D,vcas;
  if(Mach < 0) Mach=0;
  if(Mach < 1)    //calculate total pressure assuming isentropic flow
    pt=p*pow((1 + 0.2*Mach*Mach),3.5);
  else {
    // shock in front of pitot tube, we'll assume its normal and use
    // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
    // pressure behind the shock to the static pressure in front


    //the normal shock assumption should not be a bad one -- most supersonic
    //aircraft place the pitot probe out front so that it is the forward
    //most point on the aircraft.  The real shock would, of course, take
    //on something like the shape of a rounded-off cone but, here again,
    //the assumption should be good since the opening of the pitot probe
    //is very small and, therefore, the effects of the shock curvature
    //should be small as well. AFAIK, this approach is fairly well accepted
    //within the aerospace community

    B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);

    // The denominator above is zero for Mach ~ 0.38, for which
    // we'll never be here, so we're safe

    D = (2.8*Mach*Mach-0.4)*0.4167;
    pt = p*pow(B,3.5)*D;
  }

  A = pow(((pt-p)/psl+1),0.28571);
  vcas = sqrt(7*psl/rhosl*(A-1));
  //cout << "calcVcas: vcas= " << vcas*fpstokts << " mach= " << Mach << " pressure: " << pt << endl;
  return vcas;
}

//******************************************************************************

bool FGInitialCondition::findInterval(double x,double guess) {
  //void find_interval(inter_params &ip,eqfunc f,double y,double constant, int &flag){

  int i=0;
  bool found=false;
  double flo,fhi,fguess;
  double lo,hi,step;
  step=0.1;
  fguess=(this->*sfunc)(guess)-x;
  lo=hi=guess;
  do {
    step=2*step;
    lo-=step;
    hi+=step;
    if(lo < xmin) lo=xmin;
    if(hi > xmax) hi=xmax;
    i++;
    flo=(this->*sfunc)(lo)-x;
    fhi=(this->*sfunc)(hi)-x;
    if(flo*fhi <=0) {  //found interval with root
      found=true;
      if(flo*fguess <= 0) {  //narrow interval down a bit
        hi=lo+step;    //to pass solver interval that is as
        //small as possible
      }
      else if(fhi*fguess <= 0) {
        lo=hi-step;
      }
    }
    //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
  }
  while((found == 0) && (i <= 100));
  xlo=lo;
  xhi=hi;
  return found;
}