fgauxiliary.cpp

6 DOF Missle Simulation

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

 Module:       FGAuxiliary.cpp
 Author:       Tony Peden, Jon Berndt
 Date started: 01/26/99
 Purpose:      Calculates additional parameters needed by the visual system, etc.
 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
--------------------------------------------------------------------------------
This class calculates various auxiliary parameters.

REFERENCES
  Anderson, John D. "Introduction to Flight", 3rd Edition, McGraw-Hill, 1989
                    pgs. 112-126
HISTORY
--------------------------------------------------------------------------------
01/26/99   JSB   Created

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

#include "FGAuxiliary.h"
#include "FGAerodynamics.h"
#include "FGPropagate.h"
#include "FGAtmosphere.h"
#include <FGFDMExec.h>
#include "FGAircraft.h"
#include "FGInertial.h"
#include "FGExternalReactions.h"
#include "FGBuoyantForces.h"
#include "FGGroundReactions.h"
#include "FGPropulsion.h"
#include <input_output/FGPropertyManager.h>

namespace JSBSim {

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

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


FGAuxiliary::FGAuxiliary(FGFDMExec* fdmex) : FGModel(fdmex)
{
  Name = "FGAuxiliary";
  vcas = veas = pt = tat = 0;
  psl = rhosl = 1;
  qbar = 0;
  qbarUW = 0.0;
  qbarUV = 0.0;
  Mach = 0.0;
  alpha = beta = 0.0;
  adot = bdot = 0.0;
  gamma = Vt = Vground = 0.0;
  psigt = 0.0;
  day_of_year = 1;
  seconds_in_day = 0.0;
  hoverbmac = hoverbcg = 0.0;

  vPilotAccel.InitMatrix();
  vPilotAccelN.InitMatrix();
  vToEyePt.InitMatrix();
  vAeroPQR.InitMatrix();
  vEulerRates.InitMatrix();

  bind();

  Debug(0);
}

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

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

  vcas = veas = pt = tat = 0;
  psl = rhosl = 1;
  qbar = 0;
  qbarUW = 0.0;
  qbarUV = 0.0;
  Mach = 0.0;
  alpha = beta = 0.0;
  adot = bdot = 0.0;
  gamma = Vt = Vground = 0.0;
  psigt = 0.0;
  day_of_year = 1;
  seconds_in_day = 0.0;
  hoverbmac = hoverbcg = 0.0;

  vPilotAccel.InitMatrix();
  vPilotAccelN.InitMatrix();
  vToEyePt.InitMatrix();
  vAeroPQR.InitMatrix();
  vEulerRates.InitMatrix();

  return true;
}
  
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

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

bool FGAuxiliary::Run()
{
  double A,B,D;

  if (FGModel::Run()) return true; // return true if error returned from base class
  if (FDMExec->Holding()) return false;

  const FGColumnVector3& vPQR = Propagate->GetPQR();
  const FGColumnVector3& vUVW = Propagate->GetUVW();
  const FGColumnVector3& vUVWdot = Propagate->GetUVWdot();
  const FGColumnVector3& vVel = Propagate->GetVel();

  p = Atmosphere->GetPressure();
  rhosl = Atmosphere->GetDensitySL();
  psl = Atmosphere->GetPressureSL();
  sat = Atmosphere->GetTemperature();

// Rotation

  double cTht = Propagate->GetCosEuler(eTht);
  double sTht = Propagate->GetSinEuler(eTht);
  double cPhi = Propagate->GetCosEuler(ePhi);
  double sPhi = Propagate->GetSinEuler(ePhi);

  vEulerRates(eTht) = vPQR(eQ)*cPhi - vPQR(eR)*sPhi;
  if (cTht != 0.0) {
    vEulerRates(ePsi) = (vPQR(eQ)*sPhi + vPQR(eR)*cPhi)/cTht;
    vEulerRates(ePhi) = vPQR(eP) + vEulerRates(ePsi)*sTht;
  }

// 12/16/2005, JSB: For ground handling purposes, at this time, let's ramp
// in the effects of wind from 10 fps to 30 fps when there is weight on the
// landing gear wheels.

  if (GroundReactions->GetWOW() && vUVW(eU) < 10) {
    vAeroPQR = vPQR;
    vAeroUVW = vUVW;
  } else if (GroundReactions->GetWOW() && vUVW(eU) < 30) {
    double factor = (vUVW(eU) - 10.0)/20.0;
    vAeroPQR = vPQR + factor*Atmosphere->GetTurbPQR();
    vAeroUVW = vUVW + factor*Propagate->GetTl2b()*Atmosphere->GetTotalWindNED();
  } else {
    vAeroPQR = vPQR + Atmosphere->GetTurbPQR();
    vAeroUVW = vUVW + Propagate->GetTl2b()*Atmosphere->GetTotalWindNED();
  }

  Vt = vAeroUVW.Magnitude();
  if ( Vt > 0.05) {
    if (vAeroUVW(eW) != 0.0)
      alpha = vAeroUVW(eU)*vAeroUVW(eU) > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0;
    if (vAeroUVW(eV) != 0.0)
      beta = vAeroUVW(eU)*vAeroUVW(eU)+vAeroUVW(eW)*vAeroUVW(eW) > 0.0 ? atan2(vAeroUVW(eV),
             sqrt(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW))) : 0.0;

    double mUW = (vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW));
    double signU=1;
    if (vAeroUVW(eU) != 0.0)
      signU = vAeroUVW(eU)/fabs(vAeroUVW(eU));

    if ( (mUW == 0.0) || (Vt == 0.0) ) {
      adot = 0.0;
      bdot = 0.0;
    } else {
      adot = (vAeroUVW(eU)*vUVWdot(eW) - vAeroUVW(eW)*vUVWdot(eU))/mUW;
      bdot = (signU*mUW*vUVWdot(eV) - vAeroUVW(eV)*(vAeroUVW(eU)*vUVWdot(eU)
              + vAeroUVW(eW)*vUVWdot(eW)))/(Vt*Vt*sqrt(mUW));
    }
  } else {
    alpha = beta = adot = bdot = 0;
  }

  qbar = 0.5*Atmosphere->GetDensity()*Vt*Vt;
  qbarUW = 0.5*Atmosphere->GetDensity()*(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW));
  qbarUV = 0.5*Atmosphere->GetDensity()*(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eV)*vAeroUVW(eV));
  Mach = Vt / Atmosphere->GetSoundSpeed();
  MachU = vMachUVW(eU) = vAeroUVW(eU) / Atmosphere->GetSoundSpeed();
  vMachUVW(eV) = vAeroUVW(eV) / Atmosphere->GetSoundSpeed();
  vMachUVW(eW) = vAeroUVW(eW) / Atmosphere->GetSoundSpeed();

// Position

  Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) );

  psigt = atan2(vVel(eEast), vVel(eNorth));