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 <memory>
 
#include "math/FGLocation.h"
#include "math/FGQuaternion.h"
#include "simgear/misc/sg_path.hxx"
 
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
namespace JSBSim {
 
class FGFDMExec;
class FGMatrix33;
class FGColumnVector3;
class FGAircraft;
class FGAuxiliary;
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 JSBSIM_API FGInitialCondition : public FGJSBBase
{
public:
  explicit 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 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;
 
  double GetAltitudeAGLFtIC(void) const;
 
  double GetTerrainElevationFtIC(void) const;
 
  double GetEarthPositionAngleIC(void) const { return epa; }
 
  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 SetWindMagFpsIC(double mag) { SetWindMagKtsIC(mag * fpstokts); }
 
  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); }
 
  FGColumnVector3 GetWindNEDFpsIC(void) const;
 
  double GetWindNFpsIC(void) const { return GetWindNEDFpsIC()(eX); }
 
  double GetWindEFpsIC(void) const { return GetWindNEDFpsIC()(eY); }
 
  double GetWindDFpsIC(void) const { return GetWindNEDFpsIC()(eZ); }
 
  double GetWindMagFpsIC(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 useAircraftPath = true );
 
  bool IsEngineRunning(unsigned int n) const { return (enginesRunning & (1 << n)) != 0; }
 
  int TrimRequested(void) const { return trimRequested; }
 
  void InitializeIC(void);
 
  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 epa;
 
  speedset lastSpeedSet;
  altitudeset lastAltitudeSet;
  latitudeset lastLatitudeSet;
  unsigned int enginesRunning;
  int trimRequested;
 
  FGFDMExec *fdmex;
  std::shared_ptr<FGAircraft> Aircraft;
  std::shared_ptr<FGAuxiliary> Auxiliary;
 
  bool Load_v1(Element* document);
  bool Load_v2(Element* document);
 
  void SetEulerAngleRadIC(int idx, double angle);
  void SetBodyVelFpsIC(int idx, double vel);
  void SetNEDVelFpsIC(int idx, double vel);
  double GetBodyWindFpsIC(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 SetTrimRequest(std::string trim);
  void Debug(int from);
};
}
#endif