Purpose: Integrate the EOM to determine instantaneous position
Called by: FGFDMExec
- ------------- 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
Wiley & Sons, 1979 ISBN 0-471-03032-5
[5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
1982 ISBN 0-471-08936-2
+[6] Erin Catto, "Iterative Dynamics with Temporal Coherence", February 22, 2005
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#ifdef FGFS
-# include <simgear/compiler.h>
-# ifdef SG_HAVE_STD_INCLUDES
-# include <cmath>
-# include <iomanip>
-# else
-# include <math.h>
-# include <iomanip.h>
-# endif
-#else
-# if defined(sgi) && !defined(__GNUC__)
-# include <math.h>
-# if (_COMPILER_VERSION < 740)
-# include <iomanip.h>
-# else
-# include <iomanip>
-# endif
-# else
-# include <cmath>
-# include <iomanip>
-# endif
-#endif
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+#include <iomanip>
#include "FGPropagate.h"
-#include <FGState.h>
-#include <FGFDMExec.h>
+#include "FGGroundReactions.h"
+#include "FGFDMExec.h"
#include "FGAircraft.h"
#include "FGMassBalance.h"
#include "FGInertial.h"
-#include <input_output/FGPropertyManager.h>
+#include "input_output/FGPropertyManager.h"
+
+using namespace std;
namespace JSBSim {
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.88 2011/05/20 03:18:36 jberndt 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";
-// vQtrndot.zero();
+ gravType = gtWGS84;
+
+ vPQRidot.InitMatrix();
+ vQtrndot = FGQuaternion(0,0,0);
+ vUVWidot.InitMatrix();
+ vInertialVelocity.InitMatrix();
+
+ /// These define the indices use to select the various integrators.
+ // eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
+
+ integrator_rotational_rate = eRectEuler;
+ integrator_translational_rate = eAdamsBashforth2;
+ integrator_rotational_position = eRectEuler;
+ integrator_translational_position = eAdamsBashforth3;
+
+ 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));
bind();
Debug(0);
FGPropagate::~FGPropagate(void)
{
- unbind();
Debug(1);
}
bool FGPropagate::InitModel(void)
{
- FGModel::InitModel();
+ // For initialization ONLY:
+ SeaLevelRadius = LocalTerrainRadius = FDMExec->GetInertial()->GetRefRadius();
+
+ VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
+ VState.vLocation.SetEllipse(FDMExec->GetInertial()->GetSemimajor(), FDMExec->GetInertial()->GetSemiminor());
+ vOmegaEarth = FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->omega() ); // Earth rotation vector
- SeaLevelRadius = Inertial->RefRadius(); // For initialization ONLY
- RunwayRadius = SeaLevelRadius;
+ vPQRidot.InitMatrix();
+ vQtrndot = FGQuaternion(0,0,0);
+ vUVWidot.InitMatrix();
+ vInertialVelocity.InitMatrix();
- VState.vLocation.SetRadius( SeaLevelRadius + 4.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));
+
+ integrator_rotational_rate = eRectEuler;
+ integrator_translational_rate = eAdamsBashforth2;
+ integrator_rotational_position = eRectEuler;
+ integrator_translational_position = eAdamsBashforth3;
return true;
}
void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
{
- SeaLevelRadius = FGIC->GetSeaLevelRadiusFtIC();
- RunwayRadius = SeaLevelRadius;
+ SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
+ SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
+
+ // Initialize the State Vector elements and the transformation matrices
// Set the position lat/lon/radius
- VState.vLocation = FGLocation( FGIC->GetLongitudeRadIC(),
- FGIC->GetLatitudeRadIC(),
- FGIC->GetAltitudeFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
+ VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
+ FGIC->GetLatitudeRadIC(),
+ FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
+
+ VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
+
+ Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
+ Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
+
+ VState.vInertialPosition = Tec2i * VState.vLocation;
+
+ UpdateLocationMatrices();
+
+ // Set the orientation from the euler angles (is normalized within the
+ // constructor). The Euler angles represent the orientation of the body
+ // frame relative to the local frame.
+ VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
+ FGIC->GetThetaRadIC(),
+ FGIC->GetPsiRadIC() );
- // Set the Orientation from the euler angles
- VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
- FGIC->GetThetaRadIC(),
- FGIC->GetPsiRadIC() );
+ VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
+ UpdateBodyMatrices();
// Set the velocities in the instantaneus body frame
VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
- FGIC->GetVBodyFpsIC(),
- FGIC->GetWBodyFpsIC() );
+ FGIC->GetVBodyFpsIC(),
+ FGIC->GetWBodyFpsIC() );
- // Set the angular velocities in the instantaneus body frame.
- VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
- FGIC->GetQRadpsIC(),
- FGIC->GetRRadpsIC() );
+ // Compute the local frame ECEF velocity
+ vVel = Tb2l * VState.vUVW;
+
+ // Recompute the LocalTerrainRadius.
+ RecomputeLocalTerrainRadius();
- // Compute some derived values.
- vVel = VState.vQtrn.GetTInv()*VState.vUVW;
+ VehicleRadius = GetRadius();
+
+ // Set the angular velocities of the body frame relative to the ECEF frame,
+ // expressed in the body frame.
+ VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
+ FGIC->GetQRadpsIC(),
+ FGIC->GetRRadpsIC() );
- // Finally, make sure that the quaternion stays normalized.
- VState.vQtrn.Normalize();
+ VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
- // Recompute the RunwayRadius level.
- RecomputeRunwayRadius();
+ // Make an initial run and set past values
+ InitializeDerivatives();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
At the top of this Run() function, see several "shortcuts" (or, aliases) being
set up for use later, rather than using the longer class->function() notation.
-Here, propagation of state is done using a simple explicit Euler scheme (see the
-bottom of the function). This propagation is done using the current state values
+This propagation is done using the current state values
and current derivatives. Based on these values we compute an approximation to the
state values for (now + dt).
+In the code below, variables named beginning with a small "v" refer to a
+a column vector, variables named beginning with a "T" refer to a transformation
+matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
+Inertial.
+
*/
-bool FGPropagate::Run(void)
+bool FGPropagate::Run(bool Holding)
{
- if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
- if (FDMExec->Holding()) return false;
-
- RecomputeRunwayRadius();
-
- double dt = State->Getdt()*rate; // The 'stepsize'
- const FGColumnVector3 omega( 0.0, 0.0, Inertial->omega() ); // earth rotation
- const FGColumnVector3& vForces = Aircraft->GetForces(); // current forces
- const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
-
- double mass = MassBalance->GetMass(); // mass
- const FGMatrix33& J = MassBalance->GetJ(); // inertia matrix
- const FGMatrix33& Jinv = MassBalance->GetJinv(); // inertia matrix inverse
- double r = GetRadius(); // radius
- if (r == 0.0) {cerr << "radius = 0 !" << endl; r = 1e-16;} // radius check
- double rInv = 1.0/r;
- FGColumnVector3 gAccel( 0.0, 0.0, Inertial->GetGAccel(r) );
-
- // The rotation matrices:
- const FGMatrix33& Tl2b = GetTl2b(); // local to body frame
- const FGMatrix33& Tb2l = GetTb2l(); // body to local frame
- const FGMatrix33& Tec2l = VState.vLocation.GetTec2l(); // earth centered to local frame
- const FGMatrix33& Tl2ec = VState.vLocation.GetTl2ec(); // local to earth centered frame
-
- // Inertial angular velocity measured in the body frame.
- const FGColumnVector3 pqri = VState.vPQR + Tl2b*(Tec2l*omega);
-
- // Compute vehicle velocity wrt EC frame, expressed in Local horizontal frame.
+ if (FGModel::Run(Holding)) return true; // Fast return if we have nothing to do ...
+ if (Holding) return false;
+
+ double dt = FDMExec->GetDeltaT()*rate; // The 'stepsize'
+
+ RunPreFunctions();
+
+ // Calculate state derivatives
+ CalculatePQRdot(); // Angular rate derivative
+ CalculateUVWdot(); // Translational rate derivative
+ ResolveFrictionForces(dt); // Update rate derivatives with friction forces
+ CalculateQuatdot(); // Angular orientation derivative
+
+ // Propagate rotational / translational velocity, angular /translational position, respectively.
+
+ Integrate(VState.vPQRi, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate);
+ 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
+
+ // 1. Update the Earth position angle (EPA)
+ VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
+
+ // 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
+
+ // 3. Update the location from the updated Ti2ec and inertial position
+ VState.vLocation = Ti2ec*VState.vInertialPosition;
+
+ // 4. Update the other "Location-based" transformation matrices from the updated
+ // vLocation vector.
+ UpdateLocationMatrices();
+
+ // 5. Normalize the ECI Attitude quaternion
+ VState.qAttitudeECI.Normalize();
+
+ // 6. Update the "Orientation-based" transformation matrices from the updated
+ // orientation quaternion and vLocation vector.
+ UpdateBodyMatrices();
+
+ // Translational position derivative (velocities are integrated in the inertial frame)
+ CalculateUVW();
+
+ // Set auxilliary state variables
+ RecomputeLocalTerrainRadius();
+
+ VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
+
+ VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
+
+ VState.qAttitudeLocal = Tl2b.GetQuaternion();
+
+ // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
vVel = Tb2l * VState.vUVW;
- // First compute the time derivatives of the vehicle state values:
+ RunPostFunctions();
+
+ Debug(2);
+ return false;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// Compute body frame rotational accelerations based on the current body moments
+//
+// vPQRdot is the derivative of the absolute angular velocity of the vehicle
+// (body rate with respect to the inertial frame), expressed in the body frame,
+// where the derivative is taken in the body frame.
+// J is the inertia matrix
+// Jinv is the inverse inertia matrix
+// vMoments is the moment vector in the body frame
+// VState.vPQRi is the total inertial angular velocity of the vehicle
+// expressed in the body frame.
+// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
+// Second edition (2004), eqn 1.5-16e (page 50)
+
+void FGPropagate::CalculatePQRdot(void)
+{
+ 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
- vPQRdot = Jinv*(vMoments - pqri*(J*pqri));
+ // Compute body frame rotational accelerations based on the current body
+ // moments and the total inertial angular velocity expressed in the body
+ // frame.
- // Compute body frame accelerations based on the current body forces
- vUVWdot = VState.vUVW*VState.vPQR + vForces/mass;
+ vPQRidot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
+ vPQRdot = vPQRidot - VState.vPQRi * (Ti2b * vOmegaEarth);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// Compute the quaternion orientation derivative
+//
+// vQtrndot is the quaternion derivative.
+// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
+// Second edition (2004), eqn 1.5-16b (page 50)
- // Coriolis acceleration.
- FGColumnVector3 ecVel = Tl2ec*vVel;
- FGColumnVector3 ace = 2.0*omega*ecVel;
- vUVWdot -= Tl2b*(Tec2l*ace);
+void FGPropagate::CalculateQuatdot(void)
+{
+ // Compute quaternion orientation derivative on current body rates
+ vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
+}
- if (!GroundReactions->GetWOW()) {
- // Centrifugal acceleration.
- FGColumnVector3 aeec = omega*(omega*VState.vLocation);
- vUVWdot -= Tl2b*(Tec2l*aeec);
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// 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
+// frame (ECI), expressed in the Inertial frame.
+// vForces is the total force on the vehicle in the body frame.
+// VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
+// in the body frame.
+// VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
+// in the body frame.
+// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
+// Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
+
+void FGPropagate::CalculateUVWdot(void)
+{
+ 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 Centripetal acceleration.
+ vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
+
+ // Include Gravitation accel
+ switch (gravType) {
+ case gtStandard:
+ vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->GetGAccel(VehicleRadius) );
+ break;
+ case gtWGS84:
+ vGravAccel = Tec2b * FDMExec->GetInertial()->GetGravityJ2(VState.vLocation);
+ break;
}
- // Gravitation accel
- vUVWdot += Tl2b*gAccel;
+ vUVWdot += vGravAccel;
+ vUVWidot = Tb2i * (vForces/mass + vGravAccel);
+}
- // Compute vehicle velocity wrt EC frame, expressed in EC frame
- vLocationDot = Tl2ec * vVel;
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ // Transform the velocity vector of the body relative to the origin (Earth
+ // center) to be expressed in the inertial frame, and add the vehicle velocity
+ // contribution due to the rotation of the planet.
+ // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
+ // Second edition (2004), eqn 1.5-16c (page 50)
- FGColumnVector3 omegaLocal( rInv*vVel(eEast),
- -rInv*vVel(eNorth),
- -rInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
+void FGPropagate::CalculateInertialVelocity(void)
+{
+ VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
+}
- // Compute quaternion orientation derivative on current body rates
- vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*omegaLocal );
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ // 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.
- // Integrate to propagate the state
+void FGPropagate::CalculateUVW(void)
+{
+ VState.vUVW = Ti2b * (VState.vInertialVelocity - (vOmegaEarth * VState.vInertialPosition));
+}
- // Propagate rotational velocity
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- // VState.vPQR += dt*(1.5*vPQRdot - 0.5*last_vPQRdot); // Adams-Bashforth
- VState.vPQR += (1/12.0)*dt*(23.0*vPQRdot - 16.0*last_vPQRdot + 5.0*last2_vPQRdot); // Adams-Bashforth 3
- // VState.vPQR += dt*vPQRdot; // Rectangular Euler
- // VState.vPQR += 0.5*dt*(vPQRdot + last_vPQRdot); // Trapezoidal
+void FGPropagate::Integrate( FGColumnVector3& Integrand,
+ FGColumnVector3& Val,
+ deque <FGColumnVector3>& ValDot,
+ double dt,
+ eIntegrateType integration_type)
+{
+ ValDot.push_front(Val);
+ ValDot.pop_back();
+
+ switch(integration_type) {
+ case eRectEuler: Integrand += dt*ValDot[0];
+ break;
+ case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
+ break;
+ case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
+ break;
+ case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
+ break;
+ case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
+ break;
+ case eNone: // do nothing, freeze translational rate
+ break;
+ }
+}
- // Propagate translational velocity
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- // VState.vUVW += dt*(1.5*vUVWdot - 0.5*last_vUVWdot); // Adams Bashforth
- VState.vUVW += (1/12.0)*dt*(23.0*vUVWdot - 16.0*last_vUVWdot + 5.0*last2_vUVWdot); // Adams-Bashforth 3
- // VState.vUVW += dt*vUVWdot; // Rectangular Euler
- // VState.vUVW += 0.5*dt*(vUVWdot + last_vUVWdot); // Trapezoidal
+void FGPropagate::Integrate( FGQuaternion& Integrand,
+ FGQuaternion& Val,
+ deque <FGQuaternion>& ValDot,
+ double dt,
+ eIntegrateType integration_type)
+{
+ ValDot.push_front(Val);
+ ValDot.pop_back();
+
+ switch(integration_type) {
+ case eRectEuler: Integrand += dt*ValDot[0];
+ break;
+ case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
+ break;
+ case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
+ break;
+ case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
+ break;
+ case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
+ break;
+ case eNone: // do nothing, freeze rotational rate
+ break;
+ }
+}
- // Propagate angular position
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// 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;
+ }
+}
- // VState.vQtrn += dt*(1.5*vQtrndot - 0.5*last_vQtrndot); // Adams Bashforth
- VState.vQtrn += (1/12.0)*dt*(23.0*vQtrndot - 16.0*last_vQtrndot + 5.0*last2_vQtrndot); // Adams-Bashforth 3
- // VState.vQtrn += dt*vQtrndot; // Rectangular Euler
- // VState.vQtrn += 0.5*dt*(vQtrndot + last_vQtrndot); // Trapezoidal
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// Resolves the contact forces just before integrating the EOM.
+// This routine is using Lagrange multipliers and the projected Gauss-Seidel
+// (PGS) method.
+// Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
+// February 22, 2005
+// In JSBSim there is only one rigid body (the aircraft) and there can be
+// 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 / 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;
+
+ // Compiles data from the ground reactions to build up the jacobian matrix
+ 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);
+ }
- // Propagate translational position
+ // If no gears are in contact with the ground then return
+ if (!n) return;
- // VState.vLocation += dt*(1.5*vLocationDot - 0.5*last_vLocationDot); // Adams Bashforth
- VState.vLocation += (1/12.0)*dt*(23.0*vLocationDot - 16.0*last_vLocationDot + 5.0*last2_vLocationDot); // Adams-Bashforth 3
- // VState.vLocation += dt*vLocationDot; // Rectangular Euler
- // VState.vLocation += 0.5*dt*(vLocationDot + last_vLocationDot); // Trapezoidal
+ vector<double> a(n*n); // Will contain J*M^-1*J^T
+ vector<double> rhs(n);
- // Set past values
-
- last2_vPQRdot = last_vPQRdot;
- last_vPQRdot = vPQRdot;
-
- last2_vUVWdot = last_vUVWdot;
- last_vUVWdot = vUVWdot;
-
- last2_vQtrndot = last_vQtrndot;
- last_vQtrndot = vQtrndot;
+ // Assemble the linear system of equations
+ 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]);
+ }
- last2_vLocationDot = last_vLocationDot;
- last_vLocationDot = vLocationDot;
+ // Assemble the RHS member
- return false;
+ // 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 :
+ // 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];
+
+ rhs[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
+ for (int j=0; j < n; j++)
+ a[i*n+j] *= d;
+ }
+
+ // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
+ for (int iter=0; iter < 50; iter++) {
+ double norm = 0.;
+
+ for (int i=0; i < n; i++) {
+ double lambda0 = lambda[i];
+ double dlambda = rhs[i];
+
+ for (int j=0; j < n; j++)
+ dlambda -= a[i*n+j]*lambda[j];
+
+ lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
+ dlambda = lambda[i] - lambda0;
+
+ norm += fabs(dlambda);
+ }
+
+ if (norm < 1E-5) break;
+ }
+
+ // Calculate the total friction forces and moments
+
+ Fc.InitMatrix();
+ Mc.InitMatrix();
+
+ 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 += Jinv * Mc;
+
+ // Save the value of the Lagrange multipliers to accelerate the convergence
+ // of the Gauss-Seidel algorithm at next iteration.
+ int i = 0;
+ for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it)
+ (*it)->value = lambda[i++];
+
+ FDMExec->GetGroundReactions()->UpdateForcesAndMoments();
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+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 = Ti2b * Tec2i; // 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 = Ti2b * vRates;
+ 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(vUVWidot);
+ VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
+ VState.dqQtrndot.push_front(vQtrndot);
+ }
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-void FGPropagate::RecomputeRunwayRadius(void)
+void FGPropagate::RecomputeLocalTerrainRadius(void)
{
- // Get the runway radius.
FGLocation contactloc;
FGColumnVector3 dv;
- FGGroundCallback* gcb = FDMExec->GetGroundCallback();
- double t = State->Getsim_time();
- gcb->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
- RunwayRadius = contactloc.GetRadius();
+ double t = FDMExec->GetSimTime();
+
+ // Get the LocalTerrain radius.
+ FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv,
+ LocalTerrainVelocity, LocalTerrainAngularVelocity);
+ LocalTerrainRadius = contactloc.GetRadius();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-void FGPropagate::Seth(double tt)
+void FGPropagate::SetTerrainElevation(double terrainElev)
{
- VState.vLocation.SetRadius( tt + SeaLevelRadius );
+ LocalTerrainRadius = terrainElev + SeaLevelRadius;
+ FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-double FGPropagate::GetRunwayRadius(void) const
+double FGPropagate::GetTerrainElevation(void) const
{
- return RunwayRadius;
+ return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGPropagate::GetDistanceAGL(void) const
{
- return VState.vLocation.GetRadius() - RunwayRadius;
+ return VState.vLocation.GetRadius() - LocalTerrainRadius;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::SetVState(const VehicleState& vstate)
+{
+ 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.vInertialPosition = vstate.vInertialPosition;
+
+ InitializeDerivatives();
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::UpdateVehicleState(void)
+{
+ RecomputeLocalTerrainRadius();
+ VehicleRadius = GetRadius();
+ VState.vInertialPosition = Tec2i * VState.vLocation;
+ UpdateLocationMatrices();
+ UpdateBodyMatrices();
+ vVel = Tb2l * VState.vUVW;
+ VState.qAttitudeLocal = Tl2b.GetQuaternion();
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::SetLocation(const FGLocation& l)
+{
+ 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 + RunwayRadius );
+ 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;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::bind(void)
{
typedef double (FGPropagate::*PMF)(int) const;
+
PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
- PropertyManager->Tie("accelerations/pdot-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRdot);
- PropertyManager->Tie("accelerations/qdot-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRdot);
- PropertyManager->Tie("accelerations/rdot-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRdot);
+ PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
+ PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
+ PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
- PropertyManager->Tie("accelerations/udot-fps", this, eU, (PMF)&FGPropagate::GetUVWdot);
- PropertyManager->Tie("accelerations/vdot-fps", this, eV, (PMF)&FGPropagate::GetUVWdot);
- PropertyManager->Tie("accelerations/wdot-fps", this, eW, (PMF)&FGPropagate::GetUVWdot);
+ PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
- PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::Geth, &FGPropagate::Seth, true);
+ PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
+ PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
+ PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
+
+ PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
+ PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
+ PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
+
+ PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
+ PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
+ PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
+ PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
+ PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
+ PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
+ PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
+ PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
+ &FGPropagate::GetTerrainElevation,
+ &FGPropagate::SetTerrainElevation, false);
- PropertyManager->Tie("metrics/runway-radius", this, &FGPropagate::GetRunwayRadius);
+ PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-void FGPropagate::unbind(void)
-{
- PropertyManager->Untie("velocities/v-north-fps");
- PropertyManager->Untie("velocities/v-east-fps");
- PropertyManager->Untie("velocities/v-down-fps");
- PropertyManager->Untie("velocities/h-dot-fps");
- PropertyManager->Untie("velocities/u-fps");
- PropertyManager->Untie("velocities/v-fps");
- PropertyManager->Untie("velocities/w-fps");
- PropertyManager->Untie("velocities/p-rad_sec");
- PropertyManager->Untie("velocities/q-rad_sec");
- PropertyManager->Untie("velocities/r-rad_sec");
- PropertyManager->Untie("accelerations/udot-fps");
- PropertyManager->Untie("accelerations/vdot-fps");
- PropertyManager->Untie("accelerations/wdot-fps");
- PropertyManager->Untie("accelerations/pdot-rad_sec");
- PropertyManager->Untie("accelerations/qdot-rad_sec");
- PropertyManager->Untie("accelerations/rdot-rad_sec");
- PropertyManager->Untie("position/h-sl-ft");
- PropertyManager->Untie("position/lat-gc-rad");
- PropertyManager->Untie("position/long-gc-rad");
- PropertyManager->Untie("position/h-agl-ft");
- PropertyManager->Untie("position/radius-to-vehicle-ft");
- PropertyManager->Untie("metrics/runway-radius");
- PropertyManager->Untie("attitude/phi-rad");
- PropertyManager->Untie("attitude/theta-rad");
- PropertyManager->Untie("attitude/psi-rad");
- PropertyManager->Untie("attitude/roll-rad");
- PropertyManager->Untie("attitude/pitch-rad");
- PropertyManager->Untie("attitude/heading-true-rad");
+
+ PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
+ PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
+ PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
+ PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
+ PropertyManager->Tie("simulation/gravity-model", &gravType);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
- if (debug_lvl & 8 ) { // Runtime state variables
+ if (debug_lvl & 8 && from == 2) { // Runtime state variables
+ cout << endl << fgblue << highint << left
+ << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
+ << reset << endl;
+ cout << endl;
+ cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
+ << 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;
+ cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
+ cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
+ cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
+ cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
+ cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
+ cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
+ cout << endl;
+ cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
+ << reset << endl << Tec2b.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
+ << reset << endl << Tb2ec.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
+ << reset << endl << Tl2b.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
+ << reset << endl << Tb2l.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
+ << reset << endl << Tl2ec.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
+ << reset << endl << Tec2l.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
+ << reset << endl << Tec2i.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
+ << reset << endl << Ti2ec.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
+ << reset << endl << Ti2b.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
+ << reset << endl << Tb2i.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
+ << reset << endl << Ti2l.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
+ << reset << endl << Tl2i.Dump("\t", " ") << endl;
+ cout << highint << " Associated Euler angles (deg): " << setw(8)
+ << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
+ << endl << endl;
+
+ cout << setprecision(6); // reset the output stream
}
if (debug_lvl & 16) { // Sanity checking
+ if (from == 2) { // State sanity checking
+ if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
+ cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
+ exit(-1);
+ }
+ if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
+ cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
+ exit(-1);
+ }
+ if (fabs(GetDistanceAGL()) > 1e10) {
+ cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
+ exit(-1);
+ }
+ }
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
projects / flightgear.git / blobdiff