DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.43 2010/07/25 15:35:11 jberndt Exp $"
+#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 bcoconni Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@endcode
@author Jon S. Berndt, Mathias Froehlich
- @version $Id: FGPropagate.h,v 1.43 2010/07/25 15:35:11 jberndt Exp $
+ @version $Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 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;
FGColumnVector3 vInertialPosition;
- deque <FGColumnVector3> dqPQRdot;
- deque <FGColumnVector3> dqUVWdot;
+ deque <FGColumnVector3> dqPQRidot;
+ deque <FGColumnVector3> dqUVWidot;
deque <FGColumnVector3> dqInertialVelocity;
- deque <FGQuaternion> dqQtrndot;
+ deque <FGQuaternion> dqQtrndot;
};
/** Constructor.
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.qAttitudeLocal = vstate->qAttitudeLocal;
- VState.qAttitudeECI = vstate->qAttitudeECI;
+ VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
+ VState.vInertialPosition = vstate->vInertialPosition;
- VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
- VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
- VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
- VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
+ InitializeDerivatives();
}
+ void InitializeDerivatives(void);
+
void SetInertialOrientation(FGQuaternion Qi);
void SetInertialVelocity(FGColumnVector3 Vi);
+ void SetInertialRates(FGColumnVector3 vRates);
const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
// 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 NudgeBodyLocation(FGColumnVector3 deltaLoc) {
double value;
};
+ void DumpState(void);
+
private:
// state vector
FGColumnVector3 vVel;
FGColumnVector3 vPQRdot;
- FGColumnVector3 vUVWdot;
+ FGColumnVector3 vPQRidot;
+ FGColumnVector3 vUVWdot, vUVWidot;
FGColumnVector3 vInertialVelocity;
FGColumnVector3 vLocation;
FGColumnVector3 vDeltaXYZEC;
FGMatrix33 Tb2i; // body to ECI frame rotation matrix
FGMatrix33 Ti2l;
FGMatrix33 Tl2i;
- FGLocation contactloc;
- FGColumnVector3 dv;
double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
- double radInv;
+ FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
eIntegrateType integrator_rotational_rate;
eIntegrateType integrator_translational_rate;
eIntegrateType integrator_rotational_position;
void CalculatePQRdot(void);
void CalculateQuatdot(void);
void CalculateInertialVelocity(void);
+ void CalculateUVW(void);
void CalculateUVWdot(void);
void Integrate( FGColumnVector3& Integrand,
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);
};