fgatmosphere.cpp

来自「6 DOF Missle Simulation」· C++ 代码 · 共 665 行 · 第 1/2 页

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Module:       FGAtmosphere.cpp
 Author:       Jon Berndt
               Implementation of 1959 Standard Atmosphere added by Tony Peden
 Date started: 11/24/98
 Purpose:      Models the atmosphere
 Called by:    FGSimExec

 ------------- Copyright (C) 1999  Jon S. Berndt ([e-mail]) -------------

 This program is free software; you can redistribute it and/or modify it under
 the terms of the GNU Lesser General Public License as published by the Free Software
 Foundation; either version 2 of the License, or (at your option) any later
 version.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
 details.

 You should have received a copy of the GNU Lesser General Public License along with
 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
 Place - Suite 330, Boston, MA  02111-1307, USA.

 Further information about the GNU Lesser General Public License can also be found on
 the world wide web at http://www.gnu.org.

FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
Models the atmosphere. The equation used below was determined by a third order
curve fit using Excel. The data is from the ICAO atmosphere model.

HISTORY
--------------------------------------------------------------------------------
11/24/98   JSB   Created
07/23/99   TP    Added implementation of 1959 Standard Atmosphere
                 Moved calculation of Mach number to FGPropagate
                 Later updated to '76 model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
COMMENTS, REFERENCES,  and NOTES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[1]   Anderson, John D. "Introduction to Flight, Third Edition", McGraw-Hill,
      1989, ISBN 0-07-001641-0

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#include "FGAtmosphere.h"
#include <FGState.h>
#include <FGFDMExec.h>
#include "FGAircraft.h"
#include "FGPropagate.h"
#include "FGInertial.h"
#include <input_output/FGPropertyManager.h>

namespace JSBSim {

static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_ATMOSPHERE;

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

FGAtmosphere::FGAtmosphere(FGFDMExec* fdmex) : FGModel(fdmex)
{
  Name = "FGAtmosphere";
  lastIndex = 0;
  h = 0.0;
  psiw = 0.0;
  htab[0]=0;
  htab[1]=36089.239;
  htab[2]=65616.798;
  htab[3]=104986.878;
  htab[4]=154199.475;
  htab[5]=170603.675;
  htab[6]=200131.234;
  htab[7]=259186.352; //ft.

  MagnitudedAccelDt = MagnitudeAccel = Magnitude = 0.0;
  SetTurbType( ttCulp );
  TurbGain = 0.0;
  TurbRate = 1.7;
  Rhythmicity = 0.1;
  spike = target_time = strength = 0.0;
  wind_from_clockwise = 0.0;

  T_dev_sl = T_dev = delta_T = 0.0;
  StandardTempOnly = false;
  first_pass = true;
  vGustNED.InitMatrix();
  vTurbulenceNED.InitMatrix();

  bind();
  Debug(0);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

FGAtmosphere::~FGAtmosphere()
{
  Debug(1);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGAtmosphere::InitModel(void)
{
  if (!FGModel::InitModel()) return false;

  UseInternal();  // this is the default

  Calculate(h);
  StdSLtemperature = SLtemperature = 518.67;
  StdSLpressure    = SLpressure = 2116.22;
  StdSLdensity     = SLdensity = 0.00237767;
  StdSLsoundspeed  = SLsoundspeed = sqrt(SHRatio*Reng*StdSLtemperature);
  rSLtemperature = 1.0/StdSLtemperature;
  rSLpressure    = 1.0/StdSLpressure;
  rSLdensity     = 1.0/StdSLdensity;
  rSLsoundspeed  = 1.0/StdSLsoundspeed;

  return true;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGAtmosphere::Run(void)
{
  if (FGModel::Run()) return true;
  if (FDMExec->Holding()) return false;

  T_dev = 0.0;
  h = Propagate->Geth();

  if (!useExternal) {
    Calculate(h);
    CalculateDerived();
  } else {
    CalculateDerived();
  }

  Debug(2);
  return false;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
// See reference 1

void FGAtmosphere::Calculate(double altitude)
{
  double slope, reftemp, refpress;
  int i = lastIndex;

  if (altitude < htab[lastIndex]) {
    if (altitude <= 0) {
      i = 0;
      altitude=0;
    } else {
       i = lastIndex-1;
       while (htab[i] > altitude) i--;
    }
  } else if (altitude > htab[lastIndex+1]) {
    if (altitude >= htab[7]) {
      i = 7;
      altitude = htab[7];
    } else {
      i = lastIndex+1;
      while (htab[i+1] < altitude) i++;
    }
  }

  switch(i) {
  case 1:     // 36089 ft.
    slope     = 0;
    reftemp   = 389.97;
    refpress  = 472.452;
    //refdens   = 0.000706032;
    break;
  case 2:     // 65616 ft.
    slope     = 0.00054864;
    reftemp   = 389.97;
    refpress  = 114.636;
    //refdens   = 0.000171306;
    break;
  case 3:     // 104986 ft.
    slope     = 0.00153619;
    reftemp   = 411.57;
    refpress  = 8.36364;
    //refdens   = 1.18422e-05;
    break;
  case 4:     // 154199 ft.
    slope     = 0;
    reftemp   = 487.17;
    refpress  = 0.334882;
    //refdens   = 4.00585e-7;
    break;
  case 5:     // 170603 ft.
    slope     = -0.00109728;
    reftemp   = 487.17;
    refpress  = 0.683084;
    //refdens   = 8.17102e-7;
    break;
  case 6:     // 200131 ft.
    slope     = -0.00219456;
    reftemp   = 454.17;
    refpress  = 0.00684986;
    //refdens   = 8.77702e-9;
    break;
  case 7:     // 259186 ft.
    slope     = 0;
    reftemp   = 325.17;
    refpress  = 0.000122276;
    //refdens   = 2.19541e-10;
    break;
  case 0:
  default:     // sea level
    slope     = -0.00356616; // R/ft.
    reftemp   = 518.67;    // R
    refpress  = 2116.22;    // psf
    //refdens   = 0.00237767;  // slugs/cubic ft.
    break;

  }

  // If delta_T is set, then that is our temperature deviation at any altitude.
  // If not, then we'll estimate a deviation based on the sea level deviation (if set).

  if(!StandardTempOnly) {
    T_dev = 0.0;
    if (delta_T != 0.0) {
      T_dev = delta_T;
    } else {
      if ((altitude < 36089.239) && (T_dev_sl != 0.0)) {
        T_dev = T_dev_sl * ( 1.0 - (altitude/36089.239));
      }
    }
    reftemp+=T_dev;
  }

  if (slope == 0) {
    intTemperature = reftemp;
    intPressure = refpress*exp(-Inertial->SLgravity()/(reftemp*Reng)*(altitude-htab[i]));
    intDensity = intPressure/(Reng*intTemperature);
  } else {
    intTemperature = reftemp+slope*(altitude-htab[i]);
    intPressure = refpress*pow(intTemperature/reftemp,-Inertial->SLgravity()/(slope*Reng));
    intDensity = intPressure/(Reng*intTemperature);
  }

  lastIndex=i;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Calculate parameters derived from T, P and rho
// Sum gust and turbulence values in NED frame into the wind vector.

void FGAtmosphere::CalculateDerived(void)
{
  T_dev = (*temperature) - GetTemperature(h);
  density_altitude = h + T_dev * 66.7;

  if (turbType == ttStandard || ttCulp) Turbulence();

  vTotalWindNED = vWindNED + vGustNED + vTurbulenceNED;

  if (vWindNED(eX) != 0.0) psiw = atan2( vWindNED(eY), vWindNED(eX) );
  if (psiw < 0) psiw += 2*M_PI;

  soundspeed = sqrt(SHRatio*Reng*(*temperature));
}


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Get the standard atmospheric properties at a specified altitude

void FGAtmosphere::GetStdAtmosphere(double altitude) {
  StandardTempOnly = true;
  Calculate(altitude);
  StandardTempOnly = false;
  atmosphere.Temperature = intTemperature;
  atmosphere.Pressure = intPressure;
  atmosphere.Density = intDensity;

  // Reset the internal atmospheric state
  Calculate(h);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Get the standard pressure at a specified altitude

double FGAtmosphere::GetPressure(double altitude) {
  GetStdAtmosphere(altitude);
  return atmosphere.Pressure;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Get the standard temperature at a specified altitude

double FGAtmosphere::GetTemperature(double altitude) {
  GetStdAtmosphere(altitude);
  return atmosphere.Temperature;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Get the standard density at a specified altitude

double FGAtmosphere::GetDensity(double altitude) {
  GetStdAtmosphere(altitude);
  return atmosphere.Density;
}


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// square a value, but preserve the original sign

static inline double square_signed (double value)
{
    if (value < 0)
        return value * value * -1;
    else
        return value * value;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGAtmosphere::SetWindspeed(double speed)
{
  if (vWindNED.Magnitude() == 0.0) {
    psiw = 0.0;