fgmsis.cpp

6 DOF Missle Simulation

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

 Module:       FGMSIS.cpp
 Author:       David Culp
               (incorporated into C++ JSBSim class heirarchy, see model authors below)
 Date started: 12/14/03
 Purpose:      Models the MSIS-00 atmosphere

 ------------- Copyright (C) 2003  David P. Culp ([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 MSIS-00 atmosphere. Provides temperature and density to FGAtmosphere,
given day-of-year, time-of-day, altitude, latitude, longitude and local time.

HISTORY
--------------------------------------------------------------------------------
12/14/03   DPC   Created
01/11/04   DPC   Derived from FGAtmosphere

 --------------------------------------------------------------------
 ---------  N R L M S I S E - 0 0    M O D E L    2 0 0 1  ----------
 --------------------------------------------------------------------

 This file is part of the NRLMSISE-00  C source code package - release
 20020503

 The NRLMSISE-00 model was developed by Mike Picone, Alan Hedin, and
 Doug Drob. They also wrote a NRLMSISE-00 distribution package in
 FORTRAN which is available at
 http://uap-www.nrl.navy.mil/models_web/msis/msis_home.htm

 Dominik Brodowski implemented and maintains this C version. You can
 reach him at [e-mail]. See the file "DOCUMENTATION" for details,
 and check http://www.brodo.de/english/pub/nrlmsise/index.html for
 updated releases of this package.
*/

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

#include "FGMSIS.h"
#include "FGState.h"
#include <math.h>          /* maths functions */
#include <stdlib.h>        /* for malloc/free */
#include <stdio.h>         /* for printf      */
#include <iostream>        // for cout, endl

namespace JSBSim {

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

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
EXTERNAL GLOBAL DATA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

  /* POWER7 */
  extern double pt[150];
  extern double pd[9][150];
  extern double ps[150];
  extern double pdl[2][25];
  extern double ptl[4][100];
  extern double pma[10][100];
  extern double sam[100];

  /* LOWER7 */
  extern double ptm[10];
  extern double pdm[8][10];
  extern double pavgm[10];

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


MSIS::MSIS(FGFDMExec* fdmex) : FGAtmosphere(fdmex)
{
  Name = "MSIS";
  bind();
  Debug(0);
}

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

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

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

bool MSIS::InitModel(void)
{
  FGModel::InitModel();

  unsigned int i;

  flags.switches[0] = 0;
  for (i=1;i<24;i++) flags.switches[i] = 1;

  for (i=0;i<7;i++) aph.a[i] = 100.0;

  // set some common magnetic flux values
  input.f107A = 150.0;
  input.f107 = 150.0;
  input.ap = 4.0;

  SLtemperature = intTemperature = 518.0;
  SLpressure    = intPressure = 2116.7;
  SLdensity     = intDensity = 0.002378;
  SLsoundspeed  = sqrt(2403.0832 * SLtemperature);
  rSLtemperature = 1.0/intTemperature;
  rSLpressure    = 1.0/intPressure;
  rSLdensity     = 1.0/intDensity;
  rSLsoundspeed  = 1.0/SLsoundspeed;
  temperature = &intTemperature;
  pressure = &intPressure;
  density = &intDensity;

  useExternal=false;

  return true;
}

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

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

  //do temp, pressure, and density first
  if (!useExternal) {
    // get sea-level values
    Calculate(Auxiliary->GetDayOfYear(),
              Auxiliary->GetSecondsInDay(),
              0.0,
              Propagate->GetLocation().GetLatitudeDeg(),
              Propagate->GetLocation().GetLongitudeDeg());
    SLtemperature = output.t[1] * 1.8;
    SLdensity     = output.d[5] * 1.940321;
    SLpressure    = 1716.488 * SLdensity * SLtemperature;
    SLsoundspeed  = sqrt(2403.0832 * SLtemperature);
    rSLtemperature = 1.0/SLtemperature;
    rSLpressure    = 1.0/SLpressure;
    rSLdensity     = 1.0/SLdensity;
    rSLsoundspeed  = 1.0/SLsoundspeed;

    // get at-altitude values
    Calculate(Auxiliary->GetDayOfYear(),
              Auxiliary->GetSecondsInDay(),
              Propagate->Geth(),
              Propagate->GetLocation().GetLatitudeDeg(),
              Propagate->GetLocation().GetLongitudeDeg());
    intTemperature = output.t[1] * 1.8;
    intDensity     = output.d[5] * 1.940321;
    intPressure    = 1716.488 * intDensity * intTemperature;
    soundspeed     = sqrt(2403.0832 * intTemperature);
    //cout << "T=" << intTemperature << " D=" << intDensity << " P=";
    //cout << intPressure << " a=" << soundspeed << endl;
  }

  if (turbType != ttNone) {
    Turbulence();
    vWindNED += vTurbulenceNED;
  }

  if (vWindNED(1) != 0.0) psiw = atan2( vWindNED(2), vWindNED(1) );

  if (psiw < 0) psiw += 2*M_PI;

  Debug(2);

  return false;
}

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

void MSIS::Calculate(int day, double sec, double alt, double lat, double lon)
{
  input.year = 2000;
  input.doy = day;
  input.sec = sec;
  input.alt = alt / 3281;  //feet to kilometers
  input.g_lat = lat;
  input.g_long = lon;

  input.lst = (sec/3600) + (lon/15);
  if (input.lst > 24.0) input.lst -= 24.0;
  if (input.lst < 0.0) input.lst = 24 - input.lst;

  gtd7d(&input, &flags, &output);
}

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


void MSIS::UseExternal(void){
  // do nothing, external control not allowed
}


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


void MSIS::tselec(struct nrlmsise_flags *flags)
{
  int i;
  for (i=0;i<24;i++) {
    if (i!=9) {
      if (flags->switches[i]==1)
        flags->sw[i]=1;
      else
        flags->sw[i]=0;
      if (flags->switches[i]>0)
        flags->swc[i]=1;
      else
        flags->swc[i]=0;
    } else {
      flags->sw[i]=flags->switches[i];
      flags->swc[i]=flags->switches[i];
    }
  }
}


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

void MSIS::glatf(double lat, double *gv, double *reff)
{
  double dgtr = 1.74533E-2;
  double c2;
  c2 = cos(2.0*dgtr*lat);
  *gv = 980.616 * (1.0 - 0.0026373 * c2);
  *reff = 2.0 * (*gv) / (3.085462E-6 + 2.27E-9 * c2) * 1.0E-5;
}

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

double MSIS::ccor(double alt, double r, double h1, double zh)
{
/*        CHEMISTRY/DISSOCIATION CORRECTION FOR MSIS MODELS
 *         ALT - altitude
 *         R - target ratio
 *         H1 - transition scale length
 *         ZH - altitude of 1/2 R
 */
  double e;
  double ex;
  e = (alt - zh) / h1;
  if (e>70)
    return exp(0.0);
  if (e<-70)
    return exp(r);
  ex = exp(e);
  e = r / (1.0 + ex);
  return exp(e);
}

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

double MSIS::ccor2(double alt, double r, double h1, double zh, double h2)
{
/*        CHEMISTRY/DISSOCIATION CORRECTION FOR MSIS MODELS
 *         ALT - altitude
 *         R - target ratio
 *         H1 - transition scale length
 *         ZH - altitude of 1/2 R
 *         H2 - transition scale length #2 ?
 */
  double e1, e2;
  double ex1, ex2;
  double ccor2v;
  e1 = (alt - zh) / h1;
  e2 = (alt - zh) / h2;
  if ((e1 > 70) || (e2 > 70))
    return exp(0.0);
  if ((e1 < -70) && (e2 < -70))
    return exp(r);
  ex1 = exp(e1);
  ex2 = exp(e2);
  ccor2v = r / (1.0 + 0.5 * (ex1 + ex2));
  return exp(ccor2v);
}

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

double MSIS::scalh(double alt, double xm, double temp)
{
  double g;
  double rgas=831.4;
  g = gsurf / (pow((1.0 + alt/re),2.0));
  g = rgas * temp / (g * xm);
  return g;
}

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

double MSIS::dnet (double dd, double dm, double zhm, double xmm, double xm)
{
/*       TURBOPAUSE CORRECTION FOR MSIS MODELS
 *        Root mean density
 *         DD - diffusive density
 *         DM - full mixed density
 *         ZHM - transition scale length
 *         XMM - full mixed molecular weight
 *         XM  - species molecular weight
 *         DNET - combined density
 */
  double a;
  double ylog;
  a  = zhm / (xmm-xm);
  if (!((dm>0) && (dd>0))) {
    printf("dnet log error %e %e %e\n",dm,dd,xm);
    if ((dd==0) && (dm==0))
      dd=1;
    if (dm==0)
      return dd;
    if (dd==0)
      return dm;
  }
  ylog = a * log(dm/dd);
  if (ylog<-10)
    return dd;
  if (ylog>10)
    return dm;
  a = dd*pow((1.0 + exp(ylog)),(1.0/a));
  return a;
}

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

void MSIS::splini (double *xa, double *ya, double *y2a, int n, double x, double *y)
{
/*      INTEGRATE CUBIC SPLINE FUNCTION FROM XA(1) TO X
 *       XA,YA: ARRAYS OF TABULATED FUNCTION IN ASCENDING ORDER BY X
 *       Y2A: ARRAY OF SECOND DERIVATIVES
 *       N: SIZE OF ARRAYS XA,YA,Y2A
 *       X: ABSCISSA ENDPOINT FOR INTEGRATION
 *       Y: OUTPUT VALUE
 */
  double yi=0;
  int klo=0;
  int khi=1;
  double xx, h, a, b, a2, b2;
  while ((x>xa[klo]) && (khi<n)) {
    xx=x;
    if (khi<(n-1)) {
      if (x<xa[khi])
        xx=x;
      else
        xx=xa[khi];
    }
    h = xa[khi] - xa[klo];
    a = (xa[khi] - xx)/h;
    b = (xx - xa[klo])/h;
    a2 = a*a;
    b2 = b*b;
    yi += ((1.0 - a2) * ya[klo] / 2.0 + b2 * ya[khi] / 2.0 + ((-(1.0+a2*a2)/4.0 + a2/2.0) * y2a[klo] + (b2*b2/4.0 - b2/2.0) * y2a[khi]) * h * h / 6.0) * h;
    klo++;
    khi++;
  }
  *y = yi;
}

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

void MSIS::splint (double *xa, double *ya, double *y2a, int n, double x, double *y)
{
/*      CALCULATE CUBIC SPLINE INTERP VALUE
 *       ADAPTED FROM NUMERICAL RECIPES BY PRESS ET AL.
 *       XA,YA: ARRAYS OF TABULATED FUNCTION IN ASCENDING ORDER BY X
 *       Y2A: ARRAY OF SECOND DERIVATIVES
 *       N: SIZE OF ARRAYS XA,YA,Y2A
 *       X: ABSCISSA FOR INTERPOLATION
 *       Y: OUTPUT VALUE
 */
  int klo=0;
  int khi=n-1;
  int k;
  double h;
  double a, b, yi;
  while ((khi-klo)>1) {
    k=(khi+klo)/2;
    if (xa[k]>x)
      khi=k;
    else
      klo=k;
  }
  h = xa[khi] - xa[klo];
  if (h==0.0)
    printf("bad XA input to splint");
  a = (xa[khi] - x)/h;
  b = (x - xa[klo])/h;
  yi = a * ya[klo] + b * ya[khi] + ((a*a*a - a) * y2a[klo] + (b*b*b - b) * y2a[khi]) * h * h/6.0;
  *y = yi;
}