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 Lesser 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 Lesser General Public License for more
21 You should have received a copy of the GNU Lesser 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 Lesser 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 <input_output/FGPropertyManager.h>
89 static const char *IdSrc = "$Id$";
90 static const char *IdHdr = ID_PROPAGATE;
92 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
94 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
96 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
105 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
107 FGPropagate::~FGPropagate(void)
113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
115 bool FGPropagate::InitModel(void)
117 FGModel::InitModel();
119 SeaLevelRadius = Inertial->RefRadius(); // For initialization ONLY
120 RunwayRadius = SeaLevelRadius;
122 VState.vLocation.SetRadius( SeaLevelRadius + 4.0 );
127 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
129 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
131 SeaLevelRadius = FGIC->GetSeaLevelRadiusFtIC();
132 RunwayRadius = SeaLevelRadius;
134 // Set the position lat/lon/radius
135 VState.vLocation = FGLocation( FGIC->GetLongitudeRadIC(),
136 FGIC->GetLatitudeRadIC(),
137 FGIC->GetAltitudeFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
139 // Set the Orientation from the euler angles
140 VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
141 FGIC->GetThetaRadIC(),
142 FGIC->GetPsiRadIC() );
144 // Set the velocities in the instantaneus body frame
145 VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
146 FGIC->GetVBodyFpsIC(),
147 FGIC->GetWBodyFpsIC() );
149 // Set the angular velocities in the instantaneus body frame.
150 VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
152 FGIC->GetRRadpsIC() );
154 // Compute some derived values.
155 vVel = VState.vQtrn.GetTInv()*VState.vUVW;
157 // Finally, make sure that the quaternion stays normalized.
158 VState.vQtrn.Normalize();
160 // Recompute the RunwayRadius level.
161 RecomputeRunwayRadius();
164 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
166 Purpose: Called on a schedule to perform EOM integration
167 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
168 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
170 At the top of this Run() function, see several "shortcuts" (or, aliases) being
171 set up for use later, rather than using the longer class->function() notation.
173 Here, propagation of state is done using a simple explicit Euler scheme (see the
174 bottom of the function). This propagation is done using the current state values
175 and current derivatives. Based on these values we compute an approximation to the
176 state values for (now + dt).
180 bool FGPropagate::Run(void)
182 if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
183 if (FDMExec->Holding()) return false;
185 RecomputeRunwayRadius();
187 double dt = State->Getdt()*rate; // The 'stepsize'
188 const FGColumnVector3 omega( 0.0, 0.0, Inertial->omega() ); // earth rotation
189 const FGColumnVector3& vForces = Aircraft->GetForces(); // current forces
190 const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
192 double mass = MassBalance->GetMass(); // mass
193 const FGMatrix33& J = MassBalance->GetJ(); // inertia matrix
194 const FGMatrix33& Jinv = MassBalance->GetJinv(); // inertia matrix inverse
195 double r = GetRadius(); // radius
196 if (r == 0.0) {cerr << "radius = 0 !" << endl; r = 1e-16;} // radius check
198 FGColumnVector3 gAccel( 0.0, 0.0, Inertial->GetGAccel(r) );
200 // The rotation matrices:
201 const FGMatrix33& Tl2b = GetTl2b(); // local to body frame
202 const FGMatrix33& Tb2l = GetTb2l(); // body to local frame
203 const FGMatrix33& Tec2l = VState.vLocation.GetTec2l(); // earth centered to local frame
204 const FGMatrix33& Tl2ec = VState.vLocation.GetTl2ec(); // local to earth centered frame
206 // Inertial angular velocity measured in the body frame.
207 const FGColumnVector3 pqri = VState.vPQR + Tl2b*(Tec2l*omega);
209 // Compute vehicle velocity wrt EC frame, expressed in Local horizontal frame.
210 vVel = Tb2l * VState.vUVW;
212 // First compute the time derivatives of the vehicle state values:
214 // Compute body frame rotational accelerations based on the current body moments
215 vPQRdot = Jinv*(vMoments - pqri*(J*pqri));
217 // Compute body frame accelerations based on the current body forces
218 vUVWdot = VState.vUVW*VState.vPQR + vForces/mass;
220 // Coriolis acceleration.
221 FGColumnVector3 ecVel = Tl2ec*vVel;
222 FGColumnVector3 ace = 2.0*omega*ecVel;
223 vUVWdot -= Tl2b*(Tec2l*ace);
225 if (!GroundReactions->GetWOW()) {
226 // Centrifugal acceleration.
227 FGColumnVector3 aeec = omega*(omega*VState.vLocation);
228 vUVWdot -= Tl2b*(Tec2l*aeec);
232 vUVWdot += Tl2b*gAccel;
234 // Compute vehicle velocity wrt EC frame, expressed in EC frame
235 vLocationDot = Tl2ec * vVel;
237 FGColumnVector3 omegaLocal( rInv*vVel(eEast),
239 -rInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
241 // Compute quaternion orientation derivative on current body rates
242 vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*omegaLocal );
244 // Integrate to propagate the state
246 // Propagate rotational velocity
248 // VState.vPQR += dt*(1.5*vPQRdot - 0.5*last_vPQRdot); // Adams-Bashforth
249 VState.vPQR += (1/12.0)*dt*(23.0*vPQRdot - 16.0*last_vPQRdot + 5.0*last2_vPQRdot); // Adams-Bashforth 3
250 // VState.vPQR += dt*vPQRdot; // Rectangular Euler
251 // VState.vPQR += 0.5*dt*(vPQRdot + last_vPQRdot); // Trapezoidal
253 // Propagate translational velocity
255 // VState.vUVW += dt*(1.5*vUVWdot - 0.5*last_vUVWdot); // Adams Bashforth
256 VState.vUVW += (1/12.0)*dt*(23.0*vUVWdot - 16.0*last_vUVWdot + 5.0*last2_vUVWdot); // Adams-Bashforth 3
257 // VState.vUVW += dt*vUVWdot; // Rectangular Euler
258 // VState.vUVW += 0.5*dt*(vUVWdot + last_vUVWdot); // Trapezoidal
260 // Propagate angular position
262 // VState.vQtrn += dt*(1.5*vQtrndot - 0.5*last_vQtrndot); // Adams Bashforth
263 VState.vQtrn += (1/12.0)*dt*(23.0*vQtrndot - 16.0*last_vQtrndot + 5.0*last2_vQtrndot); // Adams-Bashforth 3
264 // VState.vQtrn += dt*vQtrndot; // Rectangular Euler
265 // VState.vQtrn += 0.5*dt*(vQtrndot + last_vQtrndot); // Trapezoidal
267 // Propagate translational position
269 // VState.vLocation += dt*(1.5*vLocationDot - 0.5*last_vLocationDot); // Adams Bashforth
270 VState.vLocation += (1/12.0)*dt*(23.0*vLocationDot - 16.0*last_vLocationDot + 5.0*last2_vLocationDot); // Adams-Bashforth 3
271 // VState.vLocation += dt*vLocationDot; // Rectangular Euler
272 // VState.vLocation += 0.5*dt*(vLocationDot + last_vLocationDot); // Trapezoidal
276 last2_vPQRdot = last_vPQRdot;
277 last_vPQRdot = vPQRdot;
279 last2_vUVWdot = last_vUVWdot;
280 last_vUVWdot = vUVWdot;
282 last2_vQtrndot = last_vQtrndot;
283 last_vQtrndot = vQtrndot;
285 last2_vLocationDot = last_vLocationDot;
286 last_vLocationDot = vLocationDot;
291 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
293 void FGPropagate::RecomputeRunwayRadius(void)
295 // Get the runway radius.
296 FGLocation contactloc;
298 FGGroundCallback* gcb = FDMExec->GetGroundCallback();
299 double t = State->Getsim_time();
300 gcb->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
301 RunwayRadius = contactloc.GetRadius();
304 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
306 void FGPropagate::Seth(double tt)
308 VState.vLocation.SetRadius( tt + SeaLevelRadius );
311 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
313 double FGPropagate::GetRunwayRadius(void) const
318 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
320 double FGPropagate::GetDistanceAGL(void) const
322 return VState.vLocation.GetRadius() - RunwayRadius;
325 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
327 void FGPropagate::SetDistanceAGL(double tt)
329 VState.vLocation.SetRadius( tt + RunwayRadius );
332 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
334 void FGPropagate::bind(void)
336 typedef double (FGPropagate::*PMF)(int) const;
337 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
339 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
340 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
341 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
343 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
344 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
345 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
347 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
348 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
349 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
351 PropertyManager->Tie("accelerations/pdot-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRdot);
352 PropertyManager->Tie("accelerations/qdot-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRdot);
353 PropertyManager->Tie("accelerations/rdot-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRdot);
355 PropertyManager->Tie("accelerations/udot-fps", this, eU, (PMF)&FGPropagate::GetUVWdot);
356 PropertyManager->Tie("accelerations/vdot-fps", this, eV, (PMF)&FGPropagate::GetUVWdot);
357 PropertyManager->Tie("accelerations/wdot-fps", this, eW, (PMF)&FGPropagate::GetUVWdot);
359 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::Geth, &FGPropagate::Seth, true);
360 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
361 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
362 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
363 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
365 PropertyManager->Tie("metrics/runway-radius", this, &FGPropagate::GetRunwayRadius);
367 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
368 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
369 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
371 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
372 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
373 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
376 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
378 void FGPropagate::unbind(void)
380 PropertyManager->Untie("velocities/v-north-fps");
381 PropertyManager->Untie("velocities/v-east-fps");
382 PropertyManager->Untie("velocities/v-down-fps");
383 PropertyManager->Untie("velocities/h-dot-fps");
384 PropertyManager->Untie("velocities/u-fps");
385 PropertyManager->Untie("velocities/v-fps");
386 PropertyManager->Untie("velocities/w-fps");
387 PropertyManager->Untie("velocities/p-rad_sec");
388 PropertyManager->Untie("velocities/q-rad_sec");
389 PropertyManager->Untie("velocities/r-rad_sec");
390 PropertyManager->Untie("accelerations/udot-fps");
391 PropertyManager->Untie("accelerations/vdot-fps");
392 PropertyManager->Untie("accelerations/wdot-fps");
393 PropertyManager->Untie("accelerations/pdot-rad_sec");
394 PropertyManager->Untie("accelerations/qdot-rad_sec");
395 PropertyManager->Untie("accelerations/rdot-rad_sec");
396 PropertyManager->Untie("position/h-sl-ft");
397 PropertyManager->Untie("position/lat-gc-rad");
398 PropertyManager->Untie("position/long-gc-rad");
399 PropertyManager->Untie("position/h-agl-ft");
400 PropertyManager->Untie("position/radius-to-vehicle-ft");
401 PropertyManager->Untie("metrics/runway-radius");
402 PropertyManager->Untie("attitude/phi-rad");
403 PropertyManager->Untie("attitude/theta-rad");
404 PropertyManager->Untie("attitude/psi-rad");
405 PropertyManager->Untie("attitude/roll-rad");
406 PropertyManager->Untie("attitude/pitch-rad");
407 PropertyManager->Untie("attitude/heading-true-rad");
410 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
411 // The bitmasked value choices are as follows:
412 // unset: In this case (the default) JSBSim would only print
413 // out the normally expected messages, essentially echoing
414 // the config files as they are read. If the environment
415 // variable is not set, debug_lvl is set to 1 internally
416 // 0: This requests JSBSim not to output any messages
418 // 1: This value explicity requests the normal JSBSim
420 // 2: This value asks for a message to be printed out when
421 // a class is instantiated
422 // 4: When this value is set, a message is displayed when a
423 // FGModel object executes its Run() method
424 // 8: When this value is set, various runtime state variables
425 // are printed out periodically
426 // 16: When set various parameters are sanity checked and
427 // a message is printed out when they go out of bounds
429 void FGPropagate::Debug(int from)
431 if (debug_lvl <= 0) return;
433 if (debug_lvl & 1) { // Standard console startup message output
434 if (from == 0) { // Constructor
438 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
439 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
440 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
442 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
444 if (debug_lvl & 8 ) { // Runtime state variables
446 if (debug_lvl & 16) { // Sanity checking
448 if (debug_lvl & 64) {
449 if (from == 0) { // Constructor
450 cout << IdSrc << endl;
451 cout << IdHdr << endl;