namespace JSBSim {
-static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.59 2010/07/30 11:50:01 jberndt Exp $";
+static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.76 2011/01/16 16:10:59 bcoconni Exp $";
static const char *IdHdr = ID_PROPAGATE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
+FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex),
+LocalTerrainRadius(0), SeaLevelRadius(0), VehicleRadius(0)
{
Debug(0);
Name = "FGPropagate";
integrator_rotational_position = eAdamsBashforth2;
integrator_translational_position = eTrapezoidal;
- VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
- VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
+ VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
+ VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
if (!FGModel::InitModel()) return false;
// For initialization ONLY:
- SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius();
+ SeaLevelRadius = LocalTerrainRadius = FDMExec->GetInertial()->GetRefRadius();
VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
- VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
- vOmegaEarth = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
+ VState.vLocation.SetEllipse(FDMExec->GetInertial()->GetSemimajor(), FDMExec->GetInertial()->GetSemiminor());
+ vOmegaEarth = FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->omega() ); // Earth rotation vector
vPQRdot.InitMatrix();
vQtrndot = FGQuaternion(0,0,0);
vUVWdot.InitMatrix();
vInertialVelocity.InitMatrix();
- VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
- VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
+ VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
+ VState.dqUVWidot.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));
SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
- VehicleRadius = GetRadius();
- radInv = 1.0/VehicleRadius;
-
// Initialize the State Vector elements and the transformation matrices
// Set the position lat/lon/radius
FGIC->GetLatitudeRadIC(),
FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
- VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle());
+ VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
- Ti2ec = GetTi2ec(); // ECI to ECEF transform
- Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
-
- Tl2ec = GetTl2ec(); // local to ECEF transform
- Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
+ Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
+ Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
+
+ VState.vInertialPosition = Tec2i * VState.vLocation;
- Ti2l = GetTi2l();
- Tl2i = Ti2l.Transposed();
+ UpdateLocationMatrices();
// Set the orientation from the euler angles (is normalized within the
// constructor). The Euler angles represent the orientation of the body
FGIC->GetPsiRadIC() );
VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
-
- Ti2b = GetTi2b(); // ECI to body frame transform
- Tb2i = Ti2b.Transposed(); // body to ECI frame transform
-
- Tl2b = VState.qAttitudeLocal; // local to body frame transform
- Tb2l = Tl2b.Transposed(); // body to local frame transform
-
- Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
- Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
+ UpdateBodyMatrices();
// Set the velocities in the instantaneus body frame
VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
FGIC->GetVBodyFpsIC(),
FGIC->GetWBodyFpsIC() );
- VState.vInertialPosition = Tec2i * VState.vLocation;
-
// Compute the local frame ECEF velocity
vVel = Tb2l * VState.vUVW;
+ // Recompute the LocalTerrainRadius.
+ RecomputeLocalTerrainRadius();
+
+ VehicleRadius = GetRadius();
+ double radInv = 1.0/VehicleRadius;
+
// Refer to Stevens and Lewis, 1.5-14a, pg. 49.
// This is the rotation rate of the "Local" frame, expressed in the local frame.
FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;
VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
+ VState.vPQRi_i = Tb2i * VState.vPQRi;
// Make an initial run and set past values
- CalculatePQRdot(); // Angular rate derivative
- CalculateUVWdot(); // Translational rate derivative
- ResolveFrictionForces(0.); // Update rate derivatives with friction forces
- CalculateQuatdot(); // Angular orientation derivative
- CalculateInertialVelocity(); // Translational position derivative
-
- // Initialize past values deques
- VState.dqPQRdot.clear();
- VState.dqUVWdot.clear();
- VState.dqInertialVelocity.clear();
- VState.dqQtrndot.clear();
- for (int i=0; i<4; i++) {
- VState.dqPQRdot.push_front(vPQRdot);
- VState.dqUVWdot.push_front(vUVWdot);
- VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
- VState.dqQtrndot.push_front(vQtrndot);
- }
-
- // Recompute the LocalTerrainRadius.
- RecomputeLocalTerrainRadius();
+ InitializeDerivatives();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGPropagate::Run(void)
{
-static int ctr;
-
if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
if (FDMExec->Holding()) return false;
CalculateUVWdot(); // Translational rate derivative
ResolveFrictionForces(dt); // Update rate derivatives with friction forces
CalculateQuatdot(); // Angular orientation derivative
- CalculateInertialVelocity(); // Translational position derivative
+ CalculateUVW(); // Translational position derivative (velocities are integrated in the inertial frame)
// Propagate rotational / translational velocity, angular /translational position, respectively.
- Integrate(VState.vPQRi, vPQRdot, VState.dqPQRdot, dt, integrator_rotational_rate);
- Integrate(VState.vUVW, vUVWdot, VState.dqUVWdot, dt, integrator_translational_rate);
+
+ Integrate(VState.vPQRi_i, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate); // ECI integration
Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
+ Integrate(VState.vInertialVelocity, vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
+
+ // CAUTION : the order of the operations below is very important to get transformation
+ // matrices that are consistent with the new state of the vehicle
- VState.qAttitudeECI.Normalize(); // Normalize the ECI Attitude quaternion
+ // 1. Update the Earth position angle (EPA)
+ VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
- VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle()); // Update the Earth position angle (EPA)
+ // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
+ Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
+ Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
- // Update the "Location-based" transformation matrices from the vLocation vector.
+ // 3. Update the location from the updated Ti2ec and inertial position
+ VState.vLocation = Ti2ec*VState.vInertialPosition;
- Ti2ec = GetTi2ec(); // ECI to ECEF transform
- Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
- Tl2ec = GetTl2ec(); // local to ECEF transform
- Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
- Ti2l = GetTi2l();
- Tl2i = Ti2l.Transposed();
+ // 4. Update the other "Location-based" transformation matrices from the updated
+ // vLocation vector.
+ UpdateLocationMatrices();
- // Update the "Orientation-based" transformation matrices from the orientation quaternion
+ // 5. Normalize the ECI Attitude quaternion
+ VState.qAttitudeECI.Normalize();
- Ti2b = GetTi2b(); // ECI to body frame transform
- Tb2i = Ti2b.Transposed(); // body to ECI frame transform
- Tl2b = Ti2b*Tl2i; // local to body frame transform
- Tb2l = Tl2b.Transposed(); // body to local frame transform
- Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
- Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
+ // 6. Update the "Orientation-based" transformation matrices from the updated
+ // orientation quaternion and vLocation vector.
+ UpdateBodyMatrices();
// Set auxililary state variables
- VState.vLocation = Ti2ec*VState.vInertialPosition;
RecomputeLocalTerrainRadius();
VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
- radInv = 1.0/VehicleRadius;
+ VState.vPQRi = Ti2b * VState.vPQRi_i;
VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
VState.qAttitudeLocal = Tl2b.GetQuaternion();
void FGPropagate::CalculatePQRdot(void)
{
- const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
- const FGMatrix33& J = MassBalance->GetJ(); // inertia matrix
- const FGMatrix33& Jinv = MassBalance->GetJinv(); // inertia matrix inverse
+ const FGColumnVector3& vMoments = FDMExec->GetAircraft()->GetMoments(); // current moments
+ const FGMatrix33& J = FDMExec->GetMassBalance()->GetJ(); // inertia matrix
+ const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv(); // inertia matrix inverse
// Compute body frame rotational accelerations based on the current body
// moments and the total inertial angular velocity expressed in the body
// frame.
vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
+ vPQRidot = Tb2i * vPQRdot;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-// This set of calculations results in the body frame accelerations being
-// computed.
-// Compute body frame accelerations based on the current body forces.
-// Include centripetal and coriolis accelerations.
+// This set of calculations results in the body and inertial frame accelerations
+// being computed.
+// Compute body and inertial frames accelerations based on the current body
+// forces including centripetal and coriolis accelerations for the former.
// vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
// so it has to be transformed to the body frame. More completely,
// vOmegaEarth is the rate of the ECEF frame relative to the Inertial
void FGPropagate::CalculateUVWdot(void)
{
- double mass = MassBalance->GetMass(); // mass
- const FGColumnVector3& vForces = Aircraft->GetForces(); // current forces
+ double mass = FDMExec->GetMassBalance()->GetMass(); // mass
+ const FGColumnVector3& vForces = FDMExec->GetAircraft()->GetForces(); // current forces
vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
// Include Gravitation accel
switch (gravType) {
case gtStandard:
- vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
+ vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->GetGAccel(VehicleRadius) );
break;
case gtWGS84:
- vGravAccel = Tec2b * Inertial->GetGravityJ2(VState.vLocation);
+ vGravAccel = Tec2b * FDMExec->GetInertial()->GetGravityJ2(VState.vLocation);
break;
}
vUVWdot += vGravAccel;
+ vUVWidot = Tb2i * (vForces/mass + vGravAccel);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
}
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ // Transform the velocity vector of the inertial frame to be expressed in the
+ // body frame relative to the origin (Earth center), and substract the vehicle
+ // velocity contribution due to the rotation of the planet.
+
+void FGPropagate::CalculateUVW(void)
+{
+ VState.vUVW = Ti2b * (VState.vInertialVelocity - (vOmegaEarth * VState.vInertialPosition));
+}
+
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::Integrate( FGColumnVector3& Integrand,
}
}
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// Evaluates the rates (translation or rotation) that the friction forces have
+// to resist to. This includes the external forces and moments as well as the
+// relative movement between the aircraft and the ground.
+// Erin Catto's paper (see ref [6]) only supports Euler integration scheme and
+// this algorithm has been adapted to handle the multistep algorithms that
+// JSBSim supports (i.e. Trapezoidal, Adams-Bashforth 2, 3 and 4). The capacity
+// to handle the multistep integration schemes adds some complexity but it
+// significantly helps stabilizing the friction forces.
+
+void FGPropagate::EvaluateRateToResistTo(FGColumnVector3& vdot,
+ const FGColumnVector3& Val,
+ const FGColumnVector3& ValDot,
+ const FGColumnVector3& LocalTerrainVal,
+ deque <FGColumnVector3>& dqValDot,
+ const double dt,
+ const eIntegrateType integration_type)
+{
+ switch(integration_type) {
+ case eAdamsBashforth4:
+ vdot = ValDot + Ti2b * (-59.*dqValDot[0]+37.*dqValDot[1]-9.*dqValDot[2])/55.;
+ if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
+ vdot += 24.*(Val - Tec2b * LocalTerrainVal) / (55.*dt);
+ break;
+ case eAdamsBashforth3:
+ vdot = ValDot + Ti2b * (-16.*dqValDot[0]+5.*dqValDot[1])/23.;
+ if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
+ vdot += 12.*(Val - Tec2b * LocalTerrainVal) / (23.*dt);
+ break;
+ case eAdamsBashforth2:
+ vdot = ValDot - Ti2b * dqValDot[0]/3.;
+ if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
+ vdot += 2.*(Val - Tec2b * LocalTerrainVal) / (3.*dt);
+ break;
+ case eTrapezoidal:
+ vdot = ValDot + Ti2b * dqValDot[0];
+ if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
+ vdot += 2.*(Val - Tec2b * LocalTerrainVal) / dt;
+ break;
+ case eRectEuler:
+ vdot = ValDot;
+ if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
+ vdot += (Val - Tec2b * LocalTerrainVal) / dt;
+ break;
+ case eNone:
+ break;
+ }
+}
+
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Resolves the contact forces just before integrating the EOM.
// This routine is using Lagrange multipliers and the projected Gauss-Seidel
// multiple points of contact between the aircraft and the ground. As a
// consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
// in Catto's paper has been adapted accordingly.
+// The friction forces are resolved in the body frame relative to the origin
+// (Earth center).
void FGPropagate::ResolveFrictionForces(double dt)
{
- const double invMass = 1.0 / MassBalance->GetMass();
- const FGMatrix33& Jinv = MassBalance->GetJinv();
+ const double invMass = 1.0 / FDMExec->GetMassBalance()->GetMass();
+ const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv();
vector <FGColumnVector3> JacF, JacM;
+ vector<double> lambda, lambdaMin, lambdaMax;
FGColumnVector3 vdot, wdot;
FGColumnVector3 Fc, Mc;
- int n = 0, i;
+ int n = 0;
// Compiles data from the ground reactions to build up the jacobian matrix
- for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, n++) {
+ for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it, n++) {
JacF.push_back((*it)->ForceJacobian);
JacM.push_back((*it)->MomentJacobian);
+ lambda.push_back((*it)->value);
+ lambdaMax.push_back((*it)->Max);
+ lambdaMin.push_back((*it)->Min);
}
// If no gears are in contact with the ground then return
if (!n) return;
- double *a = new double[n*n]; // Will contain J*M^-1*J^T
- double *eta = new double[n];
- double *lambda = new double[n];
- double *lambdaMin = new double[n];
- double *lambdaMax = new double[n];
-
- // Initializes the Lagrange multipliers
- i = 0;
- for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, i++) {
- lambda[i] = (*it)->value;
- lambdaMax[i] = (*it)->Max;
- lambdaMin[i] = (*it)->Min;
- }
-
- vdot = vUVWdot;
- wdot = vPQRdot;
-
- if (dt > 0.) {
- // First compute the ground velocity below the aircraft center of gravity
- FGLocation contact;
- FGColumnVector3 normal, cvel;
- double t = FDMExec->GetSimTime();
- double height = FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contact, normal, cvel);
-
- // Instruct the algorithm to zero out the relative movement between the
- // aircraft and the ground.
- vdot += (VState.vUVW - Tec2b * cvel) / dt;
- wdot += VState.vPQR / dt;
- }
+ vector<double> a(n*n); // Will contain J*M^-1*J^T
+ vector<double> rhs(n);
// Assemble the linear system of equations
- for (i=0; i < n; i++) {
+ for (int i=0; i < n; i++) {
for (int j=0; j < i; j++)
a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
for (int j=i; j < n; j++)
a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
}
+ // Assemble the RHS member
+
+ // Translation
+ EvaluateRateToResistTo(vdot, VState.vUVW, vUVWdot, LocalTerrainVelocity,
+ VState.dqUVWidot, dt, integrator_translational_rate);
+
+ // Rotation
+ EvaluateRateToResistTo(wdot, VState.vPQR, vPQRdot, LocalTerrainAngularVelocity,
+ VState.dqPQRidot, dt, integrator_rotational_rate);
+
// Prepare the linear system for the Gauss-Seidel algorithm :
- // divide every line of 'a' and eta by a[i,i]. This is in order to save
- // a division computation at each iteration of Gauss-Seidel.
- for (i=0; i < n; i++) {
+ // 1. Compute the right hand side member 'rhs'
+ // 2. Divide every line of 'a' and 'rhs' by a[i,i]. This is in order to save
+ // a division computation at each iteration of Gauss-Seidel.
+ for (int i=0; i < n; i++) {
double d = 1.0 / a[i*n+i];
- eta[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
+ rhs[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
for (int j=0; j < n; j++)
a[i*n+j] *= d;
}
for (int iter=0; iter < 50; iter++) {
double norm = 0.;
- for (i=0; i < n; i++) {
+ for (int i=0; i < n; i++) {
double lambda0 = lambda[i];
- double dlambda = eta[i];
+ double dlambda = rhs[i];
for (int j=0; j < n; j++)
dlambda -= a[i*n+j]*lambda[j];
Fc.InitMatrix();
Mc.InitMatrix();
- for (i=0; i< n; i++) {
+ for (int i=0; i< n; i++) {
Fc += lambda[i]*JacF[i];
Mc += lambda[i]*JacM[i];
}
vUVWdot += invMass * Fc;
+ vUVWidot += invMass * Tb2i * Fc;
vPQRdot += Jinv * Mc;
+ vPQRidot += Tb2i* Jinv * Mc;
// Save the value of the Lagrange multipliers to accelerate the convergence
// of the Gauss-Seidel algorithm at next iteration.
- i = 0;
- for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it)
+ int i = 0;
+ for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it)
(*it)->value = lambda[i++];
- GroundReactions->UpdateForcesAndMoments();
+ FDMExec->GetGroundReactions()->UpdateForcesAndMoments();
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- delete a, eta, lambda, lambdaMin, lambdaMax;
+void FGPropagate::UpdateLocationMatrices(void)
+{
+ Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform
+ Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
+ Ti2l = VState.vLocation.GetTi2l(); // ECI to local frame transform
+ Tl2i = Ti2l.Transposed(); // local to ECI transform
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::UpdateBodyMatrices(void)
+{
+ Ti2b = VState.qAttitudeECI.GetT(); // ECI to body frame transform
+ Tb2i = Ti2b.Transposed(); // body to ECI frame transform
+ Tl2b = Ti2b*Tl2i; // local to body frame transform
+ Tb2l = Tl2b.Transposed(); // body to local frame transform
+ Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
+ Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
VState.qAttitudeECI = Qi;
+ VState.qAttitudeECI.Normalize();
+ UpdateBodyMatrices();
+ VState.qAttitudeLocal = Tl2b.GetQuaternion();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
VState.vInertialVelocity = Vi;
+ CalculateUVW();
+ vVel = Tb2l * VState.vUVW;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::SetInertialRates(FGColumnVector3 vRates) {
+ VState.vPQRi_i = vRates;
+ VState.vPQRi = Ti2b * VState.vPQRi_i;
+ VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::InitializeDerivatives(void)
+{
+ // Make an initial run and set past values
+ CalculatePQRdot(); // Angular rate derivative
+ CalculateUVWdot(); // Translational rate derivative
+ ResolveFrictionForces(0.); // Update rate derivatives with friction forces
+ CalculateQuatdot(); // Angular orientation derivative
+ CalculateInertialVelocity(); // Translational position derivative
+
+ // Initialize past values deques
+ VState.dqPQRidot.clear();
+ VState.dqUVWidot.clear();
+ VState.dqInertialVelocity.clear();
+ VState.dqQtrndot.clear();
+ for (int i=0; i<4; i++) {
+ VState.dqPQRidot.push_front(vPQRidot);
+ VState.dqUVWidot.push_front(vUVWdot);
+ VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
+ VState.dqQtrndot.push_front(vQtrndot);
+ }
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::RecomputeLocalTerrainRadius(void)
{
+ FGLocation contactloc;
+ FGColumnVector3 dv;
double t = FDMExec->GetSimTime();
// Get the LocalTerrain radius.
- FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
+ FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv,
+ LocalTerrainVelocity, LocalTerrainAngularVelocity);
LocalTerrainRadius = contactloc.GetRadius();
}
return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
}
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-//Todo: when should this be called - when should the new EPA be used to
-// calculate the transformation matrix, so that the matrix is not a step
-// ahead of the sim and the associated calculations?
-const FGMatrix33& FGPropagate::GetTi2ec(void)
-{
- return VState.vLocation.GetTi2ec();
-}
-
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-const FGMatrix33& FGPropagate::GetTec2i(void)
+double FGPropagate::GetDistanceAGL(void) const
{
- return VState.vLocation.GetTec2i();
+ return VState.vLocation.GetRadius() - LocalTerrainRadius;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-void FGPropagate::SetAltitudeASL(double altASL)
+void FGPropagate::SetVState(const VehicleState& vstate)
{
- VState.vLocation.SetRadius( altASL + SeaLevelRadius );
+ VState.vLocation = vstate.vLocation;
+ VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
+ Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
+ Tec2i = Ti2ec.Transposed();
+ UpdateLocationMatrices();
+ SetInertialOrientation(vstate.qAttitudeECI);
+ RecomputeLocalTerrainRadius();
+ VehicleRadius = GetRadius();
+ VState.vUVW = vstate.vUVW;
+ vVel = Tb2l * VState.vUVW;
+ VState.vPQR = vstate.vPQR;
+ VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
+ VState.vPQRi_i = Tb2i * VState.vPQRi;
+ VState.vInertialPosition = vstate.vInertialPosition;
+
+ InitializeDerivatives();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-double FGPropagate::GetLocalTerrainRadius(void) const
+void FGPropagate::UpdateVehicleState(void)
{
- return LocalTerrainRadius;
+ RecomputeLocalTerrainRadius();
+ VehicleRadius = GetRadius();
+ VState.vInertialPosition = Tec2i * VState.vLocation;
+ UpdateLocationMatrices();
+ UpdateBodyMatrices();
+ vVel = Tb2l * VState.vUVW;
+ VState.qAttitudeLocal = Tl2b.GetQuaternion();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-double FGPropagate::GetDistanceAGL(void) const
+void FGPropagate::SetLocation(const FGLocation& l)
{
- return VState.vLocation.GetRadius() - LocalTerrainRadius;
+ VState.vLocation = l;
+ VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
+ Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
+ Tec2i = Ti2ec.Transposed();
+ UpdateVehicleState();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-void FGPropagate::SetDistanceAGL(double tt)
+void FGPropagate::DumpState(void)
{
- VState.vLocation.SetRadius( tt + LocalTerrainRadius );
+ cout << endl;
+ cout << fgblue
+ << "------------------------------------------------------------------" << reset << endl;
+ cout << highint
+ << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
+ cout << " " << underon
+ << "Position" << underoff << endl;
+ cout << " ECI: " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
+ cout << " ECEF: " << VState.vLocation << " (x,y,z, in ft)" << endl;
+ cout << " Local: " << VState.vLocation.GetLatitudeDeg()
+ << ", " << VState.vLocation.GetLongitudeDeg()
+ << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
+
+ cout << endl << " " << underon
+ << "Orientation" << underoff << endl;
+ cout << " ECI: " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
+ cout << " Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
+
+ cout << endl << " " << underon
+ << "Velocity" << underoff << endl;
+ cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
+ cout << " ECEF: " << (Tb2ec * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl;
+ cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
+ cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl;
+
+ cout << endl << " " << underon
+ << "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
+ cout << " ECI: " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
+ cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
<< reset << endl;
cout << endl;
cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
- << Inertial->GetEarthPositionAngleDeg() << endl;
+ << FDMExec->GetInertial()->GetEarthPositionAngleDeg() << endl;
cout << endl;
cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;