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 namespace JSBSim {
 112
 113 class FGState : public FGJSBBase
 114 {
 115 public:
 116   /** Constructor
 117       @param Executive a pointer to the parent executive object */
 118   FGState(FGFDMExec*);
 119   /// Destructor
 120   ~FGState();
 121
 122  /** Initializes the simulation state based on the passed-in parameters.
 123       @param U the body X-Axis velocity in fps.
 124       @param V the body Y-Axis velocity in fps.
 125       @param W the body Z-Axis velocity in fps.
 126       @param lat latitude measured in radians from the equator, negative values are south.
 127       @param lon longitude, measured in radians from the Greenwich meridian, negative values are west.
 128       @param phi the roll angle in radians.
 129       @param tht the pitch angle in radians.
 130       @param psi the heading angle in radians measured clockwise from north.
 131       @param h altitude in feet.
 132       @param wnorth north velocity in feet per second
 133       @param weast eastward velocity in feet per second
 134       @param wdown downward velocity in feet per second
 135       */
 136   void Initialize(double U,
 137                   double V,
 138                   double W,
 139                   double lat,
 140                   double lon,
 141                   double phi,
 142                   double tht,
 143                   double psi,
 144                   double h,
 145                   double wnorth,
 146                   double weast,
 147                   double wdown);
 148
 149   /** Initializes the simulation state based on parameters from an Initial Conditions object.
 150       @param FGIC pointer to an initial conditions object.
 151       @see FGInitialConditions.
 152       */
 153   void Initialize(FGInitialCondition *FGIC);
 154
 155   /// returns the speed of sound in feet per second.
 156   inline double Geta(void) { return a; }
 157
 158   /// Returns the simulation time in seconds.
 159   inline double Getsim_time(void) const { return sim_time; }
 160   /// Returns the simulation delta T.
 161   inline double Getdt(void) { return dt; }
 162
 163   /// Suspends the simulation and sets the delta T to zero.
 164   inline void Suspend(void) {saved_dt = dt; dt = 0.0;}
 165   /// Resumes the simulation by resetting delta T to the correct value.
 166   inline void Resume(void)  {dt = saved_dt;}
 167
 168   /** Sets the speed of sound.
 169       @param speed the speed of sound in feet per second.
 170       */
 171   inline void Seta(double speed) { a = speed; }
 172
 173   /** Sets the current sim time.
 174       @param cur_time the current time
 175       @return the current time.
 176       */
 177   inline double Setsim_time(double cur_time) {
 178     sim_time = cur_time;
 179     return sim_time;
 180   }
 181
 182   /** Sets the integration time step for the simulation executive.
 183       @param delta_t the time step in seconds.
 184       */
 185   inline void  Setdt(double delta_t) { dt = delta_t; }
 186
 187   /** Increments the simulation time.
 188       @return the new simulation time.
 189       */
 190   inline double IncrTime(void) {
 191     sim_time+=dt;
 192     return sim_time;
 193   }
 194
 195   /** Initializes the transformation matrices.
 196       @param phi the roll angle in radians.
 197       @param tht the pitch angle in radians.
 198       @param psi the heading angle in radians
 199       */
 200   void InitMatrices(double phi, double tht, double psi);
 201
 202   /** Calculates the local-to-body and body-to-local conversion matrices.
 203       */
 204   void CalcMatrices(void);
 205
 206   /** Integrates the quaternion.
 207       Given the supplied rotational rate vector and integration rate, the quaternion
 208       is integrated. The quaternion is later used to update the transformation
 209       matrices.
 210       @param vPQR the body rotational rate column vector.
 211       @param rate the integration rate in seconds.
 212       */
 213   void IntegrateQuat(FGColumnVector3 vPQR, int rate);
 214
 215   // ======================================= General Purpose INTEGRATOR
 216
 217   enum iType {AB4, AB3, AB2, AM3, EULER, TRAPZ};
 218
 219   /** Multi-method integrator.
 220       @param type Type of intergation scheme to use. Can be one of:
 221              <ul>
 222              <li>AB4 - Adams-Bashforth, fourth order</li>
 223              <li>AB3 - Adams-Bashforth, third order</li>
 224              <li>AB2 - Adams-Bashforth, second order</li>
 225              <li>AM3 - Adams Moulton, third order</li>
 226              <li>EULER - Euler</li>
 227              <li>TRAPZ - Trapezoidal</li>
 228              </ul>
 229       @param delta_t the integration time step in seconds
 230       @param vTDeriv a reference to the current value of the time derivative of
 231              the quantity being integrated (i.e. if vUVW is being integrated
 232              vTDeriv is the current value of vUVWdot)
 233       @param vLastArray an array of previously calculated and saved values of
 234              the quantity being integrated (i.e. if vUVW is being integrated
 235              vLastArray[0] is the past value of vUVWdot, vLastArray[1] is the value of
 236              vUVWdot prior to that, etc.)
 237       @return the current, incremental value of the item integrated to add to the
 238               previous value. */
 239
 240   template <class T> T Integrate(iType type, double delta_t, T& vTDeriv, T *vLastArray)
 241   {
 242     T vResult;
 243
 244     switch (type) {
 245     case AB4:
 246       vResult = (delta_t/24.0)*(  55.0 * vTDeriv
 247                                 - 59.0 * vLastArray[0]
 248                                 + 37.0 * vLastArray[1]
 249                                 -  9.0 * vLastArray[2] );
 250       vLastArray[2] = vLastArray[1];
 251       vLastArray[1] = vLastArray[0];
 252       vLastArray[0] = vTDeriv;
 253       break;
 254     case AB3:
 255       vResult = (delta_t/12.0)*(  23.0 * vTDeriv
 256                                 - 16.0 * vLastArray[0]
 257                                 +  5.0 * vLastArray[1] );
 258       vLastArray[1] = vLastArray[0];
 259       vLastArray[0] = vTDeriv;
 260       break;
 261     case AB2:
 262       vResult = (delta_t/2.0)*( 3.0 * vTDeriv - vLastArray[0] );
 263       vLastArray[0] = vTDeriv;
 264       break;
 265     case AM3:
 266       vResult = (delta_t/12.0)*(  5.0 * vTDeriv
 267                                 + 8.0 * vLastArray[0]
 268                                 - 1.0 * vLastArray[1] );
 269       vLastArray[1] = vLastArray[0];
 270       vLastArray[0] = vTDeriv;
 271       break;
 272     case EULER:
 273       vResult = delta_t * vTDeriv;
 274       break;
 275     case TRAPZ:
 276       vResult = (delta_t*0.5) * (vTDeriv + vLastArray[0]);
 277       vLastArray[0] = vTDeriv;
 278       break;
 279     }
 280
 281     return vResult;
 282   }
 283
 284   // =======================================
 285
 286   /** Calculates Euler angles from the local-to-body matrix.
 287       @return a reference to the vEuler column vector.
 288       */
 289   FGColumnVector3& CalcEuler(void);
 290
 291   /** Calculates and returns the stability-to-body axis transformation matrix.
 292       @return a reference to the stability-to-body transformation matrix.
 293       */
 294   FGMatrix33& GetTs2b(void);
 295
 296   /** Calculates and returns the body-to-stability axis transformation matrix.
 297       @return a reference to the stability-to-body transformation matrix.
 298       */
 299   FGMatrix33& GetTb2s(void);
 300
 301   /** Retrieves the local-to-body transformation matrix.
 302       @return a reference to the local-to-body transformation matrix.
 303       */
 304   FGMatrix33& GetTl2b(void) { return mTl2b; }
 305
 306   /** Retrieves a specific local-to-body matrix element.
 307       @param r matrix row index.
 308       @param c matrix column index.
 309       @return the matrix element described by the row and column supplied.
 310       */
 311   double GetTl2b(int r, int c) { return mTl2b(r,c);}
 312
 313   /** Retrieves the body-to-local transformation matrix.
 314       @return a reference to the body-to-local matrix.
 315       */
 316   FGMatrix33& GetTb2l(void) { return mTb2l; }
 317
 318   /** Retrieves a specific body-to-local matrix element.
 319       @param r matrix row index.
 320       @param c matrix column index.
 321       @return the matrix element described by the row and column supplied.
 322       */
 323   double GetTb2l(int i, int j) { return mTb2l(i,j);}
 324
 325   /** Prints a summary of simulator state (speed, altitude,
 326       configuration, etc.)
 327   */
 328   void ReportState(void);
 329
 330   void bind();
 331   void unbind();
 332
 333 private:
 334   double a;                          // speed of sound
 335   double sim_time, dt;
 336   double saved_dt;
 337
 338   FGFDMExec* FDMExec;
 339   FGMatrix33 mTb2l;
 340   FGMatrix33 mTl2b;
 341   FGMatrix33 mTs2b;
 342   FGMatrix33 mTb2s;
 343   FGColumnVector4 vQtrn;
 344   FGColumnVector4 vQdot_prev[3];
 345   FGColumnVector4 vQdot;
 346   FGColumnVector3 vUVW;
 347   FGColumnVector3 vLocalVelNED;
 348   FGColumnVector3 vLocalEuler;
 349
 350   FGColumnVector4 vTmp;
 351   FGColumnVector3 vEuler;
 352
 353   FGAircraft* Aircraft;
 354   FGPosition* Position;
 355   FGTranslation* Translation;
 356   FGRotation* Rotation;
 357   FGOutput* Output;
 358   FGAtmosphere* Atmosphere;
 359   FGFCS* FCS;
 360   FGAerodynamics* Aerodynamics;
 361   FGGroundReactions* GroundReactions;
 362   FGPropulsion* Propulsion;
 363   FGPropertyManager* PropertyManager;
 364
 365   void Debug(int from);
 366 };
 367 }
 368 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 369
 370
 371 #endif
 372