DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 bcoconni Exp $"
+#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.69 2012/04/29 13:27:51 bcoconni Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
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
+ 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
5: Adams Bashforth 4
@endcode
- @author Jon S. Berndt, Mathias Froehlich
- @version $Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 bcoconni Exp $
+ @author Jon S. Berndt, Mathias Froehlich, Bertrand Coconnier
+ @version $Id: FGPropagate.h,v 1.69 2012/04/29 13:27:51 bcoconni Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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, NED frame. */
FGQuaternion qAttitudeLocal;
/// Destructor
~FGPropagate();
-
- /// These define the indices use to select the various integrators.
- enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
- /// These define the indices use to select the gravitation models.
- enum eGravType {gtStandard, gtWGS84};
+ /// 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
+ 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.
- @return false if no error */
- bool Run(void);
+ void InitializeDerivatives();
- const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
+ /** 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
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.
*/
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
@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
*/
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.
*/
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
- 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);}
-
/** 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
*/
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.qAttitudeLocal.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.qAttitudeLocal.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. */
/** 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(); }
+ 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 VState.qAttitudeECI.GetTInv(); }
+ 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; }
/** 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(); }
+ 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) { 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.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
- VState.vInertialPosition = vstate->vInertialPosition;
-
- InitializeDerivatives();
- }
+ const FGMatrix33& GetTi2l(void) const { return Ti2l; }
+
+ const VehicleState& GetVState(void) const { return VState; }
- void InitializeDerivatives(void);
+ void SetVState(const VehicleState& vstate);
- void SetInertialOrientation(FGQuaternion Qi);
- void SetInertialVelocity(FGColumnVector3 Vi);
- void SetInertialRates(FGColumnVector3 vRates);
+ 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);
- VState.vInertialPosition = GetTec2i() * VState.vLocation;
- UpdateLocationMatrices();
+ void SetLongitude(double lon)
+ {
+ VState.vLocation.SetLongitude(lon);
+ UpdateVehicleState();
}
void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
- void SetLatitude(double lat) {
- VState.vLocation.SetLatitude(lat);
- VState.vInertialPosition = GetTec2i() * VState.vLocation;
- UpdateLocationMatrices();
+ void SetLatitude(double lat)
+ {
+ VState.vLocation.SetLatitude(lat);
+ UpdateVehicleState();
}
void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
- void SetRadius(double r) {
- VState.vLocation.SetRadius(r);
- VState.vInertialPosition = GetTec2i() * VState.vLocation;
- UpdateLocationMatrices();
+ 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 SetAltitudeASL(double altASL);
- void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
- void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
+ void SetAltitudeASLmeters(double altASL) { SetAltitudeASL(altASL/fttom); }
+
+ void SetSeaLevelRadius(double 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);
+ void SetLocation(const FGColumnVector3& lv)
+ {
+ FGLocation l = FGLocation(lv);
+ SetLocation(l);
}
- void SetLocation(const FGLocation& l) {
- VState.vLocation = l;
- VState.vInertialPosition = GetTec2i() * VState.vLocation;
- UpdateLocationMatrices();
+ void SetPosition(const double Lon, const double Lat, const double Radius)
+ {
+ FGLocation l = FGLocation(Lon, Lat, Radius);
+ SetLocation(l);
}
- void SetLocation(const FGColumnVector3& l) {
- VState.vLocation = l;
- VState.vInertialPosition = GetTec2i() * VState.vLocation;
- UpdateLocationMatrices();
- }
-
- void RecomputeLocalTerrainRadius(void);
- void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
- vDeltaXYZEC = GetTb2ec()*deltaLoc;
- VState.vLocation -= vDeltaXYZEC;
+ void NudgeBodyLocation(const FGColumnVector3& deltaLoc) {
+ VState.vInertialPosition -= Tb2i*deltaLoc;
+ VState.vLocation -= Tb2ec*deltaLoc;
}
- struct LagrangeMultiplier {
- FGColumnVector3 ForceJacobian;
- FGColumnVector3 MomentJacobian;
- double Min;
- double Max;
- double value;
- };
-
void DumpState(void);
+ struct Inputs {
+ FGColumnVector3 vPQRidot;
+ FGQuaternion vQtrndot;
+ FGColumnVector3 vUVWidot;
+ FGColumnVector3 vOmegaPlanet;
+ double RefRadius;
+ double SemiMajor;
+ double SemiMinor;
+ double DeltaT;
+ } in;
+
private:
// state vector
struct VehicleState VState;
FGColumnVector3 vVel;
- FGColumnVector3 vPQRdot;
- FGColumnVector3 vPQRidot;
- FGColumnVector3 vUVWdot, vUVWidot;
FGColumnVector3 vInertialVelocity;
FGColumnVector3 vLocation;
- 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 Tb2i; // body to ECI frame rotation matrix
FGMatrix33 Ti2l;
FGMatrix33 Tl2i;
-
- double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
+
+ double VehicleRadius;
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,
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 UpdateVehicleState(void);
void bind(void);
void Debug(int from);
};
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-#include "initialization/FGInitialCondition.h"
-
#endif
projects / flightgear.git / blobdiff