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 "FGColumnVector3.h"
  53
  54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  55 DEFINITIONS
  56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  57
  58 #define ID_INITIALCONDITION "$Id$"
  59
  60 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  61 FORWARD DECLARATIONS
  62 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  63
  64 namespace JSBSim {
  65
  66 typedef enum { setvt, setvc, setve, setmach, setuvw, setned, setvg } speedset;
  67 typedef enum { setwned, setwmd, setwhc } windset;
  68
  69 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  70 CLASS DOCUMENTATION
  71 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  72
  73 /** Takes a set of initial conditions and provide a kinematically consistent set
  74     of body axis velocity components, euler angles, and altitude.  This class
  75     does not attempt to trim the model i.e. the sim will most likely start in a
  76     very dynamic state (unless, of course, you have chosen your IC's wisely)
  77     even after setting it up with this class.
  78
  79    USAGE NOTES
  80
  81    With a valid object of FGFDMExec and an aircraft model loaded
  82    FGInitialCondition fgic=new FGInitialCondition(FDMExec);
  83    fgic->SetVcalibratedKtsIC()
  84    fgic->SetAltitudeFtIC();
  85
  86    //to directly into Run
  87    FDMExec->GetState()->Initialize(fgic)
  88    delete fgic;
  89    FDMExec->Run()
  90
  91    //or to loop the sim w/o integrating
  92    FDMExec->RunIC(fgic)
  93
  94    Speed:
  95
  96    Since vc, ve, vt, and mach all represent speed, the remaining
  97    three are recalculated each time one of them is set (using the
  98    current altitude).  The most recent speed set is remembered so
  99    that if and when altitude is reset, the last set speed is used
 100    to recalculate the remaining three. Setting any of the body
 101    components forces a recalculation of vt and vt then becomes the
 102    most recent speed set.
 103
 104    Alpha,Gamma, and Theta:
 105
 106    This class assumes that it will be used to set up the sim for a
 107    steady, zero pitch rate condition. Since any two of those angles
 108    specifies the third gamma (flight path angle) is favored when setting
 109    alpha and theta and alpha is favored when setting gamma. i.e.
 110
 111    - set alpha : recalculate theta using gamma as currently set
 112    - set theta : recalculate alpha using gamma as currently set
 113    - set gamma : recalculate theta using alpha as currently set
 114
 115    The idea being that gamma is most interesting to pilots (since it
 116    is indicative of climb rate).
 117
 118    Setting climb rate is, for the purpose of this discussion,
 119    considered equivalent to setting gamma.
 120    @author Tony Peden
 121    @version "$Id$"
 122 */
 123
 124 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 125 CLASS DECLARATION
 126 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 127
 128 class FGInitialCondition : public FGJSBBase
 129 {
 130 public:
 131   /// Constructor
 132   FGInitialCondition(FGFDMExec *fdmex);
 133   /// Destructor
 134   ~FGInitialCondition();
 135
 136   void SetVcalibratedKtsIC(double tt);
 137   void SetVequivalentKtsIC(double tt);
 138   inline void SetVtrueKtsIC(double tt)   { SetVtrueFpsIC(tt*ktstofps);   }
 139   inline void SetVgroundKtsIC(double tt) { SetVgroundFpsIC(tt*ktstofps); }
 140   void SetMachIC(double tt);
 141
 142   inline void SetAlphaDegIC(double tt)      { SetAlphaRadIC(tt*degtorad); }
 143   inline void SetBetaDegIC(double tt)       { SetBetaRadIC(tt*degtorad);}
 144
 145   inline void SetPitchAngleDegIC(double tt) { SetPitchAngleRadIC(tt*degtorad); }
 146   inline void SetRollAngleDegIC(double tt)  { SetRollAngleRadIC(tt*degtorad);}
 147   inline void SetTrueHeadingDegIC(double tt){ SetTrueHeadingRadIC(tt*degtorad); }
 148
 149   void SetClimbRateFpmIC(double tt);
 150   inline void SetFlightPathAngleDegIC(double tt) { SetFlightPathAngleRadIC(tt*degtorad); }
 151
 152   void SetAltitudeFtIC(double tt);
 153   void SetAltitudeAGLFtIC(double tt);
 154
 155   void SetSeaLevelRadiusFtIC(double tt);
 156   void SetTerrainAltitudeFtIC(double tt);
 157
 158   inline void SetLatitudeDegIC(double tt)  { latitude=tt*degtorad; }
 159   inline void SetLongitudeDegIC(double tt) { longitude=tt*degtorad; }
 160
 161
 162   inline double GetVcalibratedKtsIC(void) const { return vc*fpstokts; }
 163   inline double GetVequivalentKtsIC(void) const { return ve*fpstokts; }
 164   inline double GetVgroundKtsIC(void) const { return vg*fpstokts; }
 165   inline double GetVtrueKtsIC(void) const { return vt*fpstokts; }
 166   inline double GetMachIC(void) const { return mach; }
 167
 168   inline double GetClimbRateFpmIC(void) const { return hdot*60; }
 169   inline double GetFlightPathAngleDegIC(void)const  { return gamma*radtodeg; }
 170
 171   inline double GetAlphaDegIC(void) const { return alpha*radtodeg; }
 172   inline double GetBetaDegIC(void) const  { return beta*radtodeg; }
 173
 174   inline double GetPitchAngleDegIC(void) const { return theta*radtodeg; }
 175   inline double GetRollAngleDegIC(void) const { return phi*radtodeg; }
 176   inline double GetHeadingDegIC(void) const { return psi*radtodeg; }
 177
 178   inline double GetLatitudeDegIC(void) const { return latitude*radtodeg; }
 179   inline double GetLongitudeDegIC(void) const { return longitude*radtodeg; }
 180
 181   inline double GetAltitudeFtIC(void) const { return altitude; }
 182   inline double GetAltitudeAGLFtIC(void) const { return altitude - terrain_altitude; }
 183
 184   inline double GetSeaLevelRadiusFtIC(void) const { return sea_level_radius; }
 185   inline double GetTerrainAltitudeFtIC(void) const { return terrain_altitude; }
 186
 187   void SetVgroundFpsIC(double tt);
 188   void SetVtrueFpsIC(double tt);
 189   void SetUBodyFpsIC(double tt);
 190   void SetVBodyFpsIC(double tt);
 191   void SetWBodyFpsIC(double tt);
 192   void SetVnorthFpsIC(double tt);
 193   void SetVeastFpsIC(double tt);
 194   void SetVdownFpsIC(double tt);
 195   void SetPRadpsIC(double tt)  { p = tt; }
 196   void SetQRadpsIC(double tt) { q = tt; }
 197   void SetRRadpsIC(double tt) { r = 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) const;
 222   double GetVBodyFpsIC(void) const;
 223   double GetWBodyFpsIC(void) const;
 224   double GetPRadpsIC() const { return p; }
 225   double GetQRadpsIC() const { return q; }
 226   double GetRRadpsIC() const { return r; }
 227   void SetFlightPathAngleRadIC(double tt);
 228   void SetAlphaRadIC(double tt);
 229   void SetPitchAngleRadIC(double tt);
 230   void SetBetaRadIC(double tt);
 231   void SetRollAngleRadIC(double tt);
 232   void SetTrueHeadingRadIC(double tt);
 233   inline void SetLatitudeRadIC(double tt) { latitude=tt; }
 234   inline void SetLongitudeRadIC(double tt) { longitude=tt; }
 235   inline double GetFlightPathAngleRadIC(void) const { return gamma; }
 236   inline double GetAlphaRadIC(void) const      { return alpha; }
 237   inline double GetPitchAngleRadIC(void) const { return theta; }
 238   inline double GetBetaRadIC(void) const       { return beta; }
 239   inline double GetRollAngleRadIC(void) const  { return phi; }
 240   inline double GetHeadingRadIC(void) const   { return psi; }
 241   inline double GetLatitudeRadIC(void) const { return latitude; }
 242   inline double GetLongitudeRadIC(void) const { return longitude; }
 243   inline double GetThetaRadIC(void) const { return theta; }
 244   inline double GetPhiRadIC(void)  const  { return phi; }
 245   inline double GetPsiRadIC(void) const   { return psi; }
 246
 247   inline speedset GetSpeedSet(void) { return lastSpeedSet; }
 248   inline windset GetWindSet(void) { return lastWindSet; }
 249
 250   bool Load(string rstname, bool useStoredPath = true );
 251
 252   void bind(void);
 253   void unbind(void);
 254
 255
 256 private:
 257   double vt,vc,ve,vg;
 258   double mach;
 259   double altitude,hdot;
 260   double latitude,longitude;
 261   double u,v,w;
 262   double p,q,r;
 263   double uw,vw,ww;
 264   double vnorth,veast,vdown;
 265   double wnorth,weast,wdown;
 266   double whead, wcross, wdir, wmag;
 267   double sea_level_radius;
 268   double terrain_altitude;
 269   double radius_to_vehicle;
 270
 271   double  alpha, beta, theta, phi, psi, gamma;
 272   double salpha,sbeta,stheta,sphi,spsi,sgamma;
 273   double calpha,cbeta,ctheta,cphi,cpsi,cgamma;
 274
 275   double xlo, xhi,xmin,xmax;
 276
 277   typedef double (FGInitialCondition::*fp)(double x);
 278   fp sfunc;
 279
 280   speedset lastSpeedSet;
 281   windset lastWindSet;
 282
 283   FGFDMExec *fdmex;
 284   FGPropertyManager *PropertyManager;
 285
 286   bool getAlpha(void);
 287   bool getTheta(void);
 288   bool getMachFromVcas(double *Mach,double vcas);
 289
 290   double GammaEqOfTheta(double Theta);
 291   double GammaEqOfAlpha(double Alpha);
 292   double calcVcas(double Mach);
 293   void calcUVWfromNED(void);
 294   void calcWindUVW(void);
 295
 296   bool findInterval(double x,double guess);
 297   bool solve(double *y, double x);
 298   void Debug(int from);
 299 };
 300 }
 301 #endif
 302