src/FDM/JSBSim/FGInitialCondition.h

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Header:       FGInitialCondition.h
   4  Author:       Tony Peden
   5  Date started: 7/1/99
   6
   7  ------------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) -------------
   8
   9  This program is free software; you can redistribute it and/or modify it under
  10  the terms of the GNU General Public License as published by the Free Software
  11  Foundation; either version 2 of the License, or (at your option) any later
  12  version.
  13
  14  This program is distributed in the hope that it will be useful, but WITHOUT
  15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  16  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU General Public License along with
  20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  21  Place - Suite 330, Boston, MA  02111-1307, USA.
  22
  23  Further information about the GNU General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26  HISTORY
  27 --------------------------------------------------------------------------------
  28 7/1/99   TP   Created
  29
  30 FUNCTIONAL DESCRIPTION
  31 --------------------------------------------------------------------------------
  32
  33 The purpose of this class is to take a set of initial conditions and provide
  34 a kinematically consistent set of body axis velocity components, euler
  35 angles, and altitude.  This class does not attempt to trim the model i.e.
  36 the sim will most likely start in a very dynamic state (unless, of course,
  37 you have chosen your IC's wisely) even after setting it up with this class.
  38
  39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  40 SENTRY
  41 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  42
  43 #ifndef FGINITIALCONDITION_H
  44 #define FGINITIALCONDITION_H
  45
  46 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  47 INCLUDES
  48 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  49
  50 #include "FGFDMExec.h"
  51 #include "FGJSBBase.h"
  52 #include "FGAtmosphere.h"
  53 #include "FGMatrix33.h"
  54 #include "FGColumnVector3.h"
  55 #include "FGColumnVector4.h"
  56
  57 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  58 DEFINITIONS
  59 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  60
  61 #define ID_INITIALCONDITION "$Id$"
  62
  63 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  64 FORWARD DECLARATIONS
  65 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  66
  67 namespace JSBSim {
  68
  69 typedef enum { setvt, setvc, setve, setmach, setuvw, setned, setvg } speedset;
  70 typedef enum { setwned, setwmd, setwhc } windset;
  71
  72 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  73 CLASS DOCUMENTATION
  74 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  75
  76 /** Takes a set of initial conditions and provide a kinematically consistent set
  77     of body axis velocity components, euler angles, and altitude.  This class
  78     does not attempt to trim the model i.e. the sim will most likely start in a
  79     very dynamic state (unless, of course, you have chosen your IC's wisely)
  80     even after setting it up with this class.
  81
  82    USAGE NOTES
  83
  84    With a valid object of FGFDMExec and an aircraft model loaded
  85    FGInitialCondition fgic=new FGInitialCondition(FDMExec);
  86    fgic->SetVcalibratedKtsIC()
  87    fgic->SetAltitudeFtIC();
  88
  89    //to directly into Run
  90    FDMExec->GetState()->Initialize(fgic)
  91    delete fgic;
  92    FDMExec->Run()
  93
  94    //or to loop the sim w/o integrating
  95    FDMExec->RunIC(fgic)
  96
  97    Speed:
  98
  99    Since vc, ve, vt, and mach all represent speed, the remaining
 100    three are recalculated each time one of them is set (using the
 101    current altitude).  The most recent speed set is remembered so
 102    that if and when altitude is reset, the last set speed is used
 103    to recalculate the remaining three. Setting any of the body
 104    components forces a recalculation of vt and vt then becomes the
 105    most recent speed set.
 106
 107    Alpha,Gamma, and Theta:
 108
 109    This class assumes that it will be used to set up the sim for a
 110    steady, zero pitch rate condition. Since any two of those angles
 111    specifies the third gamma (flight path angle) is favored when setting
 112    alpha and theta and alpha is favored when setting gamma. i.e.
 113
 114    - set alpha : recalculate theta using gamma as currently set
 115    - set theta : recalculate alpha using gamma as currently set
 116    - set gamma : recalculate theta using alpha as currently set
 117
 118    The idea being that gamma is most interesting to pilots (since it
 119    is indicative of climb rate).
 120
 121    Setting climb rate is, for the purpose of this discussion,
 122    considered equivalent to setting gamma.
 123    @author Tony Peden
 124    @version "$Id$"
 125 */
 126
 127 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 128 CLASS DECLARATION
 129 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 130
 131 class FGInitialCondition : public FGJSBBase
 132 {
 133 public:
 134   /// Constructor
 135   FGInitialCondition(FGFDMExec *fdmex);
 136   /// Destructor
 137   ~FGInitialCondition();
 138
 139   void SetVcalibratedKtsIC(double tt);
 140   void SetVequivalentKtsIC(double tt);
 141   inline void SetVtrueKtsIC(double tt)   { SetVtrueFpsIC(tt*ktstofps);   }
 142   inline void SetVgroundKtsIC(double tt) { SetVgroundFpsIC(tt*ktstofps); }
 143   void SetMachIC(double tt);
 144
 145   inline void SetAlphaDegIC(double tt)      { SetAlphaRadIC(tt*degtorad); }
 146   inline void SetBetaDegIC(double tt)       { SetBetaRadIC(tt*degtorad);}
 147
 148   inline void SetPitchAngleDegIC(double tt) { SetPitchAngleRadIC(tt*degtorad); }
 149   inline void SetRollAngleDegIC(double tt)  { SetRollAngleRadIC(tt*degtorad);}
 150   inline void SetTrueHeadingDegIC(double tt){ SetTrueHeadingRadIC(tt*degtorad); }
 151
 152   void SetClimbRateFpmIC(double tt);
 153   inline void SetFlightPathAngleDegIC(double tt) { SetFlightPathAngleRadIC(tt*degtorad); }
 154
 155   void SetAltitudeFtIC(double tt);
 156   void SetAltitudeAGLFtIC(double tt);
 157
 158   void SetSeaLevelRadiusFtIC(double tt);
 159   void SetTerrainAltitudeFtIC(double tt);
 160
 161   inline void SetLatitudeDegIC(double tt)  { latitude=tt*degtorad; }
 162   inline void SetLongitudeDegIC(double tt) { longitude=tt*degtorad; }
 163
 164
 165   inline double GetVcalibratedKtsIC(void) const { return vc*fpstokts; }
 166   inline double GetVequivalentKtsIC(void) const { return ve*fpstokts; }
 167   inline double GetVgroundKtsIC(void) const { return vg*fpstokts; }
 168   inline double GetVtrueKtsIC(void) const { return vt*fpstokts; }
 169   inline double GetMachIC(void) const { return mach; }
 170
 171   inline double GetClimbRateFpmIC(void) const { return hdot*60; }
 172   inline double GetFlightPathAngleDegIC(void)const  { return gamma*radtodeg; }
 173
 174   inline double GetAlphaDegIC(void) const { return alpha*radtodeg; }
 175   inline double GetBetaDegIC(void) const  { return beta*radtodeg; }
 176
 177   inline double GetPitchAngleDegIC(void) const { return theta*radtodeg; }
 178   inline double GetRollAngleDegIC(void) const { return phi*radtodeg; }
 179   inline double GetHeadingDegIC(void) const { return psi*radtodeg; }
 180
 181   inline double GetLatitudeDegIC(void) const { return latitude*radtodeg; }
 182   inline double GetLongitudeDegIC(void) const { return longitude*radtodeg; }
 183
 184   inline double GetAltitudeFtIC(void) const { return altitude; }
 185   inline double GetAltitudeAGLFtIC(void) const { return altitude - terrain_altitude; }
 186
 187   inline double GetSeaLevelRadiusFtIC(void) const { return sea_level_radius; }
 188   inline double GetTerrainAltitudeFtIC(void) const { return terrain_altitude; }
 189
 190   void SetVgroundFpsIC(double tt);
 191   void SetVtrueFpsIC(double tt);
 192   void SetUBodyFpsIC(double tt);
 193   void SetVBodyFpsIC(double tt);
 194   void SetWBodyFpsIC(double tt);
 195   void SetVnorthFpsIC(double tt);
 196   void SetVeastFpsIC(double tt);
 197   void SetVdownFpsIC(double tt);
 198
 199   void SetWindNEDFpsIC(double wN, double wE, double wD);
 200
 201   void SetWindMagKtsIC(double mag);
 202   void SetWindDirDegIC(double dir);
 203
 204   void SetHeadWindKtsIC(double head);
 205   void SetCrossWindKtsIC(double cross);// positive from left
 206
 207   void SetWindDownKtsIC(double wD);
 208
 209   void SetClimbRateFpsIC(double tt);
 210   inline double GetVgroundFpsIC(void) const  { return vg; }
 211   inline double GetVtrueFpsIC(void) const { return vt; }
 212   inline double GetWindUFpsIC(void) const { return uw; }
 213   inline double GetWindVFpsIC(void) const { return vw; }
 214   inline double GetWindWFpsIC(void) const { return ww; }
 215   inline double GetWindNFpsIC(void) const { return wnorth; }
 216   inline double GetWindEFpsIC(void) const { return weast; }
 217   inline double GetWindDFpsIC(void) const { return wdown; }
 218   inline double GetWindFpsIC(void)  const { return sqrt(wnorth*wnorth + weast*weast); }
 219   double GetWindDirDegIC(void);
 220   inline double GetClimbRateFpsIC(void) const { return hdot; }
 221   double GetUBodyFpsIC(void);
 222   double GetVBodyFpsIC(void);
 223   double GetWBodyFpsIC(void);
 224   void SetFlightPathAngleRadIC(double tt);
 225   void SetAlphaRadIC(double tt);
 226   void SetPitchAngleRadIC(double tt);
 227   void SetBetaRadIC(double tt);
 228   void SetRollAngleRadIC(double tt);
 229   void SetTrueHeadingRadIC(double tt);
 230   inline void SetLatitudeRadIC(double tt) { latitude=tt; }
 231   inline void SetLongitudeRadIC(double tt) { longitude=tt; }
 232   inline double GetFlightPathAngleRadIC(void) const { return gamma; }
 233   inline double GetAlphaRadIC(void) const      { return alpha; }
 234   inline double GetPitchAngleRadIC(void) const { return theta; }
 235   inline double GetBetaRadIC(void) const       { return beta; }
 236   inline double GetRollAngleRadIC(void) const  { return phi; }
 237   inline double GetHeadingRadIC(void) const   { return psi; }
 238   inline double GetLatitudeRadIC(void) const { return latitude; }
 239   inline double GetLongitudeRadIC(void) const { return longitude; }
 240   inline double GetThetaRadIC(void) const { return theta; }
 241   inline double GetPhiRadIC(void)  const  { return phi; }
 242   inline double GetPsiRadIC(void) const   { return psi; }
 243
 244   inline speedset GetSpeedSet(void) { return lastSpeedSet; }
 245   inline windset GetWindSet(void) { return lastWindSet; }
 246
 247   bool Load(string rstname, bool useStoredPath = true );
 248
 249   void bind(void);
 250   void unbind(void);
 251
 252
 253 private:
 254   double vt,vc,ve,vg;
 255   double mach;
 256   double altitude,hdot;
 257   double latitude,longitude;
 258   double u,v,w;
 259   double uw,vw,ww;
 260   double vnorth,veast,vdown;
 261   double wnorth,weast,wdown;
 262   double whead, wcross, wdir, wmag;
 263   double sea_level_radius;
 264   double terrain_altitude;
 265   double radius_to_vehicle;
 266
 267   double  alpha, beta, theta, phi, psi, gamma;
 268   double salpha,sbeta,stheta,sphi,spsi,sgamma;
 269   double calpha,cbeta,ctheta,cphi,cpsi,cgamma;
 270
 271   double xlo, xhi,xmin,xmax;
 272
 273   typedef double (FGInitialCondition::*fp)(double x);
 274   fp sfunc;
 275
 276   speedset lastSpeedSet;
 277   windset lastWindSet;
 278
 279   FGFDMExec *fdmex;
 280   FGPropertyManager *PropertyManager;
 281
 282   bool getAlpha(void);
 283   bool getTheta(void);
 284   bool getMachFromVcas(double *Mach,double vcas);
 285
 286   double GammaEqOfTheta(double Theta);
 287   double GammaEqOfAlpha(double Alpha);
 288   double calcVcas(double Mach);
 289   void calcUVWfromNED(void);
 290   void calcWindUVW(void);
 291
 292   bool findInterval(double x,double guess);
 293   bool solve(double *y, double x);
 294   void Debug(int from);
 295 };
 296 }
 297 #endif
 298