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