src/FDM/JSBSim/initialization/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 <math/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
 121    These are the items that can be set in an initialization file:
 122
 123    UBODY <velocity, ft/sec>
 124    VBODY <velocity, ft/sec>
 125    WBODY <velocity, ft/sec>
 126    LATITUDE <position, degrees>
 127    LONGITUDE <position, degrees>
 128    PHI <orientation, degrees>
 129    THETA <orientation, degrees>
 130    PSI <orientation, degrees>
 131    ALPHA <angle, degrees>
 132    BETA <angle, degrees>
 133    GAMMA <angle, degrees>
 134    ROC <vertical velocity, ft/sec>
 135    ALTITUDE <altitude, ft>
 136    WINDDIR <wind from-angle, degrees>
 137    VWIND <magnitude wind speed, ft/sec>
 138    HWIND <headwind speed, knots>
 139    XWIND <crosswind speed, knots>
 140    VC <calibrated airspeed, ft/sec>
 141    MACH <mach>
 142    VGROUND <ground speed, ft/sec>
 143    RUNNING <0 or 1>
 144
 145
 146    @author Tony Peden
 147    @version "$Id$"
 148 */
 149
 150 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 151 CLASS DECLARATION
 152 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 153
 154 class FGInitialCondition : public FGJSBBase
 155 {
 156 public:
 157   /// Constructor
 158   FGInitialCondition(FGFDMExec *fdmex);
 159   /// Destructor
 160   ~FGInitialCondition();
 161
 162   void SetVcalibratedKtsIC(double tt);
 163   void SetVequivalentKtsIC(double tt);
 164   inline void SetVtrueKtsIC(double tt)   { SetVtrueFpsIC(tt*ktstofps);   }
 165   inline void SetVgroundKtsIC(double tt) { SetVgroundFpsIC(tt*ktstofps); }
 166   void SetMachIC(double tt);
 167
 168   inline void SetAlphaDegIC(double tt)      { SetAlphaRadIC(tt*degtorad); }
 169   inline void SetBetaDegIC(double tt)       { SetBetaRadIC(tt*degtorad);}
 170
 171   inline void SetPitchAngleDegIC(double tt) { SetPitchAngleRadIC(tt*degtorad); }
 172   inline void SetRollAngleDegIC(double tt)  { SetRollAngleRadIC(tt*degtorad);}
 173   inline void SetTrueHeadingDegIC(double tt){ SetTrueHeadingRadIC(tt*degtorad); }
 174
 175   void SetClimbRateFpmIC(double tt);
 176   inline void SetFlightPathAngleDegIC(double tt) { SetFlightPathAngleRadIC(tt*degtorad); }
 177
 178   void SetAltitudeFtIC(double tt);
 179   void SetAltitudeAGLFtIC(double tt);
 180
 181   void SetSeaLevelRadiusFtIC(double tt);
 182   void SetTerrainAltitudeFtIC(double tt);
 183
 184   inline void SetLatitudeDegIC(double tt)  { latitude=tt*degtorad; }
 185   inline void SetLongitudeDegIC(double tt) { longitude=tt*degtorad; }
 186
 187
 188   inline double GetVcalibratedKtsIC(void) const { return vc*fpstokts; }
 189   inline double GetVequivalentKtsIC(void) const { return ve*fpstokts; }
 190   inline double GetVgroundKtsIC(void) const { return vg*fpstokts; }
 191   inline double GetVtrueKtsIC(void) const { return vt*fpstokts; }
 192   inline double GetMachIC(void) const { return mach; }
 193
 194   inline double GetClimbRateFpmIC(void) const { return hdot*60; }
 195   inline double GetFlightPathAngleDegIC(void)const  { return gamma*radtodeg; }
 196
 197   inline double GetAlphaDegIC(void) const { return alpha*radtodeg; }
 198   inline double GetBetaDegIC(void) const  { return beta*radtodeg; }
 199
 200   inline double GetPitchAngleDegIC(void) const { return theta*radtodeg; }
 201   inline double GetRollAngleDegIC(void) const { return phi*radtodeg; }
 202   inline double GetHeadingDegIC(void) const { return psi*radtodeg; }
 203
 204   inline double GetLatitudeDegIC(void) const { return latitude*radtodeg; }
 205   inline double GetLongitudeDegIC(void) const { return longitude*radtodeg; }
 206
 207   inline double GetAltitudeFtIC(void) const { return altitude; }
 208   inline double GetAltitudeAGLFtIC(void) const { return altitude - terrain_altitude; }
 209
 210   inline double GetSeaLevelRadiusFtIC(void) const { return sea_level_radius; }
 211   inline double GetTerrainAltitudeFtIC(void) const { return terrain_altitude; }
 212
 213   void SetVgroundFpsIC(double tt);
 214   void SetVtrueFpsIC(double tt);
 215   void SetUBodyFpsIC(double tt);
 216   void SetVBodyFpsIC(double tt);
 217   void SetWBodyFpsIC(double tt);
 218   void SetVnorthFpsIC(double tt);
 219   void SetVeastFpsIC(double tt);
 220   void SetVdownFpsIC(double tt);
 221   void SetPRadpsIC(double tt)  { p = tt; }
 222   void SetQRadpsIC(double tt) { q = tt; }
 223   void SetRRadpsIC(double tt) { r = tt; }
 224
 225   void SetWindNEDFpsIC(double wN, double wE, double wD);
 226
 227   void SetWindMagKtsIC(double mag);
 228   void SetWindDirDegIC(double dir);
 229
 230   void SetHeadWindKtsIC(double head);
 231   void SetCrossWindKtsIC(double cross);// positive from left
 232
 233   void SetWindDownKtsIC(double wD);
 234
 235   void SetClimbRateFpsIC(double tt);
 236   inline double GetVgroundFpsIC(void) const  { return vg; }
 237   inline double GetVtrueFpsIC(void) const { return vt; }
 238   inline double GetWindUFpsIC(void) const { return uw; }
 239   inline double GetWindVFpsIC(void) const { return vw; }
 240   inline double GetWindWFpsIC(void) const { return ww; }
 241   inline double GetWindNFpsIC(void) const { return wnorth; }
 242   inline double GetWindEFpsIC(void) const { return weast; }
 243   inline double GetWindDFpsIC(void) const { return wdown; }
 244   inline double GetWindFpsIC(void)  const { return sqrt(wnorth*wnorth + weast*weast); }
 245   double GetWindDirDegIC(void);
 246   inline double GetClimbRateFpsIC(void) const { return hdot; }
 247   double GetUBodyFpsIC(void) const;
 248   double GetVBodyFpsIC(void) const;
 249   double GetWBodyFpsIC(void) const;
 250   double GetPRadpsIC() const { return p; }
 251   double GetQRadpsIC() const { return q; }
 252   double GetRRadpsIC() const { return r; }
 253   void SetFlightPathAngleRadIC(double tt);
 254   void SetAlphaRadIC(double tt);
 255   void SetPitchAngleRadIC(double tt);
 256   void SetBetaRadIC(double tt);
 257   void SetRollAngleRadIC(double tt);
 258   void SetTrueHeadingRadIC(double tt);
 259   inline void SetLatitudeRadIC(double tt) { latitude=tt; }
 260   inline void SetLongitudeRadIC(double tt) { longitude=tt; }
 261   inline double GetFlightPathAngleRadIC(void) const { return gamma; }
 262   inline double GetAlphaRadIC(void) const      { return alpha; }
 263   inline double GetPitchAngleRadIC(void) const { return theta; }
 264   inline double GetBetaRadIC(void) const       { return beta; }
 265   inline double GetRollAngleRadIC(void) const  { return phi; }
 266   inline double GetHeadingRadIC(void) const   { return psi; }
 267   inline double GetLatitudeRadIC(void) const { return latitude; }
 268   inline double GetLongitudeRadIC(void) const { return longitude; }
 269   inline double GetThetaRadIC(void) const { return theta; }
 270   inline double GetPhiRadIC(void)  const  { return phi; }
 271   inline double GetPsiRadIC(void) const   { return psi; }
 272
 273   inline speedset GetSpeedSet(void) { return lastSpeedSet; }
 274   inline windset GetWindSet(void) { return lastWindSet; }
 275
 276   bool Load(string rstname, bool useStoredPath = true );
 277
 278   void bind(void);
 279   void unbind(void);
 280
 281
 282 private:
 283   double vt,vc,ve,vg;
 284   double mach;
 285   double altitude,hdot;
 286   double latitude,longitude;
 287   double u,v,w;
 288   double p,q,r;
 289   double uw,vw,ww;
 290   double vnorth,veast,vdown;
 291   double wnorth,weast,wdown;
 292   double whead, wcross, wdir, wmag;
 293   double sea_level_radius;
 294   double terrain_altitude;
 295   double radius_to_vehicle;
 296
 297   double  alpha, beta, theta, phi, psi, gamma;
 298   double salpha,sbeta,stheta,sphi,spsi,sgamma;
 299   double calpha,cbeta,ctheta,cphi,cpsi,cgamma;
 300
 301   double xlo, xhi,xmin,xmax;
 302
 303   typedef double (FGInitialCondition::*fp)(double x);
 304   fp sfunc;
 305
 306   speedset lastSpeedSet;
 307   windset lastWindSet;
 308
 309   FGFDMExec *fdmex;
 310   FGPropertyManager *PropertyManager;
 311
 312   bool getAlpha(void);
 313   bool getTheta(void);
 314   bool getMachFromVcas(double *Mach,double vcas);
 315
 316   double GammaEqOfTheta(double Theta);
 317   double GammaEqOfAlpha(double Alpha);
 318   double calcVcas(double Mach);
 319   void calcUVWfromNED(void);
 320   void calcWindUVW(void);
 321
 322   bool findInterval(double x,double guess);
 323   bool solve(double *y, double x);
 324   void Debug(int from);
 325 };
 326 }
 327 #endif
 328