Author: Jon S. Berndt
Date started: 1/5/99
- ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
+ ------------- 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
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.53 2010/11/28 13:02:43 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.
+
+ The general execution of this model follows this process:
+
+ -Calculate the angular accelerations
+ -Calculate the translational accelerations
+ -Calculate the angular rate
+ -Calculate the translational velocity
+
+ -Integrate accelerations and rates
+
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
2: Trapezoidal
3: Adams Bashforth 2
4: Adams Bashforth 3
+ 5: Adams Bashforth 4
@endcode
@author Jon S. Berndt, Mathias Froehlich
- @version $Id$
+ @version $Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 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 angular velocity vector for the vehicle body frame relative to the
+ ECI frame, expressed in the ECI frame.
+ units rad/sec */
+ FGColumnVector3 vPQRi_i;
+
/** 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.
~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};
+
+ /// These define the indices use to select the gravitation models.
+ enum eGravType {gtStandard, gtWGS84};
/** Initializes the FGPropagate class after instantiation and prior to first execution.
The base class FGModel::InitModel is called first, initializing pointers to the
@return false if no error */
bool Run(void);
+ const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
+
/** 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
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
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
/** 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; }
/** 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);}
+ double GetPQRi(int axis) const {return VState.vPQRi(axis);}
/** Retrieves a body frame angular acceleration component.
Retrieves a body frame angular acceleration component. The angular
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).
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 VState.qAttitudeLocal.GetT(); }
/** 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 VState.qAttitudeLocal.GetTInv(); }
/** 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 VState.qAttitudeECI.GetT(); }
+
+ /** Retrieves the body-to-ECI transformation matrix.
+ @return a reference to the body-to-ECI matrix. */
+ const FGMatrix33& GetTb2i(void) const { return VState.qAttitudeECI.GetTInv(); }
+
/** Retrieves the ECEF-to-ECI transformation matrix.
@return a reference to the ECEF-to-ECI transformation matrix. */
const FGMatrix33& GetTec2i(void);
@return a reference to the local-to-ECEF matrix. */
const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
+ /** Retrieves the local-to-inertial transformation matrix.
+ @return a reference to the local-to-inertial transformation matrix. */
+ const FGMatrix33& GetTl2i(void) { return VState.vLocation.GetTl2i(); }
+
+ /** Retrieves the inertial-to-local transformation matrix.
+ @return a reference to the inertial-to-local matrix. */
+ const FGMatrix33& GetTi2l(void) { return VState.vLocation.GetTi2l(); }
+
VehicleState* GetVState(void) { return &VState; }
void SetVState(VehicleState* vstate) {
VState.vLocation = vstate->vLocation;
+ UpdateLocationMatrices();
+ SetInertialOrientation(vstate->qAttitudeECI);
+ VehicleRadius = GetRadius();
VState.vUVW = vstate->vUVW;
+ vVel = GetTb2l() * VState.vUVW;
VState.vPQR = vstate->vPQR;
- VState.vQtrn = vstate->vQtrn; // ... mmh
+ VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
+ VState.vInertialPosition = vstate->vInertialPosition;
+
+ InitializeDerivatives();
}
- const FGQuaternion GetQuaternion(void) const { return VState.vQtrn; }
+ void InitializeDerivatives(void);
+
+ void SetInertialOrientation(FGQuaternion Qi);
+ void SetInertialVelocity(FGColumnVector3 Vi);
+ void SetInertialRates(FGColumnVector3 vRates);
+
+ const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
void SetPQR(unsigned int i, double val) {
if ((i>=1) && (i<=3) )
// 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 SetLongitude(double lon) {
+ VState.vLocation.SetLongitude(lon);
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ UpdateLocationMatrices();
+ }
+ void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
+ void SetLatitude(double lat) {
+ VState.vLocation.SetLatitude(lat);
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ UpdateLocationMatrices();
+ }
+ void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
+ void SetRadius(double r) {
+ VState.vLocation.SetRadius(r);
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ UpdateLocationMatrices();
+ }
void SetAltitudeASL(double altASL);
void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
void SetTerrainElevation(double tt);
void SetDistanceAGL(double tt);
void SetInitialState(const FGInitialCondition *);
+ void SetPosition(const double Lon, const double Lat, const double Radius) {
+ VState.vLocation.SetPosition(Lon, Lat, Radius);
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ VehicleRadius = GetRadius();
+ UpdateLocationMatrices();
+ }
+ void SetLocation(const FGLocation& l) {
+ VState.vLocation = l;
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ UpdateLocationMatrices();
+ }
+ void SetLocation(const FGColumnVector3& l) {
+ VState.vLocation = l;
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ UpdateLocationMatrices();
+ }
+
void RecomputeLocalTerrainRadius(void);
- void CalculatePQRdot(void);
- void CalculateQuatdot(void);
- void CalculateLocationdot(void);
- void CalculateUVWdot(void);
+ void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
+ vDeltaXYZEC = GetTb2ec()*deltaLoc;
+ VState.vLocation -= vDeltaXYZEC;
+ }
+
+ struct LagrangeMultiplier {
+ FGColumnVector3 ForceJacobian;
+ FGColumnVector3 MomentJacobian;
+ double Min;
+ double Max;
+ double value;
+ };
+
+ void DumpState(void);
private:
struct VehicleState VState;
FGColumnVector3 vVel;
+ FGColumnVector3 vPQRdot;
+ FGColumnVector3 vPQRidot;
+ FGColumnVector3 vUVWdot, vUVWidot;
FGColumnVector3 vInertialVelocity;
- FGColumnVector3 vPQRdot, last_vPQRdot, last2_vPQRdot;
- FGColumnVector3 vUVWdot, last_vUVWdot, last2_vUVWdot;
- FGColumnVector3 vLocationDot, last_vLocationDot, last2_vLocationDot;
FGColumnVector3 vLocation;
- 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;
+ FGColumnVector3 vDeltaXYZEC;
+ FGColumnVector3 vGravAccel;
+ FGColumnVector3 vOmegaEarth; // The Earth angular velocity vector
+ FGQuaternion 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
+ FGMatrix33 Ti2l;
+ FGMatrix33 Tl2i;
double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
- double radInv;
- int integrator_rotational_rate;
- int integrator_translational_rate;
- int integrator_rotational_position;
- int integrator_translational_position;
+ FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
+ eIntegrateType integrator_rotational_rate;
+ eIntegrateType integrator_translational_rate;
+ eIntegrateType integrator_rotational_position;
+ eIntegrateType integrator_translational_position;
+ int gravType;
+
+ void CalculatePQRdot(void);
+ void CalculateQuatdot(void);
+ void CalculateInertialVelocity(void);
+ void CalculateUVW(void);
+ void CalculateUVWdot(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 EvaluateRateToResistTo(FGColumnVector3& vdot,
+ const FGColumnVector3& Val,
+ const FGColumnVector3& ValDot,
+ const FGColumnVector3& LocalTerrainVal,
+ deque <FGColumnVector3>& dqValDot,
+ const double dt,
+ const eIntegrateType integration_type);
+
+ void ResolveFrictionForces(double dt);
+
+ void UpdateLocationMatrices(void);
+ void UpdateBodyMatrices(void);
void bind(void);
void Debug(int from);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-#include <initialization/FGInitialCondition.h>
+#include "initialization/FGInitialCondition.h"
#endif
projects / flightgear.git / blobdiff