FGInitialCondition.h

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

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

 ------------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) -------------

 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.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#ifndef FGINITIALCONDITION_H
#define FGINITIALCONDITION_H

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

#include "math/FGLocation.h"
#include "math/FGQuaternion.h"
#include "simgear/misc/sg_path.hxx"

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#define ID_INITIALCONDITION "$Id: FGInitialCondition.h,v 1.48 2017/02/25 14:23:18 bcoconni Exp $"

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

namespace JSBSim {

class FGFDMExec;
class FGMatrix33;
class FGColumnVector3;
class FGAtmosphere;
class FGAircraft;
class FGPropertyManager;
class Element;

typedef enum { setvt, setvc, setve, setmach, setuvw, setned, setvg } speedset;
typedef enum { setasl, setagl } altitudeset;
typedef enum { setgeoc, setgeod } latitudeset;

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

class FGInitialCondition : public FGJSBBase
{
public:
  FGInitialCondition(FGFDMExec *fdmex);
  ~FGInitialCondition();

  void SetVcalibratedKtsIC(double vc);

  void SetVequivalentKtsIC(double ve);

  void SetVtrueKtsIC(double vtrue) { SetVtrueFpsIC(vtrue*ktstofps); }

  void SetVgroundKtsIC(double vg) { SetVgroundFpsIC(vg*ktstofps); }

  void SetMachIC(double mach);

  void SetAlphaDegIC(double a) { SetAlphaRadIC(a*degtorad); }

  void SetBetaDegIC(double b) { SetBetaRadIC(b*degtorad);}

  void SetThetaDegIC(double theta) { SetThetaRadIC(theta*degtorad); }

  void ResetIC(double u0, double v0, double w0, double p0, double q0, double r0,
               double alpha0, double beta0, double phi0, double theta0, double psi0,
               double latitudeRad0, double longitudeRad0, double altitudeAGL0,
               double gamma0);

  void SetPhiDegIC(double phi)  { SetPhiRadIC(phi*degtorad);}

  void SetPsiDegIC(double psi){ SetPsiRadIC(psi*degtorad); }

  void SetClimbRateFpmIC(double roc) { SetClimbRateFpsIC(roc/60.0); }

  void SetFlightPathAngleDegIC(double gamma)
  { SetClimbRateFpsIC(vt*sin(gamma*degtorad)); }

  void SetAltitudeASLFtIC(double altitudeASL);

  void SetAltitudeAGLFtIC(double agl);

  void SetSeaLevelRadiusFtIC(double slr);

  void SetTerrainElevationFtIC(double elev);

  void SetLatitudeDegIC(double lat) { SetLatitudeRadIC(lat*degtorad); }

  void SetGeodLatitudeDegIC(double glat)
  { SetGeodLatitudeRadIC(glat*degtorad); }

  void SetLongitudeDegIC(double lon) { SetLongitudeRadIC(lon*degtorad); }

  double GetVcalibratedKtsIC(void) const;

  double GetVequivalentKtsIC(void) const;

  double GetVgroundKtsIC(void) const { return GetVgroundFpsIC() * fpstokts; }

  double GetVtrueKtsIC(void) const { return vt*fpstokts; }

  double GetMachIC(void) const;

  double GetClimbRateFpmIC(void) const
  { return GetClimbRateFpsIC()*60; }

  double GetFlightPathAngleDegIC(void) const
  { return GetFlightPathAngleRadIC()*radtodeg; }

  double GetAlphaDegIC(void) const { return alpha*radtodeg; }

  double GetBetaDegIC(void) const  { return beta*radtodeg; }

  double GetThetaDegIC(void) const { return orientation.GetEulerDeg(eTht); }

  double GetPhiDegIC(void) const { return orientation.GetEulerDeg(ePhi); }

  double GetPsiDegIC(void) const { return orientation.GetEulerDeg(ePsi); }

  double GetLatitudeDegIC(void) const { return position.GetLatitudeDeg(); }

  double GetGeodLatitudeDegIC(void) const
  { return position.GetGeodLatitudeDeg(); }

  double GetLongitudeDegIC(void) const { return position.GetLongitudeDeg(); }

  double GetAltitudeASLFtIC(void) const { return position.GetAltitudeASL(); }

  double GetAltitudeAGLFtIC(void) const;

  double GetTerrainElevationFtIC(void) const;

  void SetVgroundFpsIC(double vg);

  void SetVtrueFpsIC(double vt);

  void SetUBodyFpsIC(double ubody) { SetBodyVelFpsIC(eU, ubody); }

  void SetVBodyFpsIC(double vbody) { SetBodyVelFpsIC(eV, vbody); }

  void SetWBodyFpsIC(double wbody) { SetBodyVelFpsIC(eW, wbody); }

  void SetVNorthFpsIC(double vn) { SetNEDVelFpsIC(eU, vn); }

  void SetVEastFpsIC(double ve) { SetNEDVelFpsIC(eV, ve); }

  void SetVDownFpsIC(double vd) { SetNEDVelFpsIC(eW, vd); }

  void SetPRadpsIC(double P)  { vPQR_body(eP) = P; }

  void SetQRadpsIC(double Q) { vPQR_body(eQ) = Q; }

  void SetRRadpsIC(double R) { vPQR_body(eR) = R; }

  void SetWindNEDFpsIC(double wN, double wE, double wD);

  void SetWindMagKtsIC(double mag);

  void SetWindDirDegIC(double dir);

  void SetHeadWindKtsIC(double head);

  void SetCrossWindKtsIC(double cross);

  void SetWindDownKtsIC(double wD);

  void SetClimbRateFpsIC(double roc);

  double GetVgroundFpsIC(void) const  { return vUVW_NED.Magnitude(eU, eV); }

  double GetVtrueFpsIC(void) const { return vt; }

  double GetWindUFpsIC(void) const { return GetBodyWindFpsIC(eU); }

  double GetWindVFpsIC(void) const { return GetBodyWindFpsIC(eV); }

  double GetWindWFpsIC(void) const { return GetBodyWindFpsIC(eW); }

  const FGColumnVector3 GetWindNEDFpsIC(void) const {
    const FGMatrix33& Tb2l = orientation.GetTInv();
    FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
    return _vt_NED - vUVW_NED;
  }

  double GetWindNFpsIC(void) const { return GetNEDWindFpsIC(eX); }

  double GetWindEFpsIC(void) const { return GetNEDWindFpsIC(eY); }

  double GetWindDFpsIC(void) const { return GetNEDWindFpsIC(eZ); }

  double GetWindFpsIC(void)  const;

  double GetWindDirDegIC(void) const;

  double GetClimbRateFpsIC(void) const
  {
    const FGMatrix33& Tb2l = orientation.GetTInv();
    FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
    return _vt_NED(eW);
  }

  const FGColumnVector3 GetUVWFpsIC(void) const {
    const FGMatrix33& Tl2b = orientation.GetT();
    return Tl2b * vUVW_NED;
  }

  double GetUBodyFpsIC(void) const { return GetBodyVelFpsIC(eU); }

  double GetVBodyFpsIC(void) const { return GetBodyVelFpsIC(eV); }

  double GetWBodyFpsIC(void) const { return GetBodyVelFpsIC(eW); }

  double GetVNorthFpsIC(void) const { return vUVW_NED(eU); }

  double GetVEastFpsIC(void) const { return vUVW_NED(eV); }

  double GetVDownFpsIC(void) const { return vUVW_NED(eW); }

  const FGColumnVector3 GetPQRRadpsIC(void) const { return vPQR_body; }

  double GetPRadpsIC() const { return vPQR_body(eP); }

  double GetQRadpsIC() const { return vPQR_body(eQ); }

  double GetRRadpsIC() const { return vPQR_body(eR); }

  void SetFlightPathAngleRadIC(double gamma)
  { SetClimbRateFpsIC(vt*sin(gamma)); }

  void SetAlphaRadIC(double alpha);

  void SetBetaRadIC(double beta);

  void SetPhiRadIC(double phi) { SetEulerAngleRadIC(ePhi, phi); }

  void SetThetaRadIC(double theta) { SetEulerAngleRadIC(eTht, theta); }

  void SetPsiRadIC(double psi) { SetEulerAngleRadIC(ePsi, psi); }

  void SetLatitudeRadIC(double lat);

  void SetGeodLatitudeRadIC(double glat);

  void SetLongitudeRadIC(double lon);

  void SetTargetNlfIC(double nlf) { targetNlfIC=nlf; }

  double GetFlightPathAngleRadIC(void) const
  { return (vt == 0.0)?0.0:asin(GetClimbRateFpsIC() / vt); }

  double GetAlphaRadIC(void) const { return alpha; }

  double GetBetaRadIC(void) const { return beta; }

  const FGLocation& GetPosition(void) const { return position; }

  double GetLatitudeRadIC(void) const { return position.GetLatitude(); }

  double GetGeodLatitudeRadIC(void) const
  { return position.GetGeodLatitudeRad(); }

  double GetLongitudeRadIC(void) const { return position.GetLongitude(); }

  const FGQuaternion& GetOrientation(void) const { return orientation; }

  double GetPhiRadIC(void) const { return orientation.GetEuler(ePhi); }

  double GetThetaRadIC(void) const { return orientation.GetEuler(eTht); }

  double GetPsiRadIC(void) const   { return orientation.GetEuler(ePsi); }

  speedset GetSpeedSet(void) const { return lastSpeedSet; }

  double GetTargetNlfIC(void) const { return targetNlfIC; }

  bool Load(const SGPath& rstname, bool useStoredPath = true );

  bool IsEngineRunning(unsigned int n) const { return (enginesRunning & (1 << n)) != 0; }
  
  bool NeedTrim(void) const { return needTrim == 0 ? false : true; }

  void bind(FGPropertyManager* pm);

private:
  FGColumnVector3 vUVW_NED;
  FGColumnVector3 vPQR_body;
  FGLocation position;
  FGQuaternion orientation;
  double vt;

  double targetNlfIC;

  FGMatrix33 Tw2b, Tb2w;
  double  alpha, beta;
  double a, e2;

  speedset lastSpeedSet;
  altitudeset lastAltitudeSet;
  latitudeset lastLatitudeSet;
  unsigned int enginesRunning;
  int needTrim;

  FGFDMExec *fdmex;
  FGAtmosphere* Atmosphere;
  FGAircraft* Aircraft;

  bool Load_v1(Element* document);
  bool Load_v2(Element* document);

  void InitializeIC(void);
  void SetEulerAngleRadIC(int idx, double angle);
  void SetBodyVelFpsIC(int idx, double vel);
  void SetNEDVelFpsIC(int idx, double vel);
  double GetBodyWindFpsIC(int idx) const;
  double GetNEDWindFpsIC(int idx) const;
  double GetBodyVelFpsIC(int idx) const;
  void calcAeroAngles(const FGColumnVector3& _vt_BODY);
  void calcThetaBeta(double alfa, const FGColumnVector3& _vt_NED);
  double ComputeGeodAltitude(double geodLatitude);
  bool LoadLatitude(Element* position_el);
  void Debug(int from);
};
}
#endif