1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 Module: FGPropagate.cpp
6 Purpose: Integrate the EOM to determine instantaneous position
9 ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
11 This program is free software; you can redistribute it and/or modify it under
12 the terms of the GNU General Public License as published by the Free Software
13 Foundation; either version 2 of the License, or (at your option) any later
16 This program is distributed in the hope that it will be useful, but WITHOUT
17 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
21 You should have received a copy of the GNU General Public License along with
22 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
23 Place - Suite 330, Boston, MA 02111-1307, USA.
25 Further information about the GNU General Public License can also be found on
26 the world wide web at http://www.gnu.org.
28 FUNCTIONAL DESCRIPTION
29 --------------------------------------------------------------------------------
30 This class encapsulates the integration of rates and accelerations to get the
31 current position of the aircraft.
34 --------------------------------------------------------------------------------
37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38 COMMENTS, REFERENCES, and NOTES
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
40 [1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
41 Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate
43 [2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
45 [3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
46 NASA-Ames", NASA CR-2497, January 1975
47 [4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
48 Wiley & Sons, 1979 ISBN 0-471-03032-5
49 [5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
50 1982 ISBN 0-471-08936-2
52 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
57 # include <simgear/compiler.h>
58 # ifdef SG_HAVE_STD_INCLUDES
66 # if defined(sgi) && !defined(__GNUC__)
68 # if (_COMPILER_VERSION < 740)
79 #include "FGPropagate.h"
81 #include "FGFDMExec.h"
82 #include "FGAircraft.h"
83 #include "FGMassBalance.h"
84 #include "FGInertial.h"
85 #include "FGPropertyManager.h"
89 static const char *IdSrc = "$Id$";
90 static const char *IdHdr = ID_PROPAGATE;
92 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
94 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
96 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
104 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
106 FGPropagate::~FGPropagate(void)
112 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
114 bool FGPropagate::InitModel(void)
116 FGModel::InitModel();
118 SeaLevelRadius = Inertial->RefRadius(); // For initialization ONLY
119 RunwayRadius = SeaLevelRadius;
121 VState.vLocation.SetRadius( SeaLevelRadius + 4.0 );
126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
128 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
130 SeaLevelRadius = FGIC->GetSeaLevelRadiusFtIC();
131 RunwayRadius = SeaLevelRadius;
133 // Set the position lat/lon/radius
134 VState.vLocation = FGLocation( FGIC->GetLongitudeRadIC(),
135 FGIC->GetLatitudeRadIC(),
136 FGIC->GetAltitudeFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
138 // Set the Orientation from the euler angles
139 VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
140 FGIC->GetThetaRadIC(),
141 FGIC->GetPsiRadIC() );
143 // Set the velocities in the instantaneus body frame
144 VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
145 FGIC->GetVBodyFpsIC(),
146 FGIC->GetWBodyFpsIC() );
148 // Set the angular velocities in the instantaneus body frame.
149 VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
151 FGIC->GetRRadpsIC() );
153 // Compute some derived values.
154 vVel = VState.vQtrn.GetTInv()*VState.vUVW;
156 // Finaly make shure that the quaternion stays normalized.
157 VState.vQtrn.Normalize();
159 // Recompute the RunwayRadius level.
160 RecomputeRunwayRadius();
163 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
165 Purpose: Called on a schedule to perform EOM integration
166 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
167 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
169 At the top of this Run() function, see several "shortcuts" (or, aliases) being
170 set up for use later, rather than using the longer class->function() notation.
172 Here, propagation of state is done using a simple explicit Euler scheme (see the
173 bottom of the function). This propagation is done using the current state values
174 and current derivatives. Based on these values we compute an approximation to the
175 state values for (now + dt).
179 bool FGPropagate::Run(void)
181 if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
183 RecomputeRunwayRadius();
185 double dt = State->Getdt()*rate; // The 'stepsize'
186 const FGColumnVector3 omega( 0.0, 0.0, Inertial->omega() ); // earth rotation
187 const FGColumnVector3& vForces = Aircraft->GetForces(); // current forces
188 const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
190 double mass = MassBalance->GetMass(); // mass
191 const FGMatrix33& J = MassBalance->GetJ(); // inertia matrix
192 const FGMatrix33& Jinv = MassBalance->GetJinv(); // inertia matrix inverse
193 double r = GetRadius(); // radius
194 if (r == 0.0) {cerr << "radius = 0 !" << endl; r = 1e-16;} // radius check
196 FGColumnVector3 gAccel( 0.0, 0.0, Inertial->GetGAccel(r) );
198 // The rotation matrices:
199 const FGMatrix33& Tl2b = GetTl2b(); // local to body frame
200 const FGMatrix33& Tb2l = GetTb2l(); // body to local frame
201 const FGMatrix33& Tec2l = VState.vLocation.GetTec2l(); // earth centered to local frame
202 const FGMatrix33& Tl2ec = VState.vLocation.GetTl2ec(); // local to earth centered frame
204 // Inertial angular velocity measured in the body frame.
205 const FGColumnVector3 pqri = VState.vPQR + Tl2b*(Tec2l*omega);
207 // Compute vehicle velocity wrt EC frame, expressed in Local horizontal frame.
208 vVel = Tb2l * VState.vUVW;
210 // First compute the time derivatives of the vehicle state values:
212 // Compute body frame rotational accelerations based on the current body moments
213 vPQRdot = Jinv*(vMoments - pqri*(J*pqri));
215 // Compute body frame accelerations based on the current body forces
216 vUVWdot = VState.vUVW*VState.vPQR + vForces/mass;
218 // Coriolis acceleration.
219 FGColumnVector3 ecVel = Tl2ec*vVel;
220 FGColumnVector3 ace = 2.0*omega*ecVel;
221 vUVWdot -= Tl2b*(Tec2l*ace);
223 // Centrifugal acceleration.
224 FGColumnVector3 aeec = omega*(omega*VState.vLocation);
225 vUVWdot -= Tl2b*(Tec2l*aeec);
228 vUVWdot += Tl2b*gAccel;
230 // Compute vehicle velocity wrt EC frame, expressed in EC frame
231 FGColumnVector3 vLocationDot = Tl2ec * vVel;
233 FGColumnVector3 omegaLocal( rInv*vVel(eEast),
235 -rInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
237 // Compute quaternion orientation derivative on current body rates
238 FGQuaternion vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*omegaLocal );
240 // Propagate velocities
241 VState.vPQR += dt*vPQRdot;
242 VState.vUVW += dt*vUVWdot;
244 // Propagate positions
245 VState.vQtrn += dt*vQtrndot;
246 VState.vLocation += dt*vLocationDot;
251 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
253 void FGPropagate::RecomputeRunwayRadius(void)
255 // Get the runway radius.
256 // Boring: this does not belong here, but since Jon placed the RunwayRadius
257 // value here it is better done here than somewhere else.
258 FGLocation contactloc;
260 FGGroundCallback* gcb = FDMExec->GetGroundCallback();
261 double t = State->Getsim_time();
262 gcb->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
263 RunwayRadius = contactloc.GetRadius();
266 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
268 void FGPropagate::Seth(double tt)
270 VState.vLocation.SetRadius( tt + SeaLevelRadius );
273 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
275 double FGPropagate::GetRunwayRadius(void) const
280 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
282 double FGPropagate::GetDistanceAGL(void) const
284 return VState.vLocation.GetRadius() - RunwayRadius;
287 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
289 void FGPropagate::SetDistanceAGL(double tt)
291 VState.vLocation.SetRadius( tt + RunwayRadius );
294 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
296 void FGPropagate::bind(void)
298 typedef double (FGPropagate::*PMF)(int) const;
299 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
301 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
302 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
303 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
305 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
306 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
307 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
309 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
310 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
311 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
313 PropertyManager->Tie("accelerations/pdot-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRdot);
314 PropertyManager->Tie("accelerations/qdot-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRdot);
315 PropertyManager->Tie("accelerations/rdot-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRdot);
317 PropertyManager->Tie("accelerations/udot-fps", this, eU, (PMF)&FGPropagate::GetUVWdot);
318 PropertyManager->Tie("accelerations/vdot-fps", this, eV, (PMF)&FGPropagate::GetUVWdot);
319 PropertyManager->Tie("accelerations/wdot-fps", this, eW, (PMF)&FGPropagate::GetUVWdot);
321 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::Geth, &FGPropagate::Seth, true);
322 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
323 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
324 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
325 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
327 PropertyManager->Tie("metrics/runway-radius", this, &FGPropagate::GetRunwayRadius);
329 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
330 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
331 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
333 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
334 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
335 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
338 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
340 void FGPropagate::unbind(void)
342 PropertyManager->Untie("velocities/v-north-fps");
343 PropertyManager->Untie("velocities/v-east-fps");
344 PropertyManager->Untie("velocities/v-down-fps");
345 PropertyManager->Untie("velocities/h-dot-fps");
346 PropertyManager->Untie("velocities/u-fps");
347 PropertyManager->Untie("velocities/v-fps");
348 PropertyManager->Untie("velocities/w-fps");
349 PropertyManager->Untie("velocities/p-rad_sec");
350 PropertyManager->Untie("velocities/q-rad_sec");
351 PropertyManager->Untie("velocities/r-rad_sec");
352 PropertyManager->Untie("accelerations/udot-fps");
353 PropertyManager->Untie("accelerations/vdot-fps");
354 PropertyManager->Untie("accelerations/wdot-fps");
355 PropertyManager->Untie("accelerations/pdot-rad_sec");
356 PropertyManager->Untie("accelerations/qdot-rad_sec");
357 PropertyManager->Untie("accelerations/rdot-rad_sec");
358 PropertyManager->Untie("position/h-sl-ft");
359 PropertyManager->Untie("position/lat-gc-rad");
360 PropertyManager->Untie("position/long-gc-rad");
361 PropertyManager->Untie("position/h-agl-ft");
362 PropertyManager->Untie("position/radius-to-vehicle-ft");
363 PropertyManager->Untie("metrics/runway-radius");
364 PropertyManager->Untie("attitude/phi-rad");
365 PropertyManager->Untie("attitude/theta-rad");
366 PropertyManager->Untie("attitude/psi-rad");
367 PropertyManager->Untie("attitude/roll-rad");
368 PropertyManager->Untie("attitude/pitch-rad");
369 PropertyManager->Untie("attitude/heading-true-rad");
372 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
373 // The bitmasked value choices are as follows:
374 // unset: In this case (the default) JSBSim would only print
375 // out the normally expected messages, essentially echoing
376 // the config files as they are read. If the environment
377 // variable is not set, debug_lvl is set to 1 internally
378 // 0: This requests JSBSim not to output any messages
380 // 1: This value explicity requests the normal JSBSim
382 // 2: This value asks for a message to be printed out when
383 // a class is instantiated
384 // 4: When this value is set, a message is displayed when a
385 // FGModel object executes its Run() method
386 // 8: When this value is set, various runtime state variables
387 // are printed out periodically
388 // 16: When set various parameters are sanity checked and
389 // a message is printed out when they go out of bounds
391 void FGPropagate::Debug(int from)
393 if (debug_lvl <= 0) return;
395 if (debug_lvl & 1) { // Standard console startup message output
396 if (from == 0) { // Constructor
400 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
401 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
402 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
404 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
406 if (debug_lvl & 8 ) { // Runtime state variables
408 if (debug_lvl & 16) { // Sanity checking
410 if (debug_lvl & 64) {
411 if (from == 0) { // Constructor
412 cout << IdSrc << endl;
413 cout << IdHdr << endl;