INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#include <models/FGModel.h>
-#include <math/FGColumnVector3.h>
-#include <math/FGLocation.h>
-#include <math/FGQuaternion.h>
-#include <math/FGMatrix33.h>
+#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$"
+#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.69 2012/04/29 13:27:51 bcoconni Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
namespace JSBSim {
+using std::deque;
class FGInitialCondition;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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
+
+ 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
+ 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
2: Trapezoidal
3: Adams Bashforth 2
4: Adams Bashforth 3
+ 5: Adams Bashforth 4
@endcode
- @author Jon S. Berndt, Mathias Froehlich
- @version $Id$
+ @author Jon S. Berndt, Mathias Froehlich, Bertrand Coconnier
+ @version $Id: FGPropagate.h,v 1.69 2012/04/29 13:27:51 bcoconni Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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, vehilce-carried, NED frame. */
- FGQuaternion vQtrn;
+ 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.
/// Destructor
~FGPropagate();
-
+
/// These define the indices use to select the various integrators.
- enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3};
+ 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
+ The base class FGModel::InitModel is called first, initializing pointers to the
other FGModel objects (and others). */
bool InitModel(void);
- /** Runs the Propagate model; called by the Executive.
+ 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(void);
+ bool Run(bool Holding);
/** Retrieves the velocity vector.
The vector returned is represented by an FGColumnVector reference. The vector
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
@return The body frame vehicle velocity vector in ft/sec.
*/
const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
-
- /** Retrieves the body axis acceleration.
- Retrieves the computed body axis accelerations based on the
- applied forces and accounting for a rotating body frame.
- The vector returned is represented by an FGColumnVector reference. The vector
- for the acceleration in Body frame is organized (Ax, Ay, Az). The vector
- is 1-based, so that the first element can be retrieved using the "()" operator.
- In other words, vUVWdot(1) is Ax. 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^2
- @return Body axis translational acceleration in ft/sec^2.
- */
- const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; }
-
+
/** 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.
@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.
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.
+ @return The body frame inertial angular rates in rad/sec.
*/
- const FGColumnVector3& GetPQRi(void) const {return vPQRi;}
+ const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
- /** Retrieves the body axis angular acceleration vector.
- Retrieves the body axis angular acceleration vector in rad/sec^2. The
- angular acceleration vector is determined from the applied forces and
- accounts for a rotating frame.
- The vector returned is represented by an FGColumnVector reference. The vector
- for the angular acceleration in Body frame is organized (Pdot, Qdot, Rdot). The vector
- is 1-based, so that the first element can be retrieved using the "()" operator.
- In other words, vPQRdot(1) is Pdot. 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^2
- @return The angular acceleration vector.
- */
- const FGColumnVector3& GetPQRdot(void) const {return vPQRdot;}
-
/** 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
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.vQtrn.GetEuler(); }
+ const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
/** Retrieves a body frame velocity component.
Retrieves a body frame velocity component. The velocity returned is
@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 body frame acceleration component.
- Retrieves a body frame acceleration component. The acceleration returned
- is extracted from the vUVWdot vector (an FGColumnVector). The vector for
- the acceleration in Body frame is organized (Ax, Ay, Az). The vector is
- 1-based. In other words, GetUVWdot(1) returns Ax. Various convenience
- enumerators are defined in FGJSBBase. The relevant enumerators for the
- acceleration returned by this call are, eX=1, eY=2, eZ=3.
- units ft/sec^2
- @param idx the index of the acceleration component desired (1-based).
- @return The body frame acceleration component.
- */
- double GetUVWdot(int idx) const { return vUVWdot(idx); }
+ 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
/** Retrieves the total inertial velocity in ft/sec.
*/
- double GetInertialVelocityMagnitude(void) const { return vInertialVelocity.Magnitude(); }
+ double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
+
+ /** Retrieves the inertial velocity vector in ft/sec.
+ */
+ const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
+
+ /** Retrieves the inertial position vector.
+ */
+ const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
+
+ /** 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.GetRadius() - SeaLevelRadius; }
+ double GetAltitudeASL(void) const { return VState.vLocation.GetAltitudeASL(); }
/** Returns the current altitude above sea level.
This function returns the altitude above sea level.
@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 vPQRi(axis);}
-
- /** Retrieves a body frame angular acceleration component.
- Retrieves a body frame angular acceleration component. The angular
- acceleration returned is extracted from the vPQRdot vector (an
- FGColumnVector). The vector for the angular acceleration in Body frame
- is organized (Pdot, Qdot, Rdot). The vector is 1-based. In other words,
- GetPQRdot(1) returns Pdot (roll acceleration). Various convenience
- enumerators are defined in FGJSBBase. The relevant enumerators for the
- angular acceleration returned by this call are, eP=1, eQ=2, eR=3.
- units rad/sec^2
- @param axis the index of the angular acceleration component desired (1-based).
- @return The body frame angular acceleration component.
- */
- double GetPQRdot(int axis) const {return vPQRdot(axis);}
+ 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
units radians
@return An Euler angle.
*/
- double GetEuler(int axis) const { return VState.vQtrn.GetEuler(axis); }
+ double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
/** Retrieves the cosine of a vehicle Euler angle component.
Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
units none
@return The cosine of an Euler angle.
*/
- double GetCosEuler(int idx) const { return VState.vQtrn.GetCosEuler(idx); }
+ 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
units none
@return The sine of an Euler angle.
*/
- double GetSinEuler(int idx) const { return VState.vQtrn.GetSinEuler(idx); }
+ double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
/** Returns the current altitude rate.
Returns the current altitude rate (rate of climb).
*/
double GetLocalTerrainRadius(void) const;
- double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
- double GetTerrainElevation(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 GetRadius(void) const {
if (VState.vLocation.GetRadius() == 0) return 1.0;
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 VState.vQtrn.GetT(); }
+ 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 VState.vQtrn.GetTInv(); }
+ const FGMatrix33& GetTb2l(void) const { return Tb2l; }
/** Retrieves the ECEF-to-body transformation matrix.
@return a reference to the ECEF-to-body 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 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 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 VState.vLocation.GetTec2l(); }
+ 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 VState.vLocation.GetTl2ec(); }
+ const FGMatrix33& GetTl2ec(void) const { return Tl2ec; }
- VehicleState* GetVState(void) { return &VState; }
+ /** Retrieves the local-to-inertial transformation matrix.
+ @return a reference to the local-to-inertial transformation matrix. */
+ const FGMatrix33& GetTl2i(void) const { return Tl2i; }
- void SetVState(VehicleState* vstate) {
- VState.vLocation = vstate->vLocation;
- VState.vUVW = vstate->vUVW;
- VState.vPQR = vstate->vPQR;
- VState.vQtrn = vstate->vQtrn; // ... mmh
- }
+ /** 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; }
- const FGQuaternion GetQuaternion(void) const { return VState.vQtrn; }
+ 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);
+
+ const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
+ const FGQuaternion GetQuaternionECI(void) const { return VState.qAttitudeECI; }
void SetPQR(unsigned int i, double val) {
- if ((i>=1) && (i<=3) )
- VState.vPQR(i) = val;
+ VState.vPQR(i) = val;
+ VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
}
void SetUVW(unsigned int i, double val) {
- if ((i>=1) && (i<=3) )
- VState.vUVW(i) = val;
+ VState.vUVW(i) = val;
+ CalculateInertialVelocity();
}
// SET functions
- void SetLongitude(double lon) { VState.vLocation.SetLongitude(lon); }
- void SetLongitudeDeg(double lon) {SetLongitude(lon*degtorad);}
- void SetLatitude(double lat) { VState.vLocation.SetLatitude(lat); }
- void SetLatitudeDeg(double lat) {SetLatitude(lat*degtorad);}
- void SetRadius(double r) { VState.vLocation.SetRadius(r); }
- void SetLocation(const FGLocation& l) { VState.vLocation = l; }
- void SetAltitudeASL(double altASL);
- void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
- void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
+ 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 SetInitialState(const FGInitialCondition *);
- void RecomputeLocalTerrainRadius(void);
+ 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(FGColumnVector3 deltaLoc) {
- vDeltaXYZEC = GetTb2ec()*deltaLoc;
- VState.vLocation -= vDeltaXYZEC;
+ void NudgeBodyLocation(const FGColumnVector3& deltaLoc) {
+ VState.vInertialPosition -= Tb2i*deltaLoc;
+ VState.vLocation -= Tb2ec*deltaLoc;
}
- void CalculatePQRdot(void);
- void CalculateQuatdot(void);
- void CalculateLocationdot(void);
- void CalculateUVWdot(void);
+ void DumpState(void);
+
+ struct Inputs {
+ FGColumnVector3 vPQRidot;
+ FGQuaternion vQtrndot;
+ FGColumnVector3 vUVWidot;
+ FGColumnVector3 vOmegaPlanet;
+ double RefRadius;
+ double SemiMajor;
+ double SemiMinor;
+ double DeltaT;
+ } in;
private:
FGColumnVector3 vVel;
FGColumnVector3 vInertialVelocity;
- FGColumnVector3 vPQRdot, last_vPQRdot, last2_vPQRdot;
- FGColumnVector3 vUVWdot, last_vUVWdot, last2_vUVWdot;
- FGColumnVector3 vLocationDot, last_vLocationDot, last2_vLocationDot;
FGColumnVector3 vLocation;
- FGColumnVector3 vDeltaXYZEC;
- FGColumnVector3 vPQRi; // Inertial frame angular velocity
- FGColumnVector3 vOmega; // The Earth angular velocity vector
- FGColumnVector3 vOmegaLocal; // The local frame angular velocity vector
- FGQuaternion vQtrndot, last_vQtrndot, last2_vQtrndot;
FGMatrix33 Tec2b;
FGMatrix33 Tb2ec;
FGMatrix33 Tl2b; // local to body 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
-
- double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
- double radInv;
- int integrator_rotational_rate;
- int integrator_translational_rate;
- int integrator_rotational_position;
- int integrator_translational_position;
+ FGMatrix33 Ti2l;
+ FGMatrix33 Tl2i;
+
+ 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 bind(void);
void Debug(int from);
};
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-#include <initialization/FGInitialCondition.h>
-
#endif
projects / flightgear.git / blobdiff