]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/FGPropagate.cpp
Sync. w. JSBSim CVS.
[flightgear.git] / src / FDM / JSBSim / FGPropagate.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3  Module:       FGPropagate.cpp
4  Author:       Jon S. Berndt
5  Date started: 01/05/99
6  Purpose:      Integrate the EOM to determine instantaneous position
7  Called by:    FGFDMExec
8
9  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
10
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
14  version.
15
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
19  details.
20
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.
24
25  Further information about the GNU General Public License can also be found on
26  the world wide web at http://www.gnu.org.
27
28 FUNCTIONAL DESCRIPTION
29 --------------------------------------------------------------------------------
30 This class encapsulates the integration of rates and accelerations to get the
31 current position of the aircraft.
32
33 HISTORY
34 --------------------------------------------------------------------------------
35 01/05/99   JSB   Created
36
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
42     School, January 1994
43 [2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
44     JSC 12960, July 1977
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
51
52 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
53 INCLUDES
54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
55
56 #ifdef FGFS
57 #  include <simgear/compiler.h>
58 #  ifdef SG_HAVE_STD_INCLUDES
59 #    include <cmath>
60 #    include <iomanip>
61 #  else
62 #    include <math.h>
63 #    include <iomanip.h>
64 #  endif
65 #else
66 #  if defined(sgi) && !defined(__GNUC__)
67 #    include <math.h>
68 #    if (_COMPILER_VERSION < 740)
69 #      include <iomanip.h>
70 #    else
71 #      include <iomanip>
72 #    endif
73 #  else
74 #    include <cmath>
75 #    include <iomanip>
76 #  endif
77 #endif
78
79 #include "FGPropagate.h"
80 #include "FGState.h"
81 #include "FGFDMExec.h"
82 #include "FGAircraft.h"
83 #include "FGMassBalance.h"
84 #include "FGInertial.h"
85 #include "FGPropertyManager.h"
86
87 namespace JSBSim {
88
89 static const char *IdSrc = "$Id$";
90 static const char *IdHdr = ID_PROPAGATE;
91
92 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
93 CLASS IMPLEMENTATION
94 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
95
96 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
97 {
98   Name = "FGPropagate";
99
100   bind();
101   Debug(0);
102 }
103
104 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
105
106 FGPropagate::~FGPropagate(void)
107 {
108   unbind();
109   Debug(1);
110 }
111
112 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
113
114 bool FGPropagate::InitModel(void)
115 {
116   FGModel::InitModel();
117
118   SeaLevelRadius = Inertial->RefRadius();          // For initialization ONLY
119   RunwayRadius   = SeaLevelRadius;
120
121   VState.vLocation.SetRadius( SeaLevelRadius + 4.0 );
122
123   return true;
124 }
125
126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
127
128 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
129 {
130   SeaLevelRadius = FGIC->GetSeaLevelRadiusFtIC();
131   RunwayRadius = FGIC->GetSeaLevelRadiusFtIC() + FGIC->GetTerrainAltitudeFtIC();
132
133   // Set the position lat/lon/radius
134   VState.vLocation = FGLocation( FGIC->GetLongitudeRadIC(),
135                           FGIC->GetLatitudeRadIC(),
136                           FGIC->GetAltitudeFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
137
138   // Set the Orientation from the euler angles
139   VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
140                         FGIC->GetThetaRadIC(),
141                         FGIC->GetPsiRadIC() );
142
143   // Set the velocities in the instantaneus body frame
144   VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
145                           FGIC->GetVBodyFpsIC(),
146                           FGIC->GetWBodyFpsIC() );
147
148   // Set the angular velocities in the instantaneus body frame.
149   VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
150                           FGIC->GetQRadpsIC(),
151                           FGIC->GetRRadpsIC() );
152
153   // Compute some derived values.
154   vVel = VState.vQtrn.GetTInv()*VState.vUVW;
155
156   // Finaly make shure that the quaternion stays normalized.
157   VState.vQtrn.Normalize();
158 }
159
160 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
161 /*
162 Purpose: Called on a schedule to perform EOM integration
163 Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
164          In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
165
166 At the top of this Run() function, see several "shortcuts" (or, aliases) being
167 set up for use later, rather than using the longer class->function() notation.
168
169 Here, propagation of state is done using a simple explicit Euler scheme (see the
170 bottom of the function). This propagation is done using the current state values
171 and current derivatives. Based on these values we compute an approximation to the
172 state values for (now + dt).
173
174 */
175
176 bool FGPropagate::Run(void)
177 {
178   if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
179
180   double dt = State->Getdt()*rate;  // The 'stepsize'
181   const FGColumnVector3 omega( 0.0, 0.0, Inertial->omega() ); // earth rotation
182   const FGColumnVector3& vForces = Aircraft->GetForces();     // current forces
183   const FGColumnVector3& vMoments = Aircraft->GetMoments();   // current moments
184
185   double mass = MassBalance->GetMass();             // mass
186   const FGMatrix33& J = MassBalance->GetJ();        // inertia matrix
187   const FGMatrix33& Jinv = MassBalance->GetJinv();  // inertia matrix inverse
188   double r = GetRadius();                           // radius
189   if (r == 0.0) {cerr << "radius = 0 !" << endl; r = 1e-16;} // radius check
190   double rInv = 1.0/r;
191   FGColumnVector3 gAccel( 0.0, 0.0, Inertial->GetGAccel(r) );
192
193   // The rotation matrices:
194   const FGMatrix33& Tl2b = GetTl2b();  // local to body frame
195   const FGMatrix33& Tb2l = GetTb2l();  // body to local frame
196   const FGMatrix33& Tec2l = VState.vLocation.GetTec2l();  // earth centered to local frame
197   const FGMatrix33& Tl2ec = VState.vLocation.GetTl2ec();  // local to earth centered frame
198
199   // Inertial angular velocity measured in the body frame.
200   const FGColumnVector3 pqri = VState.vPQR + Tl2b*(Tec2l*omega);
201
202   // Compute vehicle velocity wrt EC frame, expressed in Local horizontal frame.
203   vVel = Tb2l * VState.vUVW;
204
205   // First compute the time derivatives of the vehicle state values:
206
207   // Compute body frame rotational accelerations based on the current body moments
208   vPQRdot = Jinv*(vMoments - pqri*(J*pqri));
209
210   // Compute body frame accelerations based on the current body forces
211   vUVWdot = VState.vUVW*VState.vPQR + vForces/mass;
212
213   // Centrifugal acceleration.
214   FGColumnVector3 ecVel = Tl2ec*vVel;
215   FGColumnVector3 ace = 2.0*omega*ecVel;
216   vUVWdot -= Tl2b*(Tec2l*ace);
217
218   // Coriolis acceleration.
219   FGColumnVector3 aeec = omega*(omega*VState.vLocation);
220   vUVWdot -= Tl2b*(Tec2l*aeec);
221
222   // Gravitation accel
223   vUVWdot += Tl2b*gAccel;
224
225   // Compute vehicle velocity wrt EC frame, expressed in EC frame
226   FGColumnVector3 vLocationDot = Tl2ec * vVel;
227
228   FGColumnVector3 omegaLocal( rInv*vVel(eEast),
229                               -rInv*vVel(eNorth),
230                               -rInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
231
232   // Compute quaternion orientation derivative on current body rates
233   FGQuaternion vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*omegaLocal );
234
235   // Propagate velocities
236   VState.vPQR += dt*vPQRdot;
237   VState.vUVW += dt*vUVWdot;
238
239   // Propagate positions
240   VState.vQtrn += dt*vQtrndot;
241   VState.vLocation += dt*vLocationDot;
242
243   return false;
244 }
245
246 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
247
248 void FGPropagate::Seth(double tt)
249 {
250   VState.vLocation.SetRadius( tt + SeaLevelRadius );
251 }
252
253 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
254
255 void FGPropagate::SetDistanceAGL(double tt)
256 {
257   VState.vLocation.SetRadius( tt + RunwayRadius );
258 }
259
260 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
261
262 void FGPropagate::bind(void)
263 {
264   typedef double (FGPropagate::*PMF)(int) const;
265   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
266
267   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
268   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
269   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
270
271   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
272   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
273   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
274
275   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
276   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
277   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
278
279   PropertyManager->Tie("accelerations/pdot-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRdot);
280   PropertyManager->Tie("accelerations/qdot-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRdot);
281   PropertyManager->Tie("accelerations/rdot-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRdot);
282
283   PropertyManager->Tie("accelerations/udot-fps", this, eU, (PMF)&FGPropagate::GetUVWdot);
284   PropertyManager->Tie("accelerations/vdot-fps", this, eV, (PMF)&FGPropagate::GetUVWdot);
285   PropertyManager->Tie("accelerations/wdot-fps", this, eW, (PMF)&FGPropagate::GetUVWdot);
286
287   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::Geth, &FGPropagate::Seth, true);
288   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
289   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
290   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
291   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
292
293   PropertyManager->Tie("metrics/runway-radius", this, &FGPropagate::GetRunwayRadius, &FGPropagate::SetRunwayRadius);
294
295   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
296   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
297   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
298
299   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
300   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
301   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
302 }
303
304 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
305
306 void FGPropagate::unbind(void)
307 {
308   PropertyManager->Untie("velocities/v-north-fps");
309   PropertyManager->Untie("velocities/v-east-fps");
310   PropertyManager->Untie("velocities/v-down-fps");
311   PropertyManager->Untie("velocities/h-dot-fps");
312   PropertyManager->Untie("velocities/u-fps");
313   PropertyManager->Untie("velocities/v-fps");
314   PropertyManager->Untie("velocities/w-fps");
315   PropertyManager->Untie("velocities/p-rad_sec");
316   PropertyManager->Untie("velocities/q-rad_sec");
317   PropertyManager->Untie("velocities/r-rad_sec");
318   PropertyManager->Untie("accelerations/udot-fps");
319   PropertyManager->Untie("accelerations/vdot-fps");
320   PropertyManager->Untie("accelerations/wdot-fps");
321   PropertyManager->Untie("accelerations/pdot-rad_sec");
322   PropertyManager->Untie("accelerations/qdot-rad_sec");
323   PropertyManager->Untie("accelerations/rdot-rad_sec");
324   PropertyManager->Untie("position/h-sl-ft");
325   PropertyManager->Untie("position/lat-gc-rad");
326   PropertyManager->Untie("position/long-gc-rad");
327   PropertyManager->Untie("position/h-agl-ft");
328   PropertyManager->Untie("position/radius-to-vehicle-ft");
329   PropertyManager->Untie("metrics/runway-radius");
330   PropertyManager->Untie("attitude/phi-rad");
331   PropertyManager->Untie("attitude/theta-rad");
332   PropertyManager->Untie("attitude/psi-rad");
333   PropertyManager->Untie("attitude/roll-rad");
334   PropertyManager->Untie("attitude/pitch-rad");
335   PropertyManager->Untie("attitude/heading-true-rad");
336 }
337
338 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
339 //    The bitmasked value choices are as follows:
340 //    unset: In this case (the default) JSBSim would only print
341 //       out the normally expected messages, essentially echoing
342 //       the config files as they are read. If the environment
343 //       variable is not set, debug_lvl is set to 1 internally
344 //    0: This requests JSBSim not to output any messages
345 //       whatsoever.
346 //    1: This value explicity requests the normal JSBSim
347 //       startup messages
348 //    2: This value asks for a message to be printed out when
349 //       a class is instantiated
350 //    4: When this value is set, a message is displayed when a
351 //       FGModel object executes its Run() method
352 //    8: When this value is set, various runtime state variables
353 //       are printed out periodically
354 //    16: When set various parameters are sanity checked and
355 //       a message is printed out when they go out of bounds
356
357 void FGPropagate::Debug(int from)
358 {
359   if (debug_lvl <= 0) return;
360
361   if (debug_lvl & 1) { // Standard console startup message output
362     if (from == 0) { // Constructor
363
364     }
365   }
366   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
367     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
368     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
369   }
370   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
371   }
372   if (debug_lvl & 8 ) { // Runtime state variables
373   }
374   if (debug_lvl & 16) { // Sanity checking
375   }
376   if (debug_lvl & 64) {
377     if (from == 0) { // Constructor
378       cout << IdSrc << endl;
379       cout << IdHdr << endl;
380     }
381   }
382 }
383 }