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.69 2012/04/29 13:27:51 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.69 2012/04/29 13:27:51 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,
 162                        eAdamsBashforth3, eAdamsBashforth4, eBuss1, eBuss2, eLocalLinearization};
 163
 164   /** Initializes the FGPropagate class after instantiation and prior to first execution.
 165       The base class FGModel::InitModel is called first, initializing pointers to the
 166       other FGModel objects (and others).  */
 167   bool InitModel(void);
 168
 169   void InitializeDerivatives();
 170
 171   /** Runs the state propagation model; called by the Executive
 172       Can pass in a value indicating if the executive is directing the simulation to Hold.
 173       @param Holding if true, the executive has been directed to hold the sim from
 174                      advancing time. Some models may ignore this flag, such as the Input
 175                      model, which may need to be active to listen on a socket for the
 176                      "Resume" command to be given.
 177       @return false if no error */
 178   bool Run(bool Holding);
 179
 180   /** Retrieves the velocity vector.
 181       The vector returned is represented by an FGColumnVector reference. The vector
 182       for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
 183       is 1-based, so that the first element can be retrieved using the "()" operator.
 184       In other words, vVel(1) is Vnorth. Various convenience enumerators are defined
 185       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 186       eNorth=1, eEast=2, eDown=3.
 187       units ft/sec
 188       @return The vehicle velocity vector with respect to the Earth centered frame,
 189               expressed in Local horizontal frame.
 190   */
 191   const FGColumnVector3& GetVel(void) const { return vVel; }
 192
 193   /** Retrieves the body frame vehicle velocity vector.
 194       The vector returned is represented by an FGColumnVector reference. The vector
 195       for the velocity in Body frame is organized (Vx, Vy, Vz). The vector
 196       is 1-based, so that the first element can be retrieved using the "()" operator.
 197       In other words, vUVW(1) is Vx. Various convenience enumerators are defined
 198       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 199       eX=1, eY=2, eZ=3.
 200       units ft/sec
 201       @return The body frame vehicle velocity vector in ft/sec.
 202   */
 203   const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
 204
 205   /** Retrieves the body angular rates vector, relative to the ECEF frame.
 206       Retrieves the body angular rates (p, q, r), which are calculated by integration
 207       of the angular acceleration.
 208       The vector returned is represented by an FGColumnVector reference. The vector
 209       for the angular velocity in Body frame is organized (P, Q, R). The vector
 210       is 1-based, so that the first element can be retrieved using the "()" operator.
 211       In other words, vPQR(1) is P. Various convenience enumerators are defined
 212       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 213       eP=1, eQ=2, eR=3.
 214       units rad/sec
 215       @return The body frame angular rates in rad/sec.
 216   */
 217   const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
 218
 219   /** Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
 220       Retrieves the body angular rates (p, q, r), which are calculated by integration
 221       of the angular acceleration.
 222       The vector returned is represented by an FGColumnVector reference. The vector
 223       for the angular velocity in Body frame is organized (P, Q, R). The vector
 224       is 1-based, so that the first element can be retrieved using the "()" operator.
 225       In other words, vPQR(1) is P. Various convenience enumerators are defined
 226       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 227       eP=1, eQ=2, eR=3.
 228       units rad/sec
 229       @return The body frame inertial angular rates in rad/sec.
 230   */
 231   const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
 232
 233   /** Retrieves the Euler angles that define the vehicle orientation.
 234       Extracts the Euler angles from the quaternion that stores the orientation
 235       in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
 236       vector returned is represented by an FGColumnVector reference. The vector
 237       for the Euler angles is organized (Phi, Theta, Psi). The vector
 238       is 1-based, so that the first element can be retrieved using the "()" operator.
 239       In other words, the returned vector item with subscript (1) is Phi.
 240       Various convenience enumerators are defined in FGJSBBase. The relevant
 241       enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
 242       units radians
 243       @return The Euler angle vector, where the first item in the
 244               vector is the angle about the X axis, the second is the
 245               angle about the Y axis, and the third item is the angle
 246               about the Z axis (Phi, Theta, Psi).
 247   */
 248   const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
 249
 250   /** Retrieves a body frame velocity component.
 251       Retrieves a body frame velocity component. The velocity returned is
 252       extracted from the vUVW vector (an FGColumnVector). The vector for the
 253       velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based.
 254       In other words, GetUVW(1) returns Vx. Various convenience enumerators
 255       are defined in FGJSBBase. The relevant enumerators for the velocity
 256       returned by this call are, eX=1, eY=2, eZ=3.
 257       units ft/sec
 258       @param idx the index of the velocity component desired (1-based).
 259       @return The body frame velocity component.
 260   */
 261   double GetUVW(int idx) const { return VState.vUVW(idx); }
 262
 263   /** Retrieves a Local frame velocity component.
 264       Retrieves a Local frame velocity component. The velocity returned is
 265       extracted from the vVel vector (an FGColumnVector). The vector for the
 266       velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
 267       is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience
 268       enumerators are defined in FGJSBBase. The relevant enumerators for the
 269       velocity returned by this call are, eNorth=1, eEast=2, eDown=3.
 270       units ft/sec
 271       @param idx the index of the velocity component desired (1-based).
 272       @return The body frame velocity component.
 273   */
 274   double GetVel(int idx) const { return vVel(idx); }
 275
 276   /** Retrieves the total inertial velocity in ft/sec.
 277   */
 278   double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
 279
 280   /** Retrieves the inertial velocity vector in ft/sec.
 281   */
 282   const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
 283
 284   /** Retrieves the inertial position vector.
 285   */
 286   const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
 287
 288   /** Calculates and retrieves the velocity vector relative to the earth centered earth fixed (ECEF) frame.
 289   */
 290   FGColumnVector3 GetECEFVelocity(void) const {return Tb2ec * VState.vUVW; }
 291
 292   /** Returns the current altitude above sea level.
 293       This function returns the altitude above sea level.
 294       units ft
 295       @return The current altitude above sea level in feet.
 296   */
 297   double GetAltitudeASL(void) const { return VState.vLocation.GetAltitudeASL(); }
 298
 299   /** Returns the current altitude above sea level.
 300       This function returns the altitude above sea level.
 301       units meters
 302       @return The current altitude above sea level in meters.
 303   */
 304   double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
 305
 306   /** Retrieves a body frame angular velocity component relative to the ECEF frame.
 307       Retrieves a body frame angular velocity component. The angular velocity
 308       returned is extracted from the vPQR vector (an FGColumnVector). The vector
 309       for the angular velocity in Body frame is organized (P, Q, R). The vector
 310       is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
 311       convenience enumerators are defined in FGJSBBase. The relevant enumerators
 312       for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
 313       units rad/sec
 314       @param axis the index of the angular velocity component desired (1-based).
 315       @return The body frame angular velocity component.
 316   */
 317   double GetPQR(int axis) const {return VState.vPQR(axis);}
 318
 319   /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame.
 320       Retrieves a body frame angular velocity component. The angular velocity
 321       returned is extracted from the vPQR vector (an FGColumnVector). The vector
 322       for the angular velocity in Body frame is organized (P, Q, R). The vector
 323       is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
 324       convenience enumerators are defined in FGJSBBase. The relevant enumerators
 325       for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
 326       units rad/sec
 327       @param axis the index of the angular velocity component desired (1-based).
 328       @return The body frame angular velocity component.
 329   */
 330   double GetPQRi(int axis) const {return VState.vPQRi(axis);}
 331
 332   /** Retrieves a vehicle Euler angle component.
 333       Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
 334       stores the vehicle orientation relative to the Local frame. The order of
 335       rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
 336       Phi. Various convenience enumerators are defined in FGJSBBase. The
 337       relevant enumerators for the Euler angle returned by this call are,
 338       ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
 339       units radians
 340       @return An Euler angle.
 341   */
 342   double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
 343
 344   /** Retrieves the cosine of a vehicle Euler angle component.
 345       Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
 346       quaternion that stores the vehicle orientation relative to the Local frame.
 347       The order of rotations used is Yaw-Pitch-Roll. The Euler angle
 348       with subscript (1) is Phi. Various convenience enumerators are defined in
 349       FGJSBBase. The relevant enumerators for the Euler angle referred to in this
 350       call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
 351       units none
 352       @return The cosine of an Euler angle.
 353   */
 354   double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
 355
 356   /** Retrieves the sine of a vehicle Euler angle component.
 357       Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
 358       quaternion that stores the vehicle orientation relative to the Local frame.
 359       The order of rotations used is Yaw-Pitch-Roll. The Euler angle
 360       with subscript (1) is Phi. Various convenience enumerators are defined in
 361       FGJSBBase. The relevant enumerators for the Euler angle referred to in this
 362       call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
 363       units none
 364       @return The sine of an Euler angle.
 365   */
 366   double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
 367
 368   /** Returns the current altitude rate.
 369       Returns the current altitude rate (rate of climb).
 370       units ft/sec
 371       @return The current rate of change in altitude.
 372   */
 373   double Gethdot(void) const { return -vVel(eDown); }
 374
 375   /** Returns the "constant" LocalTerrainRadius.
 376       The LocalTerrainRadius parameter is set by the calling application or set to
 377       sea level + terrain elevation if JSBSim is running in standalone mode.
 378       units feet
 379       @return distance of the local terrain from the center of the earth.
 380       */
 381   double GetLocalTerrainRadius(void) const;
 382
 383   double GetEarthPositionAngle(void) const { return VState.vLocation.GetEPA(); }
 384
 385   double GetEarthPositionAngleDeg(void) const { return GetEarthPositionAngle()*radtodeg;}
 386
 387   const FGColumnVector3& GetTerrainVelocity(void) const { return LocalTerrainVelocity; }
 388   const FGColumnVector3& GetTerrainAngularVelocity(void) const { return LocalTerrainAngularVelocity; }
 389   void RecomputeLocalTerrainVelocity();
 390
 391   double GetTerrainElevation(void) const { return GetLocalTerrainRadius() - VState.vLocation.GetSeaLevelRadius(); }
 392   double GetDistanceAGL(void)  const;
 393   double GetRadius(void) const {
 394       if (VState.vLocation.GetRadius() == 0) return 1.0;
 395       else return VState.vLocation.GetRadius();
 396   }
 397   double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
 398   double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
 399
 400   double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
 401   double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
 402
 403   double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
 404
 405   double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
 406   double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
 407   const FGLocation& GetLocation(void) const { return VState.vLocation; }
 408
 409   /** Retrieves the local-to-body transformation matrix.
 410       The quaternion class, being the means by which the orientation of the
 411       vehicle is stored, manages the local-to-body transformation matrix.
 412       @return a reference to the local-to-body transformation matrix.  */
 413   const FGMatrix33& GetTl2b(void) const { return Tl2b; }
 414
 415   /** Retrieves the body-to-local transformation matrix.
 416       The quaternion class, being the means by which the orientation of the
 417       vehicle is stored, manages the body-to-local transformation matrix.
 418       @return a reference to the body-to-local matrix.  */
 419   const FGMatrix33& GetTb2l(void) const { return Tb2l; }
 420
 421   /** Retrieves the ECEF-to-body transformation matrix.
 422       @return a reference to the ECEF-to-body transformation matrix.  */
 423   const FGMatrix33& GetTec2b(void) const { return Tec2b; }
 424
 425   /** Retrieves the body-to-ECEF transformation matrix.
 426       @return a reference to the body-to-ECEF matrix.  */
 427   const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
 428
 429   /** Retrieves the ECI-to-body transformation matrix.
 430       @return a reference to the ECI-to-body transformation matrix.  */
 431   const FGMatrix33& GetTi2b(void) const { return Ti2b; }
 432
 433   /** Retrieves the body-to-ECI transformation matrix.
 434       @return a reference to the body-to-ECI matrix.  */
 435   const FGMatrix33& GetTb2i(void) const { return Tb2i; }
 436
 437   /** Retrieves the ECEF-to-ECI transformation matrix.
 438       @return a reference to the ECEF-to-ECI transformation matrix.  */
 439   const FGMatrix33& GetTec2i(void) const { return Tec2i; }
 440
 441   /** Retrieves the ECI-to-ECEF transformation matrix.
 442       @return a reference to the ECI-to-ECEF matrix.  */
 443   const FGMatrix33& GetTi2ec(void) const { return Ti2ec; }
 444
 445   /** Retrieves the ECEF-to-local transformation matrix.
 446       Retrieves the ECEF-to-local transformation matrix. Note that the so-called
 447       local from is also know as the NED frame (for North, East, Down).
 448       @return a reference to the ECEF-to-local matrix.  */
 449   const FGMatrix33& GetTec2l(void) const { return Tec2l; }
 450
 451   /** Retrieves the local-to-ECEF transformation matrix.
 452       Retrieves the local-to-ECEF transformation matrix. Note that the so-called
 453       local from is also know as the NED frame (for North, East, Down).
 454       @return a reference to the local-to-ECEF matrix.  */
 455   const FGMatrix33& GetTl2ec(void) const { return Tl2ec; }
 456
 457   /** Retrieves the local-to-inertial transformation matrix.
 458       @return a reference to the local-to-inertial transformation matrix.  */
 459   const FGMatrix33& GetTl2i(void) const { return Tl2i; }
 460
 461   /** Retrieves the inertial-to-local transformation matrix.
 462       @return a reference to the inertial-to-local matrix.  */
 463   const FGMatrix33& GetTi2l(void) const { return Ti2l; }
 464
 465   const VehicleState& GetVState(void) const { return VState; }
 466
 467   void SetVState(const VehicleState& vstate);
 468
 469   void SetEarthPositionAngle(double epa) {VState.vLocation.SetEarthPositionAngle(epa);}
 470
 471   void SetInertialOrientation(const FGQuaternion& Qi);
 472   void SetInertialVelocity(const FGColumnVector3& Vi);
 473   void SetInertialRates(const FGColumnVector3& vRates);
 474
 475   const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
 476   const FGQuaternion GetQuaternionECI(void) const { return VState.qAttitudeECI; }
 477
 478   void SetPQR(unsigned int i, double val) {
 479     VState.vPQR(i) = val;
 480     VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
 481   }
 482
 483   void SetUVW(unsigned int i, double val) {
 484     VState.vUVW(i) = val;
 485     CalculateInertialVelocity();
 486   }
 487
 488 // SET functions
 489
 490   void SetLongitude(double lon)
 491   {
 492     VState.vLocation.SetLongitude(lon);
 493     UpdateVehicleState();
 494   }
 495   void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
 496   void SetLatitude(double lat)
 497   {
 498     VState.vLocation.SetLatitude(lat);
 499     UpdateVehicleState();
 500   }
 501   void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
 502   void SetRadius(double r)
 503   {
 504     VState.vLocation.SetRadius(r);
 505     VehicleRadius = r;
 506     VState.vInertialPosition = Tec2i * VState.vLocation;
 507   }
 508
 509   void SetAltitudeASL(double altASL)
 510   {
 511     VState.vLocation.SetAltitudeASL(altASL);
 512     UpdateVehicleState();
 513   }
 514   void SetAltitudeASLmeters(double altASL) { SetAltitudeASL(altASL/fttom); }
 515
 516   void SetSeaLevelRadius(double tt);
 517   void SetTerrainElevation(double tt);
 518   void SetDistanceAGL(double tt);
 519
 520   void SetInitialState(const FGInitialCondition *);
 521   void SetLocation(const FGLocation& l);
 522   void SetLocation(const FGColumnVector3& lv)
 523   {
 524       FGLocation l = FGLocation(lv);
 525       SetLocation(l);
 526   }
 527   void SetPosition(const double Lon, const double Lat, const double Radius)
 528   {
 529       FGLocation l = FGLocation(Lon, Lat, Radius);
 530       SetLocation(l);
 531   }
 532
 533   void NudgeBodyLocation(const FGColumnVector3& deltaLoc) {
 534     VState.vInertialPosition -= Tb2i*deltaLoc;
 535     VState.vLocation -= Tb2ec*deltaLoc;
 536   }
 537
 538   void DumpState(void);
 539
 540   struct Inputs {
 541     FGColumnVector3 vPQRidot;
 542     FGQuaternion vQtrndot;
 543     FGColumnVector3 vUVWidot;
 544     FGColumnVector3 vOmegaPlanet;
 545     double RefRadius;
 546     double SemiMajor;
 547     double SemiMinor;
 548     double DeltaT;
 549   } in;
 550
 551 private:
 552
 553 // state vector
 554
 555   struct VehicleState VState;
 556
 557   FGColumnVector3 vVel;
 558   FGColumnVector3 vInertialVelocity;
 559   FGColumnVector3 vLocation;
 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