src/FDM/JSBSim/FGState.h

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Header:       FGState.h
   4  Author:       Jon S. Berndt
   5  Date started: 11/17/98
   6
   7  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
   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 FUNCTIONAL DESCRIPTION
  27 --------------------------------------------------------------------------------
  28
  29 HISTORY
  30 --------------------------------------------------------------------------------
  31 11/17/98   JSB   Created
  32
  33 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  34 SENTRY
  35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  36
  37 #ifndef FGSTATE_H
  38 #define FGSTATE_H
  39
  40 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  41 INCLUDES
  42 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  43
  44 #ifdef FGFS
  45 #  include <simgear/compiler.h>
  46 #  ifdef SG_HAVE_STD_INCLUDES
  47 #    include <fstream>
  48 #  else
  49 #    include <fstream.h>
  50 #  endif
  51 #else
  52 #  if defined(sgi) && !defined(__GNUC__)
  53 #    include <fstream.h>
  54 #  else
  55 #    include <fstream>
  56 #  endif
  57 #endif
  58
  59 #include <string>
  60 #include <map>
  61 #include "FGJSBBase.h"
  62 #include "FGInitialCondition.h"
  63 #include "FGMatrix33.h"
  64 #include "FGColumnVector3.h"
  65 #include "FGColumnVector4.h"
  66
  67 #include "FGFDMExec.h"
  68 #include "FGAtmosphere.h"
  69 #include "FGFCS.h"
  70 #include "FGTranslation.h"
  71 #include "FGRotation.h"
  72 #include "FGPosition.h"
  73 #include "FGAerodynamics.h"
  74 #include "FGOutput.h"
  75 #include "FGAircraft.h"
  76 #include "FGGroundReactions.h"
  77 #include "FGPropulsion.h"
  78
  79
  80 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  81 DEFINITIONS
  82 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  83
  84 #define ID_STATE "$Id$"
  85
  86 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  87 FORWARD DECLARATIONS
  88 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  89
  90 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  91 COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs]
  92 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  93
  94 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  95 CLASS DOCUMENTATION
  96 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  97
  98 /** Encapsulates the calculation of aircraft state.
  99     @author Jon S. Berndt
 100     @version $Id$
 101     @see <a href="http://cvs.sourceforge.net/cgi-bin/viewcvs.cgi/jsbsim/JSBSim/FGState.h?rev=HEAD&content-type=text/vnd.viewcvs-markup">
 102          Header File </a>
 103     @see <a href="http://cvs.sourceforge.net/cgi-bin/viewcvs.cgi/jsbsim/JSBSim/FGState.cpp?rev=HEAD&content-type=text/vnd.viewcvs-markup">
 104          Source File </a>
 105 */
 106
 107 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 108 CLASS DECLARATION
 109 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 110
 111 class FGState : public FGJSBBase
 112 {
 113 public:
 114   /** Constructor
 115       @param Executive a pointer to the parent executive object */
 116   FGState(FGFDMExec*);
 117   /// Destructor
 118   ~FGState();
 119
 120  /** Initializes the simulation state based on the passed-in parameters.
 121       @param U the body X-Axis velocity in fps.
 122       @param V the body Y-Axis velocity in fps.
 123       @param W the body Z-Axis velocity in fps.
 124       @param lat latitude measured in radians from the equator, negative values are south.
 125       @param lon longitude, measured in radians from the Greenwich meridian, negative values are west.
 126       @param phi the roll angle in radians.
 127       @param tht the pitch angle in radians.
 128       @param psi the heading angle in radians measured clockwise from north.
 129       @param h altitude in feet.
 130       @param wnorth north velocity in feet per second
 131       @param weast eastward velocity in feet per second
 132       @param wdown downward velocity in feet per second
 133       */
 134   void Initialize(double U,
 135                   double V,
 136                   double W,
 137                   double lat,
 138                   double lon,
 139                   double phi,
 140                   double tht,
 141                   double psi,
 142                   double h,
 143                   double wnorth,
 144                   double weast,
 145                   double wdown);
 146
 147   /** Initializes the simulation state based on parameters from an Initial Conditions object.
 148       @param FGIC pointer to an initial conditions object.
 149       @see FGInitialConditions.
 150       */
 151   void Initialize(FGInitialCondition *FGIC);
 152
 153   /// returns the speed of sound in feet per second.
 154   inline double Geta(void) { return a; }
 155
 156   /// Returns the simulation time in seconds.
 157   inline double Getsim_time(void) const { return sim_time; }
 158   /// Returns the simulation delta T.
 159   inline double Getdt(void) { return dt; }
 160
 161   /// Suspends the simulation and sets the delta T to zero.
 162   inline void Suspend(void) {saved_dt = dt; dt = 0.0;}
 163   /// Resumes the simulation by resetting delta T to the correct value.
 164   inline void Resume(void)  {dt = saved_dt;}
 165
 166   /** Sets the speed of sound.
 167       @param speed the speed of sound in feet per second.
 168       */
 169   inline void Seta(double speed) { a = speed; }
 170
 171   /** Sets the current sim time.
 172       @param cur_time the current time
 173       @return the current time.
 174       */
 175   inline double Setsim_time(double cur_time) {
 176     sim_time = cur_time;
 177     return sim_time;
 178   }
 179
 180   /** Sets the integration time step for the simulation executive.
 181       @param delta_t the time step in seconds.
 182       */
 183   inline void  Setdt(double delta_t) { dt = delta_t; }
 184
 185   /** Increments the simulation time.
 186       @return the new simulation time.
 187       */
 188   inline double IncrTime(void) {
 189     sim_time+=dt;
 190     return sim_time;
 191   }
 192
 193   /** Initializes the transformation matrices.
 194       @param phi the roll angle in radians.
 195       @param tht the pitch angle in radians.
 196       @param psi the heading angle in radians
 197       */
 198   void InitMatrices(double phi, double tht, double psi);
 199
 200   /** Calculates the local-to-body and body-to-local conversion matrices.
 201       */
 202   void CalcMatrices(void);
 203
 204   /** Integrates the quaternion.
 205       Given the supplied rotational rate vector and integration rate, the quaternion
 206       is integrated. The quaternion is later used to update the transformation
 207       matrices.
 208       @param vPQR the body rotational rate column vector.
 209       @param rate the integration rate in seconds.
 210       */
 211   void IntegrateQuat(FGColumnVector3 vPQR, int rate);
 212
 213   // ======================================= General Purpose INTEGRATOR
 214
 215   enum iType {AB4, AB3, AB2, AM3, EULER, TRAPZ};
 216
 217   /** Multi-method integrator.
 218       @param type Type of intergation scheme to use. Can be one of:
 219              <ul>
 220              <li>AB4 - Adams-Bashforth, fourth order</li>
 221              <li>AB3 - Adams-Bashforth, third order</li>
 222              <li>AB2 - Adams-Bashforth, second order</li>
 223              <li>AM3 - Adams Moulton, third order</li>
 224              <li>EULER - Euler</li>
 225              <li>TRAPZ - Trapezoidal</li>
 226              </ul>
 227       @param delta_t the integration time step in seconds
 228       @param vTDeriv a reference to the current value of the time derivative of
 229              the quantity being integrated (i.e. if vUVW is being integrated
 230              vTDeriv is the current value of vUVWdot)
 231       @param vLastArray an array of previously calculated and saved values of
 232              the quantity being integrated (i.e. if vUVW is being integrated
 233              vLastArray[0] is the past value of vUVWdot, vLastArray[1] is the value of
 234              vUVWdot prior to that, etc.)
 235       @return the current, incremental value of the item integrated to add to the
 236               previous value. */
 237
 238   template <class T> T Integrate(iType type, double delta_t, T& vTDeriv, T *vLastArray)
 239   {
 240     T vResult;
 241
 242     switch (type) {
 243     case AB4:
 244       vResult = (delta_t/24.0)*(  55.0 * vTDeriv
 245                                 - 59.0 * vLastArray[0]
 246                                 + 37.0 * vLastArray[1]
 247                                 -  9.0 * vLastArray[2] );
 248       vLastArray[2] = vLastArray[1];
 249       vLastArray[1] = vLastArray[0];
 250       vLastArray[0] = vTDeriv;
 251       break;
 252     case AB3:
 253       vResult = (delta_t/12.0)*(  23.0 * vTDeriv
 254                                 - 16.0 * vLastArray[0]
 255                                 +  5.0 * vLastArray[1] );
 256       vLastArray[1] = vLastArray[0];
 257       vLastArray[0] = vTDeriv;
 258       break;
 259     case AB2:
 260       vResult = (delta_t/2.0)*( 3.0 * vTDeriv - vLastArray[0] );
 261       vLastArray[0] = vTDeriv;
 262       break;
 263     case AM3:
 264       vResult = (delta_t/12.0)*(  5.0 * vTDeriv
 265                                 + 8.0 * vLastArray[0]
 266                                 - 1.0 * vLastArray[1] );
 267       vLastArray[1] = vLastArray[0];
 268       vLastArray[0] = vTDeriv;
 269       break;
 270     case EULER:
 271       vResult = delta_t * vTDeriv;
 272       break;
 273     case TRAPZ:
 274       vResult = (delta_t*0.5) * (vTDeriv + vLastArray[0]);
 275       vLastArray[0] = vTDeriv;
 276       break;
 277     }
 278
 279     return vResult;
 280   }
 281
 282   // =======================================
 283
 284   /** Calculates Euler angles from the local-to-body matrix.
 285       @return a reference to the vEuler column vector.
 286       */
 287   FGColumnVector3& CalcEuler(void);
 288
 289   /** Calculates and returns the stability-to-body axis transformation matrix.
 290       @return a reference to the stability-to-body transformation matrix.
 291       */
 292   FGMatrix33& GetTs2b(void);
 293
 294   /** Calculates and returns the body-to-stability axis transformation matrix.
 295       @return a reference to the stability-to-body transformation matrix.
 296       */
 297   FGMatrix33& GetTb2s(void);
 298
 299   /** Retrieves the local-to-body transformation matrix.
 300       @return a reference to the local-to-body transformation matrix.
 301       */
 302   FGMatrix33& GetTl2b(void) { return mTl2b; }
 303
 304   /** Retrieves a specific local-to-body matrix element.
 305       @param r matrix row index.
 306       @param c matrix column index.
 307       @return the matrix element described by the row and column supplied.
 308       */
 309   double GetTl2b(int r, int c) { return mTl2b(r,c);}
 310
 311   /** Retrieves the body-to-local transformation matrix.
 312       @return a reference to the body-to-local matrix.
 313       */
 314   FGMatrix33& GetTb2l(void) { return mTb2l; }
 315
 316   /** Retrieves a specific body-to-local matrix element.
 317       @param r matrix row index.
 318       @param c matrix column index.
 319       @return the matrix element described by the row and column supplied.
 320       */
 321   double GetTb2l(int i, int j) { return mTb2l(i,j);}
 322
 323   /** Prints a summary of simulator state (speed, altitude,
 324       configuration, etc.)
 325   */
 326   void ReportState(void);
 327
 328   void bind();
 329   void unbind();
 330
 331 private:
 332   double a;                          // speed of sound
 333   double sim_time, dt;
 334   double saved_dt;
 335
 336   FGFDMExec* FDMExec;
 337   FGMatrix33 mTb2l;
 338   FGMatrix33 mTl2b;
 339   FGMatrix33 mTs2b;
 340   FGMatrix33 mTb2s;
 341   FGColumnVector4 vQtrn;
 342   FGColumnVector4 vQdot_prev[3];
 343   FGColumnVector4 vQdot;
 344   FGColumnVector3 vUVW;
 345   FGColumnVector3 vLocalVelNED;
 346   FGColumnVector3 vLocalEuler;
 347
 348   FGColumnVector4 vTmp;
 349   FGColumnVector3 vEuler;
 350
 351   FGAircraft* Aircraft;
 352   FGPosition* Position;
 353   FGTranslation* Translation;
 354   FGRotation* Rotation;
 355   FGOutput* Output;
 356   FGAtmosphere* Atmosphere;
 357   FGFCS* FCS;
 358   FGAerodynamics* Aerodynamics;
 359   FGGroundReactions* GroundReactions;
 360   FGPropulsion* Propulsion;
 361   FGPropertyManager* PropertyManager;
 362
 363   void Debug(int from);
 364 };
 365
 366 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 367
 368
 369 #endif
 370