fginitialcondition.cpp

6 DOF Missle Simulation

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

 Header:       FGInitialCondition.cpp
 Author:       Tony Peden
 Date started: 7/1/99

 ------------- Copyright (C) 1999  Anthony K. Peden ([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.


 HISTORY
--------------------------------------------------------------------------------
7/1/99   TP   Created


FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------

The purpose of this class is to take a set of initial conditions and provide
a kinematically consistent set of body axis velocity components, euler
angles, and altitude.  This class does not attempt to trim the model i.e.
the sim will most likely start in a very dynamic state (unless, of course,
you have chosen your IC's wisely) even after setting it up with this class.

********************************************************************************
INCLUDES
*******************************************************************************/

#include "FGInitialCondition.h"
#include <FGFDMExec.h>
#include <models/FGInertial.h>
#include <models/FGAtmosphere.h>
#include <models/FGAerodynamics.h>
#include <models/FGPropagate.h>
#include <input_output/FGPropertyManager.h>
#include <models/FGPropulsion.h>
#include <input_output/FGXMLParse.h>
#include <math/FGQuaternion.h>
#include <fstream>

namespace JSBSim {

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

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

FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) : fdmex(FDMExec)
{
  InitializeIC();

  if(FDMExec != NULL ) {
    fdmex->GetPropagate()->Seth(altitude);
    fdmex->GetAtmosphere()->Run();
    PropertyManager=fdmex->GetPropertyManager();
    bind();
  } else {
    cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
  }

  Debug(0);
}

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

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

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

void FGInitialCondition::ResetIC(double u0, double v0, double w0,
                                 double p0, double q0, double r0,
                                 double alpha0, double beta0,
                                 double phi0, double theta0, double psi0,
                                 double latRad0, double lonRad0, double altAGLFt0,
                                 double gamma0)
{
  InitializeIC();

  u = u0;  v = v0;  w = w0;
  p = p0;  q = q0;  r = r0;
  alpha = alpha0;  beta = beta0;
  phi = phi0;  theta = theta0;  psi = psi0;
  gamma = gamma0;

  latitude = latRad0;
  longitude = lonRad0;
  SetAltitudeAGLFtIC(altAGLFt0);

  cphi   = cos(phi);   sphi   = sin(phi);   // phi, rad
  ctheta = cos(theta); stheta = sin(theta); // theta, rad
  cpsi   = cos(psi);   spsi   = sin(psi);   // psi, rad

  FGQuaternion Quat( phi, theta, psi );
  Quat.Normalize();

  const FGMatrix33& _Tl2b  = Quat.GetT();     // local to body frame
  const FGMatrix33& _Tb2l  = Quat.GetTInv();  // body to local

  FGColumnVector3 _vUVW_BODY(u,v,w);
  FGColumnVector3 _vUVW_NED = _Tb2l * _vUVW_BODY;
  FGColumnVector3 _vWIND_NED(wnorth,weast,wdown);
  FGColumnVector3 _vUVWAero = _Tl2b * ( _vUVW_NED + _vWIND_NED );

  uw=_vWIND_NED(1); vw=_vWIND_NED(2); ww=_vWIND_NED(3);

}

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

void FGInitialCondition::InitializeIC(void)
{
  vt=vc=ve=vg=0;
  mach=0;
  alpha=beta=gamma=0;
  theta=phi=psi=0;
  altitude=hdot=0;
  latitude=longitude=0;
  u=v=w=0;
  p=q=r=0;
  uw=vw=ww=0;
  vnorth=veast=vdown=0;
  wnorth=weast=wdown=0;
  whead=wcross=0;
  wdir=wmag=0;
  lastSpeedSet=setvt;
  lastWindSet=setwned;
  radius_to_vehicle = sea_level_radius = fdmex->GetInertial()->GetRefRadius();
  terrain_altitude = 0;

  targetNlfIC = 1.0;

  salpha=sbeta=stheta=sphi=spsi=sgamma=0;
  calpha=cbeta=ctheta=cphi=cpsi=cgamma=1;
}

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

void FGInitialCondition::WriteStateFile(int num)
{
  string filename = fdmex->GetFullAircraftPath() + "/" + "initfile.xml";
  ofstream outfile(filename.c_str());
  FGPropagate* Propagate = fdmex->GetPropagate();
  
  if (outfile.is_open()) {
    outfile << "<?xml version=\"1.0\"?>" << endl;
    outfile << "<initialize name=\"reset00\">" << endl;
    outfile << "  <ubody unit=\"FT/SEC\"> " << Propagate->GetUVW(eX) << " </ubody> " << endl;
    outfile << "  <vbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eY) << " </vbody> " << endl;
    outfile << "  <wbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eZ) << " </wbody> " << endl;
    outfile << "  <phi unit=\"DEG\"> " << Propagate->GetEuler(ePhi) << " </phi>" << endl;
    outfile << "  <theta unit=\"DEG\"> " << Propagate->GetEuler(eTht) << " </theta>" << endl;
    outfile << "  <psi unit=\"DEG\"> " << Propagate->GetEuler(ePsi) << " </psi>" << endl;
    outfile << "  <longitude unit=\"DEG\"> " << Propagate->GetLongitudeDeg() << " </longitude>" << endl;
    outfile << "  <latitude unit=\"DEG\"> " << Propagate->GetLatitudeDeg() << " </latitude>" << endl;
    outfile << "  <altitude unit=\"FT\"> " << Propagate->Geth() << " </altitude>" << endl;
    outfile << "</initialize>" << endl;
  } else {
    cerr << "Could not open and/or write the state to the initial conditions file." << endl;
  }

  outfile.close();
}

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

void FGInitialCondition::SetVcalibratedKtsIC(double tt) {

  if(getMachFromVcas(&mach,tt*ktstofps)) {
    //cout << "Mach: " << mach << endl;
    lastSpeedSet=setvc;
    vc=tt*ktstofps;
    vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
    ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
    //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl;
  }
  else {
    cout << "Failed to get Mach number for given Vc and altitude, Vc unchanged." << endl;
    cout << "Please mail the set of initial conditions used to [e-mail]" << endl;
  }
}

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

void FGInitialCondition::SetVequivalentKtsIC(double tt) {
  ve=tt*ktstofps;
  lastSpeedSet=setve;
  vt=ve*1/sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
  mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
  vc=calcVcas(mach);
}

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

void FGInitialCondition::SetVgroundFpsIC(double tt) {
  double ua,va,wa;
  double vxz;

  vg=tt;
  lastSpeedSet=setvg;
  vnorth = vg*cos(psi); veast = vg*sin(psi); vdown = 0;
  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());
}

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

void FGInitialCondition::SetVtrueFpsIC(double tt) {
  vt=tt;
  lastSpeedSet=setvt;
  mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
  vc=calcVcas(mach);
  ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
}

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

void FGInitialCondition::SetMachIC(double tt) {
  mach=tt;
  lastSpeedSet=setmach;
  vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
  vc=calcVcas(mach);
  ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
  //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl;
}

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

void FGInitialCondition::SetClimbRateFpmIC(double tt) {
  SetClimbRateFpsIC(tt/60.0);
}

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

void FGInitialCondition::SetClimbRateFpsIC(double tt) {

  if(vt > 0.1) {
    hdot=tt;
    gamma=asin(hdot/vt);
    sgamma=sin(gamma); cgamma=cos(gamma);
  }
}

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

void FGInitialCondition::SetFlightPathAngleRadIC(double tt) {
  gamma=tt;
  sgamma=sin(gamma); cgamma=cos(gamma);
  getTheta();
  hdot=vt*sgamma;
}

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

void FGInitialCondition::SetAlphaRadIC(double tt) {
  alpha=tt;
  salpha=sin(alpha); calpha=cos(alpha);
  getTheta();
}

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

void FGInitialCondition::SetThetaRadIC(double tt) {
  theta=tt;
  stheta=sin(theta); ctheta=cos(theta);
  getAlpha();
}

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

void FGInitialCondition::SetBetaRadIC(double tt) {
  beta=tt;
  sbeta=sin(beta); cbeta=cos(beta);
  getTheta();

}

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

void FGInitialCondition::SetPhiRadIC(double tt) {
  phi=tt;
  sphi=sin(phi); cphi=cos(phi);
  getTheta();
}

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

void FGInitialCondition::SetPsiRadIC(double tt) {
    psi=tt;
    spsi=sin(psi); cpsi=cos(psi);
    calcWindUVW();
}

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

void FGInitialCondition::SetUBodyFpsIC(double tt) {
  u=tt;
  vt=sqrt(u*u + v*v + w*w);
  lastSpeedSet=setuvw;
}

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

void FGInitialCondition::SetVBodyFpsIC(double tt) {
  v=tt;
  vt=sqrt(u*u + v*v + w*w);
  lastSpeedSet=setuvw;
}

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

void FGInitialCondition::SetWBodyFpsIC(double tt) {
  w=tt;
  vt=sqrt( u*u + v*v + w*w );
  lastSpeedSet=setuvw;
}


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

void FGInitialCondition::SetVNorthFpsIC(double tt) {
  double ua,va,wa;
  double vxz;
  vnorth = 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());
  lastSpeedSet=setned;
}

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

void FGInitialCondition::SetVEastFpsIC(double tt) {
  double ua,va,wa;
  double vxz;
  veast = 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());