650 lines
28 KiB
C++
650 lines
28 KiB
C++
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
Header: FGPropagate.h
|
|
Author: Jon S. Berndt
|
|
Date started: 1/5/99
|
|
|
|
------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) -------------
|
|
|
|
This program is free software; you can redistribute it and/or modify it under
|
|
the terms of the GNU Lesser General Public License as published by the Free Software
|
|
Foundation; either version 2 of the License, or (at your option) any later
|
|
version.
|
|
|
|
This program is distributed in the hope that it will be useful, but WITHOUT
|
|
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
|
|
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
|
|
details.
|
|
|
|
You should have received a copy of the GNU Lesser General Public License along with
|
|
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
|
|
Place - Suite 330, Boston, MA 02111-1307, USA.
|
|
|
|
Further information about the GNU Lesser General Public License can also be found on
|
|
the world wide web at http://www.gnu.org.
|
|
|
|
HISTORY
|
|
--------------------------------------------------------------------------------
|
|
01/05/99 JSB Created
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
SENTRY
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
|
|
|
#ifndef FGPROPAGATE_H
|
|
#define FGPROPAGATE_H
|
|
|
|
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
INCLUDES
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
|
|
|
#include "models/FGModel.h"
|
|
#include "math/FGColumnVector3.h"
|
|
#include "math/FGLocation.h"
|
|
#include "math/FGQuaternion.h"
|
|
#include "math/FGMatrix33.h"
|
|
#include <deque>
|
|
|
|
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
DEFINITIONS
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
|
|
|
#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.80 2013/12/22 17:08:59 jberndt Exp $"
|
|
|
|
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
FORWARD DECLARATIONS
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
|
|
|
namespace JSBSim {
|
|
|
|
using std::deque;
|
|
class FGInitialCondition;
|
|
|
|
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
CLASS DOCUMENTATION
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
|
|
|
/** Models the EOM and integration/propagation of state.
|
|
The Equations of Motion (EOM) for JSBSim are integrated to propagate the
|
|
state of the vehicle given the forces and moments that act on it. The
|
|
integration accounts for a rotating Earth.
|
|
|
|
Integration of rotational and translation position and rate can be
|
|
customized as needed or frozen by the selection of no integrator. The
|
|
selection of which integrator to use is done through the setting of
|
|
the associated property. There are four properties which can be set:
|
|
|
|
@code
|
|
simulation/integrator/rate/rotational
|
|
simulation/integrator/rate/translational
|
|
simulation/integrator/position/rotational
|
|
simulation/integrator/position/translational
|
|
@endcode
|
|
|
|
Each of the integrators listed above can be set to one of the following values:
|
|
|
|
@code
|
|
0: No integrator (Freeze)
|
|
1: Rectangular Euler
|
|
2: Trapezoidal
|
|
3: Adams Bashforth 2
|
|
4: Adams Bashforth 3
|
|
5: Adams Bashforth 4
|
|
@endcode
|
|
|
|
@author Jon S. Berndt, Mathias Froehlich, Bertrand Coconnier
|
|
@version $Id: FGPropagate.h,v 1.80 2013/12/22 17:08:59 jberndt Exp $
|
|
*/
|
|
|
|
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
CLASS DECLARATION
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
|
|
|
class FGPropagate : public FGModel {
|
|
public:
|
|
|
|
/** The current vehicle state vector structure contains the translational and
|
|
angular position, and the translational and angular velocity. */
|
|
struct VehicleState {
|
|
/** Represents the current location of the vehicle in Earth centered Earth
|
|
fixed (ECEF) frame.
|
|
units ft */
|
|
FGLocation vLocation;
|
|
|
|
/** The velocity vector of the vehicle with respect to the ECEF frame,
|
|
expressed in the body system.
|
|
units ft/sec */
|
|
FGColumnVector3 vUVW;
|
|
|
|
/** The angular velocity vector for the vehicle relative to the ECEF frame,
|
|
expressed in the body frame.
|
|
units rad/sec */
|
|
FGColumnVector3 vPQR;
|
|
|
|
/** The angular velocity vector for the vehicle body frame relative to the
|
|
ECI frame, expressed in the body frame.
|
|
units rad/sec */
|
|
FGColumnVector3 vPQRi;
|
|
|
|
/** The current orientation of the vehicle, that is, the orientation of the
|
|
body frame relative to the local, NED frame. */
|
|
FGQuaternion qAttitudeLocal;
|
|
|
|
/** The current orientation of the vehicle, that is, the orientation of the
|
|
body frame relative to the inertial (ECI) frame. */
|
|
FGQuaternion qAttitudeECI;
|
|
|
|
FGColumnVector3 vInertialVelocity;
|
|
|
|
FGColumnVector3 vInertialPosition;
|
|
|
|
deque <FGColumnVector3> dqPQRidot;
|
|
deque <FGColumnVector3> dqUVWidot;
|
|
deque <FGColumnVector3> dqInertialVelocity;
|
|
deque <FGQuaternion> dqQtrndot;
|
|
};
|
|
|
|
/** Constructor.
|
|
The constructor initializes several variables, and sets the initial set
|
|
of integrators to use as follows:
|
|
- integrator, rotational rate = Adams Bashforth 2
|
|
- integrator, translational rate = Adams Bashforth 2
|
|
- integrator, rotational position = Trapezoidal
|
|
- integrator, translational position = Trapezoidal
|
|
@param Executive a pointer to the parent executive object */
|
|
FGPropagate(FGFDMExec* Executive);
|
|
|
|
/// Destructor
|
|
~FGPropagate();
|
|
|
|
/// These define the indices use to select the various integrators.
|
|
enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2,
|
|
eAdamsBashforth3, eAdamsBashforth4, eBuss1, eBuss2, eLocalLinearization};
|
|
|
|
/** Initializes the FGPropagate class after instantiation and prior to first execution.
|
|
The base class FGModel::InitModel is called first, initializing pointers to the
|
|
other FGModel objects (and others). */
|
|
bool InitModel(void);
|
|
|
|
void InitializeDerivatives();
|
|
|
|
/** Runs the state propagation model; called by the Executive
|
|
Can pass in a value indicating if the executive is directing the simulation to Hold.
|
|
@param Holding if true, the executive has been directed to hold the sim from
|
|
advancing time. Some models may ignore this flag, such as the Input
|
|
model, which may need to be active to listen on a socket for the
|
|
"Resume" command to be given.
|
|
@return false if no error */
|
|
bool Run(bool Holding);
|
|
|
|
/** Retrieves the velocity vector.
|
|
The vector returned is represented by an FGColumnVector reference. The vector
|
|
for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
|
|
is 1-based, so that the first element can be retrieved using the "()" operator.
|
|
In other words, vVel(1) is Vnorth. Various convenience enumerators are defined
|
|
in FGJSBBase. The relevant enumerators for the vector returned by this call are,
|
|
eNorth=1, eEast=2, eDown=3.
|
|
units ft/sec
|
|
@return The vehicle velocity vector with respect to the Earth centered frame,
|
|
expressed in Local horizontal frame.
|
|
*/
|
|
const FGColumnVector3& GetVel(void) const { return vVel; }
|
|
|
|
/** Retrieves the body frame vehicle velocity vector.
|
|
The vector returned is represented by an FGColumnVector reference. The vector
|
|
for the velocity in Body frame is organized (Vx, Vy, Vz). The vector
|
|
is 1-based, so that the first element can be retrieved using the "()" operator.
|
|
In other words, vUVW(1) is Vx. Various convenience enumerators are defined
|
|
in FGJSBBase. The relevant enumerators for the vector returned by this call are,
|
|
eX=1, eY=2, eZ=3.
|
|
units ft/sec
|
|
@return The body frame vehicle velocity vector in ft/sec.
|
|
*/
|
|
const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
|
|
|
|
/** Retrieves the body angular rates vector, relative to the ECEF frame.
|
|
Retrieves the body angular rates (p, q, r), which are calculated by integration
|
|
of the angular acceleration.
|
|
The vector returned is represented by an FGColumnVector reference. The vector
|
|
for the angular velocity in Body frame is organized (P, Q, R). The vector
|
|
is 1-based, so that the first element can be retrieved using the "()" operator.
|
|
In other words, vPQR(1) is P. Various convenience enumerators are defined
|
|
in FGJSBBase. The relevant enumerators for the vector returned by this call are,
|
|
eP=1, eQ=2, eR=3.
|
|
units rad/sec
|
|
@return The body frame angular rates in rad/sec.
|
|
*/
|
|
const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
|
|
|
|
/** Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
|
|
Retrieves the body angular rates (p, q, r), which are calculated by integration
|
|
of the angular acceleration.
|
|
The vector returned is represented by an FGColumnVector reference. The vector
|
|
for the angular velocity in Body frame is organized (P, Q, R). The vector
|
|
is 1-based, so that the first element can be retrieved using the "()" operator.
|
|
In other words, vPQR(1) is P. Various convenience enumerators are defined
|
|
in FGJSBBase. The relevant enumerators for the vector returned by this call are,
|
|
eP=1, eQ=2, eR=3.
|
|
units rad/sec
|
|
@return The body frame inertial angular rates in rad/sec.
|
|
*/
|
|
const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
|
|
|
|
/** Retrieves the Euler angles that define the vehicle orientation.
|
|
Extracts the Euler angles from the quaternion that stores the orientation
|
|
in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
|
|
vector returned is represented by an FGColumnVector reference. The vector
|
|
for the Euler angles is organized (Phi, Theta, Psi). The vector
|
|
is 1-based, so that the first element can be retrieved using the "()" operator.
|
|
In other words, the returned vector item with subscript (1) is Phi.
|
|
Various convenience enumerators are defined in FGJSBBase. The relevant
|
|
enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
|
|
units radians
|
|
@return The Euler angle vector, where the first item in the
|
|
vector is the angle about the X axis, the second is the
|
|
angle about the Y axis, and the third item is the angle
|
|
about the Z axis (Phi, Theta, Psi).
|
|
*/
|
|
const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
|
|
|
|
/** Retrieves the Euler angles (in degrees) that define the vehicle orientation.
|
|
Extracts the Euler angles from the quaternion that stores the orientation
|
|
in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
|
|
vector returned is represented by an FGColumnVector reference. The vector
|
|
for the Euler angles is organized (Phi, Theta, Psi). The vector
|
|
is 1-based, so that the first element can be retrieved using the "()" operator.
|
|
In other words, the returned vector item with subscript (1) is Phi.
|
|
Various convenience enumerators are defined in FGJSBBase. The relevant
|
|
enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
|
|
units degrees
|
|
@return The Euler angle vector, where the first item in the
|
|
vector is the angle about the X axis, the second is the
|
|
angle about the Y axis, and the third item is the angle
|
|
about the Z axis (Phi, Theta, Psi).
|
|
*/
|
|
FGColumnVector3 GetEulerDeg(void) const;
|
|
|
|
/** Retrieves a body frame velocity component.
|
|
Retrieves a body frame velocity component. The velocity returned is
|
|
extracted from the vUVW vector (an FGColumnVector). The vector for the
|
|
velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based.
|
|
In other words, GetUVW(1) returns Vx. Various convenience enumerators
|
|
are defined in FGJSBBase. The relevant enumerators for the velocity
|
|
returned by this call are, eX=1, eY=2, eZ=3.
|
|
units ft/sec
|
|
@param idx the index of the velocity component desired (1-based).
|
|
@return The body frame velocity component.
|
|
*/
|
|
double GetUVW(int idx) const { return VState.vUVW(idx); }
|
|
|
|
/** Retrieves a Local frame velocity component.
|
|
Retrieves a Local frame velocity component. The velocity returned is
|
|
extracted from the vVel vector (an FGColumnVector). The vector for the
|
|
velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
|
|
is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience
|
|
enumerators are defined in FGJSBBase. The relevant enumerators for the
|
|
velocity returned by this call are, eNorth=1, eEast=2, eDown=3.
|
|
units ft/sec
|
|
@param idx the index of the velocity component desired (1-based).
|
|
@return The body frame velocity component.
|
|
*/
|
|
double GetVel(int idx) const { return vVel(idx); }
|
|
|
|
/** Retrieves the total inertial velocity in ft/sec.
|
|
*/
|
|
double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
|
|
|
|
/** Retrieves the total local NED velocity in ft/sec.
|
|
*/
|
|
double GetNEDVelocityMagnitude(void) const { return VState.vUVW.Magnitude(); }
|
|
|
|
/** Retrieves the inertial velocity vector in ft/sec.
|
|
*/
|
|
const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
|
|
double GetInertialVelocity(int i) const { return VState.vInertialVelocity(i); }
|
|
|
|
/** Retrieves the inertial position vector.
|
|
*/
|
|
const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
|
|
double GetInertialPosition(int i) const { return VState.vInertialPosition(i); }
|
|
|
|
/** Calculates and retrieves the velocity vector relative to the earth centered earth fixed (ECEF) frame.
|
|
*/
|
|
FGColumnVector3 GetECEFVelocity(void) const {return Tb2ec * VState.vUVW; }
|
|
|
|
/** Returns the current altitude above sea level.
|
|
This function returns the altitude above sea level.
|
|
units ft
|
|
@return The current altitude above sea level in feet.
|
|
*/
|
|
double GetAltitudeASL(void) const { return VState.vLocation.GetAltitudeASL(); }
|
|
|
|
/** Returns the current altitude above sea level.
|
|
This function returns the altitude above sea level.
|
|
units meters
|
|
@return The current altitude above sea level in meters.
|
|
*/
|
|
double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
|
|
|
|
/** Retrieves a body frame angular velocity component relative to the ECEF frame.
|
|
Retrieves a body frame angular velocity component. The angular velocity
|
|
returned is extracted from the vPQR vector (an FGColumnVector). The vector
|
|
for the angular velocity in Body frame is organized (P, Q, R). The vector
|
|
is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
|
|
convenience enumerators are defined in FGJSBBase. The relevant enumerators
|
|
for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
|
|
units rad/sec
|
|
@param axis the index of the angular velocity component desired (1-based).
|
|
@return The body frame angular velocity component.
|
|
*/
|
|
double GetPQR(int axis) const {return VState.vPQR(axis);}
|
|
|
|
/** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame.
|
|
Retrieves a body frame angular velocity component. The angular velocity
|
|
returned is extracted from the vPQR vector (an FGColumnVector). The vector
|
|
for the angular velocity in Body frame is organized (P, Q, R). The vector
|
|
is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
|
|
convenience enumerators are defined in FGJSBBase. The relevant enumerators
|
|
for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
|
|
units rad/sec
|
|
@param axis the index of the angular velocity component desired (1-based).
|
|
@return The body frame angular velocity component.
|
|
*/
|
|
double GetPQRi(int axis) const {return VState.vPQRi(axis);}
|
|
|
|
/** Retrieves a vehicle Euler angle component.
|
|
Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
|
|
stores the vehicle orientation relative to the Local frame. The order of
|
|
rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
|
|
Phi. Various convenience enumerators are defined in FGJSBBase. The
|
|
relevant enumerators for the Euler angle returned by this call are,
|
|
ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
|
|
units radians
|
|
@return An Euler angle.
|
|
*/
|
|
double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
|
|
|
|
/** Retrieves a vehicle Euler angle component in degrees.
|
|
Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
|
|
stores the vehicle orientation relative to the Local frame. The order of
|
|
rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
|
|
Phi. Various convenience enumerators are defined in FGJSBBase. The
|
|
relevant enumerators for the Euler angle returned by this call are,
|
|
ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
|
|
units degrees
|
|
@return An Euler angle in degrees.
|
|
*/
|
|
double GetEulerDeg(int axis) const { return VState.qAttitudeLocal.GetEuler(axis) * radtodeg; }
|
|
|
|
/** Retrieves the cosine of a vehicle Euler angle component.
|
|
Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
|
|
quaternion that stores the vehicle orientation relative to the Local frame.
|
|
The order of rotations used is Yaw-Pitch-Roll. The Euler angle
|
|
with subscript (1) is Phi. Various convenience enumerators are defined in
|
|
FGJSBBase. The relevant enumerators for the Euler angle referred to in this
|
|
call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
|
|
units none
|
|
@return The cosine of an Euler angle.
|
|
*/
|
|
double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
|
|
|
|
/** Retrieves the sine of a vehicle Euler angle component.
|
|
Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
|
|
quaternion that stores the vehicle orientation relative to the Local frame.
|
|
The order of rotations used is Yaw-Pitch-Roll. The Euler angle
|
|
with subscript (1) is Phi. Various convenience enumerators are defined in
|
|
FGJSBBase. The relevant enumerators for the Euler angle referred to in this
|
|
call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
|
|
units none
|
|
@return The sine of an Euler angle.
|
|
*/
|
|
double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
|
|
|
|
/** Returns the current altitude rate.
|
|
Returns the current altitude rate (rate of climb).
|
|
units ft/sec
|
|
@return The current rate of change in altitude.
|
|
*/
|
|
double Gethdot(void) const { return -vVel(eDown); }
|
|
|
|
/** Returns the "constant" LocalTerrainRadius.
|
|
The LocalTerrainRadius parameter is set by the calling application or set to
|
|
sea level + terrain elevation if JSBSim is running in standalone mode.
|
|
units feet
|
|
@return distance of the local terrain from the center of the earth.
|
|
*/
|
|
double GetLocalTerrainRadius(void) const;
|
|
|
|
double GetEarthPositionAngle(void) const { return VState.vLocation.GetEPA(); }
|
|
|
|
double GetEarthPositionAngleDeg(void) const { return GetEarthPositionAngle()*radtodeg;}
|
|
|
|
const FGColumnVector3& GetTerrainVelocity(void) const { return LocalTerrainVelocity; }
|
|
const FGColumnVector3& GetTerrainAngularVelocity(void) const { return LocalTerrainAngularVelocity; }
|
|
void RecomputeLocalTerrainVelocity();
|
|
|
|
double GetTerrainElevation(void) const { return GetLocalTerrainRadius() - VState.vLocation.GetSeaLevelRadius(); }
|
|
double GetDistanceAGL(void) const;
|
|
double GetDistanceAGLKm(void) const;
|
|
double GetRadius(void) const {
|
|
if (VState.vLocation.GetRadius() == 0) return 1.0;
|
|
else return VState.vLocation.GetRadius();
|
|
}
|
|
double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
|
|
double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
|
|
|
|
double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
|
|
double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
|
|
|
|
double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
|
|
double GetGeodeticAltitudeKm(void) const { return VState.vLocation.GetGeodAltitude()*0.0003048; }
|
|
|
|
double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
|
|
double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
|
|
const FGLocation& GetLocation(void) const { return VState.vLocation; }
|
|
double GetLocation(int i) const { return VState.vLocation(i); }
|
|
|
|
/** Retrieves the local-to-body transformation matrix.
|
|
The quaternion class, being the means by which the orientation of the
|
|
vehicle is stored, manages the local-to-body transformation matrix.
|
|
@return a reference to the local-to-body transformation matrix. */
|
|
const FGMatrix33& GetTl2b(void) const { return Tl2b; }
|
|
|
|
/** Retrieves the body-to-local transformation matrix.
|
|
The quaternion class, being the means by which the orientation of the
|
|
vehicle is stored, manages the body-to-local transformation matrix.
|
|
@return a reference to the body-to-local matrix. */
|
|
const FGMatrix33& GetTb2l(void) const { return Tb2l; }
|
|
|
|
/** Retrieves the ECEF-to-body transformation matrix.
|
|
@return a reference to the ECEF-to-body transformation matrix. */
|
|
const FGMatrix33& GetTec2b(void) const { return Tec2b; }
|
|
|
|
/** Retrieves the body-to-ECEF transformation matrix.
|
|
@return a reference to the body-to-ECEF matrix. */
|
|
const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
|
|
|
|
/** Retrieves the ECI-to-body transformation matrix.
|
|
@return a reference to the ECI-to-body transformation matrix. */
|
|
const FGMatrix33& GetTi2b(void) const { return Ti2b; }
|
|
|
|
/** Retrieves the body-to-ECI transformation matrix.
|
|
@return a reference to the body-to-ECI matrix. */
|
|
const FGMatrix33& GetTb2i(void) const { return Tb2i; }
|
|
|
|
/** Retrieves the ECEF-to-ECI transformation matrix.
|
|
@return a reference to the ECEF-to-ECI transformation matrix. */
|
|
const FGMatrix33& GetTec2i(void) const { return Tec2i; }
|
|
|
|
/** Retrieves the ECI-to-ECEF transformation matrix.
|
|
@return a reference to the ECI-to-ECEF matrix. */
|
|
const FGMatrix33& GetTi2ec(void) const { return Ti2ec; }
|
|
|
|
/** Retrieves the ECEF-to-local transformation matrix.
|
|
Retrieves the ECEF-to-local transformation matrix. Note that the so-called
|
|
local from is also know as the NED frame (for North, East, Down).
|
|
@return a reference to the ECEF-to-local matrix. */
|
|
const FGMatrix33& GetTec2l(void) const { return Tec2l; }
|
|
|
|
/** Retrieves the local-to-ECEF transformation matrix.
|
|
Retrieves the local-to-ECEF transformation matrix. Note that the so-called
|
|
local from is also know as the NED frame (for North, East, Down).
|
|
@return a reference to the local-to-ECEF matrix. */
|
|
const FGMatrix33& GetTl2ec(void) const { return Tl2ec; }
|
|
|
|
/** Retrieves the local-to-inertial transformation matrix.
|
|
@return a reference to the local-to-inertial transformation matrix. */
|
|
const FGMatrix33& GetTl2i(void) const { return Tl2i; }
|
|
|
|
/** Retrieves the inertial-to-local transformation matrix.
|
|
@return a reference to the inertial-to-local matrix. */
|
|
const FGMatrix33& GetTi2l(void) const { return Ti2l; }
|
|
|
|
const VehicleState& GetVState(void) const { return VState; }
|
|
|
|
void SetVState(const VehicleState& vstate);
|
|
|
|
void SetEarthPositionAngle(double epa) {VState.vLocation.SetEarthPositionAngle(epa);}
|
|
|
|
void SetInertialOrientation(const FGQuaternion& Qi);
|
|
void SetInertialVelocity(const FGColumnVector3& Vi);
|
|
void SetInertialRates(const FGColumnVector3& vRates);
|
|
|
|
/** Returns the quaternion that goes from Local to Body. */
|
|
const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
|
|
|
|
/** Returns the quaternion that goes from ECI to Body. */
|
|
const FGQuaternion GetQuaternionECI(void) const { return VState.qAttitudeECI; }
|
|
|
|
/** Returns the quaternion that goes from ECEF to Body. */
|
|
const FGQuaternion GetQuaternionECEF(void) const { return Qec2b; }
|
|
|
|
void SetPQR(unsigned int i, double val) {
|
|
VState.vPQR(i) = val;
|
|
VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
|
|
}
|
|
|
|
void SetUVW(unsigned int i, double val) {
|
|
VState.vUVW(i) = val;
|
|
CalculateInertialVelocity();
|
|
}
|
|
|
|
// SET functions
|
|
|
|
void SetLongitude(double lon)
|
|
{
|
|
VState.vLocation.SetLongitude(lon);
|
|
UpdateVehicleState();
|
|
}
|
|
void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
|
|
void SetLatitude(double lat)
|
|
{
|
|
VState.vLocation.SetLatitude(lat);
|
|
UpdateVehicleState();
|
|
}
|
|
void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
|
|
void SetRadius(double r)
|
|
{
|
|
VState.vLocation.SetRadius(r);
|
|
VehicleRadius = r;
|
|
VState.vInertialPosition = Tec2i * VState.vLocation;
|
|
}
|
|
|
|
void SetAltitudeASL(double altASL)
|
|
{
|
|
VState.vLocation.SetAltitudeASL(altASL);
|
|
UpdateVehicleState();
|
|
}
|
|
void SetAltitudeASLmeters(double altASL) { SetAltitudeASL(altASL/fttom); }
|
|
|
|
void SetSeaLevelRadius(double tt);
|
|
void SetTerrainElevation(double tt);
|
|
void SetDistanceAGL(double tt);
|
|
void SetDistanceAGLKm(double tt);
|
|
|
|
void SetInitialState(const FGInitialCondition *);
|
|
void SetLocation(const FGLocation& l);
|
|
void SetLocation(const FGColumnVector3& lv)
|
|
{
|
|
FGLocation l = FGLocation(lv);
|
|
SetLocation(l);
|
|
}
|
|
void SetPosition(const double Lon, const double Lat, const double Radius)
|
|
{
|
|
FGLocation l = FGLocation(Lon, Lat, Radius);
|
|
SetLocation(l);
|
|
}
|
|
|
|
void NudgeBodyLocation(const FGColumnVector3& deltaLoc) {
|
|
VState.vInertialPosition -= Tb2i*deltaLoc;
|
|
VState.vLocation -= Tb2ec*deltaLoc;
|
|
}
|
|
|
|
void DumpState(void);
|
|
|
|
struct Inputs {
|
|
FGColumnVector3 vPQRidot;
|
|
FGQuaternion vQtrndot;
|
|
FGColumnVector3 vUVWidot;
|
|
FGColumnVector3 vOmegaPlanet;
|
|
double SemiMajor;
|
|
double SemiMinor;
|
|
double DeltaT;
|
|
} in;
|
|
|
|
private:
|
|
|
|
// state vector
|
|
|
|
struct VehicleState VState;
|
|
|
|
FGColumnVector3 vVel;
|
|
FGMatrix33 Tec2b;
|
|
FGMatrix33 Tb2ec;
|
|
FGMatrix33 Tl2b; // local to body frame matrix copy for immediate local use
|
|
FGMatrix33 Tb2l; // body to local frame matrix copy for immediate local use
|
|
FGMatrix33 Tl2ec; // local to ECEF matrix copy for immediate local use
|
|
FGMatrix33 Tec2l; // ECEF to local frame matrix copy for immediate local use
|
|
FGMatrix33 Tec2i; // ECEF to ECI frame matrix copy for immediate local use
|
|
FGMatrix33 Ti2ec; // ECI to ECEF frame matrix copy for immediate local use
|
|
FGMatrix33 Ti2b; // ECI to body frame rotation matrix
|
|
FGMatrix33 Tb2i; // body to ECI frame rotation matrix
|
|
FGMatrix33 Ti2l;
|
|
FGMatrix33 Tl2i;
|
|
|
|
FGQuaternion Qec2b;
|
|
|
|
double VehicleRadius;
|
|
FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
|
|
|
|
eIntegrateType integrator_rotational_rate;
|
|
eIntegrateType integrator_translational_rate;
|
|
eIntegrateType integrator_rotational_position;
|
|
eIntegrateType integrator_translational_position;
|
|
|
|
void CalculateInertialVelocity(void);
|
|
void CalculateUVW(void);
|
|
|
|
void Integrate( FGColumnVector3& Integrand,
|
|
FGColumnVector3& Val,
|
|
deque <FGColumnVector3>& ValDot,
|
|
double dt,
|
|
eIntegrateType integration_type);
|
|
|
|
void Integrate( FGQuaternion& Integrand,
|
|
FGQuaternion& Val,
|
|
deque <FGQuaternion>& ValDot,
|
|
double dt,
|
|
eIntegrateType integration_type);
|
|
|
|
void UpdateLocationMatrices(void);
|
|
void UpdateBodyMatrices(void);
|
|
void UpdateVehicleState(void);
|
|
|
|
void WriteStateFile(int num);
|
|
void bind(void);
|
|
void Debug(int from);
|
|
};
|
|
}
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
#endif
|