src/FDM/JSBSim/models/FGPropagate.h

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Header:       FGPropagate.h
   4  Author:       Jon S. Berndt
   5  Date started: 1/5/99
   6
   7  ------------- Copyright (C) 1999  Jon S. Berndt (jon@jsbsim.org) -------------
   8
   9  This program is free software; you can redistribute it and/or modify it under
  10  the terms of the GNU Lesser 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 Lesser General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26 HISTORY
  27 --------------------------------------------------------------------------------
  28 01/05/99   JSB   Created
  29
  30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  31 SENTRY
  32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  33
  34 #ifndef FGPROPAGATE_H
  35 #define FGPROPAGATE_H
  36
  37 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  38 INCLUDES
  39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  40
  41 #include "models/FGModel.h"
  42 #include "math/FGColumnVector3.h"
  43 #include "math/FGLocation.h"
  44 #include "math/FGQuaternion.h"
  45 #include "math/FGMatrix33.h"
  46 #include <deque>
  47
  48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  49 DEFINITIONS
  50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  51
  52 #define ID_PROPAGATE "$Id: FGPropagate.h,v 1.67 2011/11/09 22:07:17 bcoconni Exp $"
  53
  54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  55 FORWARD DECLARATIONS
  56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  57
  58 namespace JSBSim {
  59
  60 using std::deque;
  61 class FGInitialCondition;
  62
  63 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  64 CLASS DOCUMENTATION
  65 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  66
  67 /** Models the EOM and integration/propagation of state.
  68     The Equations of Motion (EOM) for JSBSim are integrated to propagate the
  69     state of the vehicle given the forces and moments that act on it. The
  70     integration accounts for a rotating Earth.
  71
  72     Integration of rotational and translation position and rate can be
  73     customized as needed or frozen by the selection of no integrator. The
  74     selection of which integrator to use is done through the setting of
  75     the associated property. There are four properties which can be set:
  76
  77     @code
  78     simulation/integrator/rate/rotational
  79     simulation/integrator/rate/translational
  80     simulation/integrator/position/rotational
  81     simulation/integrator/position/translational
  82     @endcode
  83
  84     Each of the integrators listed above can be set to one of the following values:
  85
  86     @code
  87     0: No integrator (Freeze)
  88     1: Rectangular Euler
  89     2: Trapezoidal
  90     3: Adams Bashforth 2
  91     4: Adams Bashforth 3
  92     5: Adams Bashforth 4
  93     @endcode
  94
  95     @author Jon S. Berndt, Mathias Froehlich, Bertrand Coconnier
  96     @version $Id: FGPropagate.h,v 1.67 2011/11/09 22:07:17 bcoconni Exp $
  97   */
  98
  99 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 100 CLASS DECLARATION
 101 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 102
 103 class FGPropagate : public FGModel {
 104 public:
 105
 106   /** The current vehicle state vector structure contains the translational and
 107     angular position, and the translational and angular velocity. */
 108   struct VehicleState {
 109     /** Represents the current location of the vehicle in Earth centered Earth
 110         fixed (ECEF) frame.
 111         units ft */
 112     FGLocation vLocation;
 113
 114     /** The velocity vector of the vehicle with respect to the ECEF frame,
 115         expressed in the body system.
 116         units ft/sec */
 117     FGColumnVector3 vUVW;
 118
 119     /** The angular velocity vector for the vehicle relative to the ECEF frame,
 120         expressed in the body frame.
 121         units rad/sec */
 122     FGColumnVector3 vPQR;
 123
 124     /** The angular velocity vector for the vehicle body frame relative to the
 125         ECI frame, expressed in the body frame.
 126         units rad/sec */
 127     FGColumnVector3 vPQRi;
 128
 129     /** The current orientation of the vehicle, that is, the orientation of the
 130         body frame relative to the local, NED frame. */
 131     FGQuaternion qAttitudeLocal;
 132
 133     /** The current orientation of the vehicle, that is, the orientation of the
 134         body frame relative to the inertial (ECI) frame. */
 135     FGQuaternion qAttitudeECI;
 136
 137     FGColumnVector3 vInertialVelocity;
 138
 139     FGColumnVector3 vInertialPosition;
 140
 141     deque <FGColumnVector3> dqPQRidot;
 142     deque <FGColumnVector3> dqUVWidot;
 143     deque <FGColumnVector3> dqInertialVelocity;
 144     deque <FGQuaternion>    dqQtrndot;
 145   };
 146
 147   /** Constructor.
 148       The constructor initializes several variables, and sets the initial set
 149       of integrators to use as follows:
 150       - integrator, rotational rate = Adams Bashforth 2
 151       - integrator, translational rate = Adams Bashforth 2
 152       - integrator, rotational position = Trapezoidal
 153       - integrator, translational position = Trapezoidal
 154       @param Executive a pointer to the parent executive object */
 155   FGPropagate(FGFDMExec* Executive);
 156
 157   /// Destructor
 158   ~FGPropagate();
 159
 160   /// These define the indices use to select the various integrators.
 161   enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
 162
 163   /** Initializes the FGPropagate class after instantiation and prior to first execution.
 164       The base class FGModel::InitModel is called first, initializing pointers to the
 165       other FGModel objects (and others).  */
 166   bool InitModel(void);
 167
 168   void InitializeDerivatives();
 169
 170   /** Runs the state propagation model; called by the Executive
 171       Can pass in a value indicating if the executive is directing the simulation to Hold.
 172       @param Holding if true, the executive has been directed to hold the sim from
 173                      advancing time. Some models may ignore this flag, such as the Input
 174                      model, which may need to be active to listen on a socket for the
 175                      "Resume" command to be given.
 176       @return false if no error */
 177   bool Run(bool Holding);
 178
 179   /** Retrieves the velocity vector.
 180       The vector returned is represented by an FGColumnVector reference. The vector
 181       for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
 182       is 1-based, so that the first element can be retrieved using the "()" operator.
 183       In other words, vVel(1) is Vnorth. Various convenience enumerators are defined
 184       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 185       eNorth=1, eEast=2, eDown=3.
 186       units ft/sec
 187       @return The vehicle velocity vector with respect to the Earth centered frame,
 188               expressed in Local horizontal frame.
 189   */
 190   const FGColumnVector3& GetVel(void) const { return vVel; }
 191
 192   /** Retrieves the body frame vehicle velocity vector.
 193       The vector returned is represented by an FGColumnVector reference. The vector
 194       for the velocity in Body frame is organized (Vx, Vy, Vz). The vector
 195       is 1-based, so that the first element can be retrieved using the "()" operator.
 196       In other words, vUVW(1) is Vx. Various convenience enumerators are defined
 197       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 198       eX=1, eY=2, eZ=3.
 199       units ft/sec
 200       @return The body frame vehicle velocity vector in ft/sec.
 201   */
 202   const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
 203
 204   /** Retrieves the body angular rates vector, relative to the ECEF frame.
 205       Retrieves the body angular rates (p, q, r), which are calculated by integration
 206       of the angular acceleration.
 207       The vector returned is represented by an FGColumnVector reference. The vector
 208       for the angular velocity in Body frame is organized (P, Q, R). The vector
 209       is 1-based, so that the first element can be retrieved using the "()" operator.
 210       In other words, vPQR(1) is P. Various convenience enumerators are defined
 211       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 212       eP=1, eQ=2, eR=3.
 213       units rad/sec
 214       @return The body frame angular rates in rad/sec.
 215   */
 216   const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
 217
 218   /** Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
 219       Retrieves the body angular rates (p, q, r), which are calculated by integration
 220       of the angular acceleration.
 221       The vector returned is represented by an FGColumnVector reference. The vector
 222       for the angular velocity in Body frame is organized (P, Q, R). The vector
 223       is 1-based, so that the first element can be retrieved using the "()" operator.
 224       In other words, vPQR(1) is P. Various convenience enumerators are defined
 225       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 226       eP=1, eQ=2, eR=3.
 227       units rad/sec
 228       @return The body frame inertial angular rates in rad/sec.
 229   */
 230   const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
 231
 232   /** Retrieves the Euler angles that define the vehicle orientation.
 233       Extracts the Euler angles from the quaternion that stores the orientation
 234       in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
 235       vector returned is represented by an FGColumnVector reference. The vector
 236       for the Euler angles is organized (Phi, Theta, Psi). The vector
 237       is 1-based, so that the first element can be retrieved using the "()" operator.
 238       In other words, the returned vector item with subscript (1) is Phi.
 239       Various convenience enumerators are defined in FGJSBBase. The relevant
 240       enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
 241       units radians
 242       @return The Euler angle vector, where the first item in the
 243               vector is the angle about the X axis, the second is the
 244               angle about the Y axis, and the third item is the angle
 245               about the Z axis (Phi, Theta, Psi).
 246   */
 247   const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
 248
 249   /** Retrieves a body frame velocity component.
 250       Retrieves a body frame velocity component. The velocity returned is
 251       extracted from the vUVW vector (an FGColumnVector). The vector for the
 252       velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based.
 253       In other words, GetUVW(1) returns Vx. Various convenience enumerators
 254       are defined in FGJSBBase. The relevant enumerators for the velocity
 255       returned by this call are, eX=1, eY=2, eZ=3.
 256       units ft/sec
 257       @param idx the index of the velocity component desired (1-based).
 258       @return The body frame velocity component.
 259   */
 260   double GetUVW(int idx) const { return VState.vUVW(idx); }
 261
 262   /** Retrieves a Local frame velocity component.
 263       Retrieves a Local frame velocity component. The velocity returned is
 264       extracted from the vVel vector (an FGColumnVector). The vector for the
 265       velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
 266       is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience
 267       enumerators are defined in FGJSBBase. The relevant enumerators for the
 268       velocity returned by this call are, eNorth=1, eEast=2, eDown=3.
 269       units ft/sec
 270       @param idx the index of the velocity component desired (1-based).
 271       @return The body frame velocity component.
 272   */
 273   double GetVel(int idx) const { return vVel(idx); }
 274
 275   /** Retrieves the total inertial velocity in ft/sec.
 276   */
 277   double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
 278
 279   /** Retrieves the inertial velocity vector in ft/sec.
 280   */
 281   const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
 282
 283   /** Retrieves the inertial position vector.
 284   */
 285   const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
 286
 287   /** Calculates and retrieves the velocity vector relative to the earth centered earth fixed (ECEF) frame.
 288   */
 289   FGColumnVector3 GetECEFVelocity(void) const {return Tb2ec * VState.vUVW; }
 290
 291   /** Returns the current altitude above sea level.
 292       This function returns the altitude above sea level.
 293       units ft
 294       @return The current altitude above sea level in feet.
 295   */
 296   double GetAltitudeASL(void) const { return VState.vLocation.GetAltitudeASL(); }
 297
 298   /** Returns the current altitude above sea level.
 299       This function returns the altitude above sea level.
 300       units meters
 301       @return The current altitude above sea level in meters.
 302   */
 303   double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
 304
 305   /** Retrieves a body frame angular velocity component relative to the ECEF frame.
 306       Retrieves a body frame angular velocity component. The angular velocity
 307       returned is extracted from the vPQR vector (an FGColumnVector). The vector
 308       for the angular velocity in Body frame is organized (P, Q, R). The vector
 309       is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
 310       convenience enumerators are defined in FGJSBBase. The relevant enumerators
 311       for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
 312       units rad/sec
 313       @param axis the index of the angular velocity component desired (1-based).
 314       @return The body frame angular velocity component.
 315   */
 316   double GetPQR(int axis) const {return VState.vPQR(axis);}
 317
 318   /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame.
 319       Retrieves a body frame angular velocity component. The angular velocity
 320       returned is extracted from the vPQR vector (an FGColumnVector). The vector
 321       for the angular velocity in Body frame is organized (P, Q, R). The vector
 322       is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
 323       convenience enumerators are defined in FGJSBBase. The relevant enumerators
 324       for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
 325       units rad/sec
 326       @param axis the index of the angular velocity component desired (1-based).
 327       @return The body frame angular velocity component.
 328   */
 329   double GetPQRi(int axis) const {return VState.vPQRi(axis);}
 330
 331   /** Retrieves a vehicle Euler angle component.
 332       Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
 333       stores the vehicle orientation relative to the Local frame. The order of
 334       rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
 335       Phi. Various convenience enumerators are defined in FGJSBBase. The
 336       relevant enumerators for the Euler angle returned by this call are,
 337       ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
 338       units radians
 339       @return An Euler angle.
 340   */
 341   double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
 342
 343   /** Retrieves the cosine of a vehicle Euler angle component.
 344       Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
 345       quaternion that stores the vehicle orientation relative to the Local frame.
 346       The order of rotations used is Yaw-Pitch-Roll. The Euler angle
 347       with subscript (1) is Phi. Various convenience enumerators are defined in
 348       FGJSBBase. The relevant enumerators for the Euler angle referred to in this
 349       call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
 350       units none
 351       @return The cosine of an Euler angle.
 352   */
 353   double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
 354
 355   /** Retrieves the sine of a vehicle Euler angle component.
 356       Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
 357       quaternion that stores the vehicle orientation relative to the Local frame.
 358       The order of rotations used is Yaw-Pitch-Roll. The Euler angle
 359       with subscript (1) is Phi. Various convenience enumerators are defined in
 360       FGJSBBase. The relevant enumerators for the Euler angle referred to in this
 361       call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
 362       units none
 363       @return The sine of an Euler angle.
 364   */
 365   double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
 366
 367   /** Returns the current altitude rate.
 368       Returns the current altitude rate (rate of climb).
 369       units ft/sec
 370       @return The current rate of change in altitude.
 371   */
 372   double Gethdot(void) const { return -vVel(eDown); }
 373
 374   /** Returns the "constant" LocalTerrainRadius.
 375       The LocalTerrainRadius parameter is set by the calling application or set to
 376       sea level + terrain elevation if JSBSim is running in standalone mode.
 377       units feet
 378       @return distance of the local terrain from the center of the earth.
 379       */
 380   double GetLocalTerrainRadius(void) const;
 381
 382   double GetEarthPositionAngle(void) const { return VState.vLocation.GetEPA(); }
 383
 384   double GetEarthPositionAngleDeg(void) const { return GetEarthPositionAngle()*radtodeg;}
 385
 386   const FGColumnVector3& GetTerrainVelocity(void) const { return LocalTerrainVelocity; }
 387   const FGColumnVector3& GetTerrainAngularVelocity(void) const { return LocalTerrainAngularVelocity; }
 388   void RecomputeLocalTerrainVelocity();
 389
 390   double GetTerrainElevation(void) const { return GetLocalTerrainRadius() - VState.vLocation.GetSeaLevelRadius(); }
 391   double GetDistanceAGL(void)  const;
 392   double GetRadius(void) const {
 393       if (VState.vLocation.GetRadius() == 0) return 1.0;
 394       else return VState.vLocation.GetRadius();
 395   }
 396   double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
 397   double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
 398
 399   double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
 400   double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
 401
 402   double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
 403
 404   double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
 405   double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
 406   const FGLocation& GetLocation(void) const { return VState.vLocation; }
 407
 408   /** Retrieves the local-to-body transformation matrix.
 409       The quaternion class, being the means by which the orientation of the
 410       vehicle is stored, manages the local-to-body transformation matrix.
 411       @return a reference to the local-to-body transformation matrix.  */
 412   const FGMatrix33& GetTl2b(void) const { return Tl2b; }
 413
 414   /** Retrieves the body-to-local transformation matrix.
 415       The quaternion class, being the means by which the orientation of the
 416       vehicle is stored, manages the body-to-local transformation matrix.
 417       @return a reference to the body-to-local matrix.  */
 418   const FGMatrix33& GetTb2l(void) const { return Tb2l; }
 419
 420   /** Retrieves the ECEF-to-body transformation matrix.
 421       @return a reference to the ECEF-to-body transformation matrix.  */
 422   const FGMatrix33& GetTec2b(void) const { return Tec2b; }
 423
 424   /** Retrieves the body-to-ECEF transformation matrix.
 425       @return a reference to the body-to-ECEF matrix.  */
 426   const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
 427
 428   /** Retrieves the ECI-to-body transformation matrix.
 429       @return a reference to the ECI-to-body transformation matrix.  */
 430   const FGMatrix33& GetTi2b(void) const { return Ti2b; }
 431
 432   /** Retrieves the body-to-ECI transformation matrix.
 433       @return a reference to the body-to-ECI matrix.  */
 434   const FGMatrix33& GetTb2i(void) const { return Tb2i; }
 435
 436   /** Retrieves the ECEF-to-ECI transformation matrix.
 437       @return a reference to the ECEF-to-ECI transformation matrix.  */
 438   const FGMatrix33& GetTec2i(void) const { return Tec2i; }
 439
 440   /** Retrieves the ECI-to-ECEF transformation matrix.
 441       @return a reference to the ECI-to-ECEF matrix.  */
 442   const FGMatrix33& GetTi2ec(void) const { return Ti2ec; }
 443
 444   /** Retrieves the ECEF-to-local transformation matrix.
 445       Retrieves the ECEF-to-local transformation matrix. Note that the so-called
 446       local from is also know as the NED frame (for North, East, Down).
 447       @return a reference to the ECEF-to-local matrix.  */
 448   const FGMatrix33& GetTec2l(void) const { return Tec2l; }
 449
 450   /** Retrieves the local-to-ECEF transformation matrix.
 451       Retrieves the local-to-ECEF transformation matrix. Note that the so-called
 452       local from is also know as the NED frame (for North, East, Down).
 453       @return a reference to the local-to-ECEF matrix.  */
 454   const FGMatrix33& GetTl2ec(void) const { return Tl2ec; }
 455
 456   /** Retrieves the local-to-inertial transformation matrix.
 457       @return a reference to the local-to-inertial transformation matrix.  */
 458   const FGMatrix33& GetTl2i(void) const { return Tl2i; }
 459
 460   /** Retrieves the inertial-to-local transformation matrix.
 461       @return a reference to the inertial-to-local matrix.  */
 462   const FGMatrix33& GetTi2l(void) const { return Ti2l; }
 463
 464   const VehicleState& GetVState(void) const { return VState; }
 465
 466   void SetVState(const VehicleState& vstate);
 467
 468   void SetEarthPositionAngle(double epa) {VState.vLocation.SetEarthPositionAngle(epa);}
 469
 470   void SetInertialOrientation(const FGQuaternion& Qi);
 471   void SetInertialVelocity(const FGColumnVector3& Vi);
 472   void SetInertialRates(const FGColumnVector3& vRates);
 473
 474   const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
 475   const FGQuaternion GetQuaternionECI(void) const { return VState.qAttitudeECI; }
 476
 477   void SetPQR(unsigned int i, double val) {
 478     VState.vPQR(i) = val;
 479     VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
 480   }
 481
 482   void SetUVW(unsigned int i, double val) {
 483     VState.vUVW(i) = val;
 484     CalculateInertialVelocity();
 485   }
 486
 487 // SET functions
 488
 489   void SetLongitude(double lon)
 490   {
 491     VState.vLocation.SetLongitude(lon);
 492     UpdateVehicleState();
 493   }
 494   void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
 495   void SetLatitude(double lat)
 496   {
 497     VState.vLocation.SetLatitude(lat);
 498     UpdateVehicleState();
 499   }
 500   void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
 501   void SetRadius(double r)
 502   {
 503     VState.vLocation.SetRadius(r);
 504     VehicleRadius = r;
 505     VState.vInertialPosition = Tec2i * VState.vLocation;
 506   }
 507
 508   void SetAltitudeASL(double altASL)
 509   {
 510     VState.vLocation.SetAltitudeASL(altASL);
 511     UpdateVehicleState();
 512   }
 513   void SetAltitudeASLmeters(double altASL) { SetAltitudeASL(altASL/fttom); }
 514
 515   void SetSeaLevelRadius(double tt);
 516   void SetTerrainElevation(double tt);
 517   void SetDistanceAGL(double tt);
 518
 519   void SetInitialState(const FGInitialCondition *);
 520   void SetLocation(const FGLocation& l);
 521   void SetLocation(const FGColumnVector3& lv)
 522   {
 523       FGLocation l = FGLocation(lv);
 524       SetLocation(l);
 525   }
 526   void SetPosition(const double Lon, const double Lat, const double Radius)
 527   {
 528       FGLocation l = FGLocation(Lon, Lat, Radius);
 529       SetLocation(l);
 530   }
 531
 532   void NudgeBodyLocation(const FGColumnVector3& deltaLoc) {
 533     VState.vInertialPosition -= Tb2i*deltaLoc;
 534     VState.vLocation -= Tb2ec*deltaLoc;
 535   }
 536
 537   void DumpState(void);
 538
 539   struct Inputs {
 540     FGColumnVector3 vPQRidot;
 541     FGQuaternion vQtrndot;
 542     FGColumnVector3 vUVWidot;
 543     FGColumnVector3 vOmegaPlanet;
 544     double RefRadius;
 545     double SemiMajor;
 546     double SemiMinor;
 547     double DeltaT;
 548   } in;
 549
 550 private:
 551
 552 // state vector
 553
 554   struct VehicleState VState;
 555
 556   FGColumnVector3 vVel;
 557   FGColumnVector3 vInertialVelocity;
 558   FGColumnVector3 vLocation;
 559   FGColumnVector3 vDeltaXYZEC;
 560   FGMatrix33 Tec2b;
 561   FGMatrix33 Tb2ec;
 562   FGMatrix33 Tl2b;   // local to body frame matrix copy for immediate local use
 563   FGMatrix33 Tb2l;   // body to local frame matrix copy for immediate local use
 564   FGMatrix33 Tl2ec;  // local to ECEF matrix copy for immediate local use
 565   FGMatrix33 Tec2l;  // ECEF to local frame matrix copy for immediate local use
 566   FGMatrix33 Tec2i;  // ECEF to ECI frame matrix copy for immediate local use
 567   FGMatrix33 Ti2ec;  // ECI to ECEF frame matrix copy for immediate local use
 568   FGMatrix33 Ti2b;   // ECI to body frame rotation matrix
 569   FGMatrix33 Tb2i;   // body to ECI frame rotation matrix
 570   FGMatrix33 Ti2l;
 571   FGMatrix33 Tl2i;
 572
 573   double VehicleRadius;
 574   FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
 575
 576   eIntegrateType integrator_rotational_rate;
 577   eIntegrateType integrator_translational_rate;
 578   eIntegrateType integrator_rotational_position;
 579   eIntegrateType integrator_translational_position;
 580
 581   void CalculateInertialVelocity(void);
 582   void CalculateUVW(void);
 583
 584   void Integrate( FGColumnVector3& Integrand,
 585                   FGColumnVector3& Val,
 586                   deque <FGColumnVector3>& ValDot,
 587                   double dt,
 588                   eIntegrateType integration_type);
 589
 590   void Integrate( FGQuaternion& Integrand,
 591                   FGQuaternion& Val,
 592                   deque <FGQuaternion>& ValDot,
 593                   double dt,
 594                   eIntegrateType integration_type);
 595
 596   void UpdateLocationMatrices(void);
 597   void UpdateBodyMatrices(void);
 598   void UpdateVehicleState(void);
 599
 600   void bind(void);
 601   void Debug(int from);
 602 };
 603 }
 604 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 605 #endif