1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7 ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
9 This program is free software; you can redistribute it and/or modify it under
10 the terms of the GNU Lesser General Public License as published by the Free Software
11 Foundation; either version 2 of the License, or (at your option) any later
14 This program is distributed in the hope that it will be useful, but WITHOUT
15 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
19 You should have received a copy of the GNU Lesser General Public License along with
20 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21 Place - Suite 330, Boston, MA 02111-1307, USA.
23 Further information about the GNU Lesser General Public License can also be found on
24 the world wide web at http://www.gnu.org.
27 --------------------------------------------------------------------------------
30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
37 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
41 #include <models/FGModel.h>
42 #include <math/FGColumnVector3.h>
43 #include <initialization/FGInitialCondition.h>
44 #include <math/FGLocation.h>
45 #include <math/FGQuaternion.h>
46 #include <math/FGMatrix33.h>
48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
52 #define ID_PROPAGATE "$Id$"
54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
60 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
62 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
64 /** Models the EOM and integration/propagation of state.
65 The Equations of Motion (EOM) for JSBSim are integrated to propagate the
66 state of the vehicle given the forces and moments that act on it. The
67 integration accounts for a rotating Earth.
68 Integration of rotational and translation position and rate can be
69 customized as needed or frozen by the selection of no integrator. The
70 selection of which integrator to use is done through the setting of
71 the associated property. There are four properties which can be set:
74 simulation/integrator/rate/rotational
75 simulation/integrator/rate/translational
76 simulation/integrator/position/rotational
77 simulation/integrator/position/translational
80 Each of the integrators listed above can be set to one of the following values:
83 0: No integrator (Freeze)
90 @author Jon S. Berndt, Mathias Froehlich
94 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
96 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
98 class FGPropagate : public FGModel {
101 The constructor initializes several variables, and sets the initial set
102 of integrators to use as follows:
103 - integrator, rotational rate = Adams Bashforth 2
104 - integrator, translational rate = Adams Bashforth 2
105 - integrator, rotational position = Trapezoidal
106 - integrator, translational position = Trapezoidal
107 @param Executive a pointer to the parent executive object */
108 FGPropagate(FGFDMExec* Executive);
113 /// These define the indices use to select the various integrators.
114 enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3};
116 /** The current vehicle state vector structure contains the translational and
117 angular position, and the translational and angular velocity. */
118 struct VehicleState {
119 /** Represents the current location of the vehicle in Earth centered Earth
122 FGLocation vLocation;
123 /** The velocity vector of the vehicle with respect to the ECEF frame,
124 expressed in the body system.
126 FGColumnVector3 vUVW;
127 /** The angular velocity vector for the vehicle relative to the ECEF frame,
128 expressed in the body frame.
130 FGColumnVector3 vPQR;
131 /** The current orientation of the vehicle, that is, the orientation of the
132 body frame relative to the local, vehilce-carried, NED frame. */
136 /** Initializes the FGPropagate class after instantiation and prior to first execution.
137 The base class FGModel::InitModel is called first, initializing pointers to the
138 other FGModel objects (and others). */
139 bool InitModel(void);
141 /** Runs the Propagate model; called by the Executive.
142 @return false if no error */
145 /** Retrieves the velocity vector.
146 The vector returned is represented by an FGColumnVector reference. The vector
147 for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
148 is 1-based, so that the first element can be retrieved using the "()" operator.
149 In other words, vVel(1) is Vnorth. Various convenience enumerators are defined
150 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
151 eNorth=1, eEast=2, eDown=3.
153 @return The vehicle velocity vector with respect to the Earth centered frame,
154 expressed in Local horizontal frame.
156 const FGColumnVector3& GetVel(void) const { return vVel; }
158 /** Retrieves the body frame vehicle velocity vector.
159 The vector returned is represented by an FGColumnVector reference. The vector
160 for the velocity in Body frame is organized (Vx, Vy, Vz). The vector
161 is 1-based, so that the first element can be retrieved using the "()" operator.
162 In other words, vUVW(1) is Vx. Various convenience enumerators are defined
163 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
166 @return The body frame vehicle velocity vector in ft/sec.
168 const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
170 /** Retrieves the body axis acceleration.
171 Retrieves the computed body axis accelerations based on the
172 applied forces and accounting for a rotating body frame.
173 The vector returned is represented by an FGColumnVector reference. The vector
174 for the acceleration in Body frame is organized (Ax, Ay, Az). The vector
175 is 1-based, so that the first element can be retrieved using the "()" operator.
176 In other words, vUVWdot(1) is Ax. Various convenience enumerators are defined
177 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
180 @return Body axis translational acceleration in ft/sec^2.
182 const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; }
184 /** Retrieves the body angular rates vector.
185 Retrieves the body angular rates (p, q, r), which are calculated by integration
186 of the angular acceleration.
187 The vector returned is represented by an FGColumnVector reference. The vector
188 for the angular velocity in Body frame is organized (P, Q, R). The vector
189 is 1-based, so that the first element can be retrieved using the "()" operator.
190 In other words, vPQR(1) is P. Various convenience enumerators are defined
191 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
194 @return The body frame angular rates in rad/sec.
196 const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
198 /** Retrieves the body axis angular acceleration vector.
199 Retrieves the body axis angular acceleration vector in rad/sec^2. The
200 angular acceleration vector is determined from the applied forces and
201 accounts for a rotating frame.
202 The vector returned is represented by an FGColumnVector reference. The vector
203 for the angular acceleration in Body frame is organized (Pdot, Qdot, Rdot). The vector
204 is 1-based, so that the first element can be retrieved using the "()" operator.
205 In other words, vPQRdot(1) is Pdot. Various convenience enumerators are defined
206 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
209 @return The angular acceleration vector.
211 const FGColumnVector3& GetPQRdot(void) const {return vPQRdot;}
213 /** Retrieves the Euler angles that define the vehicle orientation.
214 Extracts the Euler angles from the quaternion that stores the orientation
215 in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
216 vector returned is represented by an FGColumnVector reference. The vector
217 for the Euler angles is organized (Phi, Theta, Psi). The vector
218 is 1-based, so that the first element can be retrieved using the "()" operator.
219 In other words, the returned vector item with subscript (1) is Phi.
220 Various convenience enumerators are defined in FGJSBBase. The relevant
221 enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
223 @return The Euler angle vector, where the first item in the
224 vector is the angle about the X axis, the second is the
225 angle about the Y axis, and the third item is the angle
226 about the Z axis (Phi, Theta, Psi).
228 const FGColumnVector3& GetEuler(void) const { return VState.vQtrn.GetEuler(); }
230 /** Retrieves a body frame velocity component.
231 Retrieves a body frame velocity component. The velocity returned is
232 extracted from the vUVW vector (an FGColumnVector). The vector for the
233 velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based.
234 In other words, GetUVW(1) returns Vx. Various convenience enumerators
235 are defined in FGJSBBase. The relevant enumerators for the velocity
236 returned by this call are, eX=1, eY=2, eZ=3.
238 @param idx the index of the velocity component desired (1-based).
239 @return The body frame velocity component.
241 double GetUVW (int idx) const { return VState.vUVW(idx); }
243 /** Retrieves a body frame acceleration component.
244 Retrieves a body frame acceleration component. The acceleration returned
245 is extracted from the vUVWdot vector (an FGColumnVector). The vector for
246 the acceleration in Body frame is organized (Ax, Ay, Az). The vector is
247 1-based. In other words, GetUVWdot(1) returns Ax. Various convenience
248 enumerators are defined in FGJSBBase. The relevant enumerators for the
249 acceleration returned by this call are, eX=1, eY=2, eZ=3.
251 @param idx the index of the acceleration component desired (1-based).
252 @return The body frame acceleration component.
254 double GetUVWdot(int idx) const { return vUVWdot(idx); }
256 /** Retrieves a Local frame velocity component.
257 Retrieves a Local frame velocity component. The velocity returned is
258 extracted from the vVel vector (an FGColumnVector). The vector for the
259 velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
260 is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience
261 enumerators are defined in FGJSBBase. The relevant enumerators for the
262 velocity returned by this call are, eNorth=1, eEast=2, eDown=3.
264 @param idx the index of the velocity component desired (1-based).
265 @return The body frame velocity component.
267 double GetVel(int idx) const { return vVel(idx); }
269 /** Retrieves the total inertial velocity in ft/sec.
271 double GetInertialVelocityMagnitude(void) const { return vInertialVelocity.Magnitude(); }
273 /** Returns the current altitude.
274 Returns the current altitude. Specifically, this function returns the
275 difference between the distance to the center of the Earth, and sea level.
277 @return The current altitude above sea level in feet.
279 double Geth(void) const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
281 /** Returns the current altitude.
282 Returns the curren altitude. Specifically, this function returns the
283 difference between the distance to the center of the Earth, and sea level.
285 @return The current altitude above sea level in meters.
287 double Gethmeters(void) const { return Geth()*fttom;}
289 /** Retrieves a body frame angular velocity component.
290 Retrieves a body frame angular velocity component. The angular velocity
291 returned is extracted from the vPQR vector (an FGColumnVector). The vector
292 for the angular velocity in Body frame is organized (P, Q, R). The vector
293 is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
294 convenience enumerators are defined in FGJSBBase. The relevant enumerators
295 for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
297 @param axis the index of the angular velocity component desired (1-based).
298 @return The body frame angular velocity component.
300 double GetPQR(int axis) const {return VState.vPQR(axis);}
302 /** Retrieves a body frame angular acceleration component.
303 Retrieves a body frame angular acceleration component. The angular
304 acceleration returned is extracted from the vPQRdot vector (an
305 FGColumnVector). The vector for the angular acceleration in Body frame
306 is organized (Pdot, Qdot, Rdot). The vector is 1-based. In other words,
307 GetPQRdot(1) returns Pdot (roll acceleration). Various convenience
308 enumerators are defined in FGJSBBase. The relevant enumerators for the
309 angular acceleration returned by this call are, eP=1, eQ=2, eR=3.
311 @param axis the index of the angular acceleration component desired (1-based).
312 @return The body frame angular acceleration component.
314 double GetPQRdot(int axis) const {return vPQRdot(axis);}
316 /** Retrieves a vehicle Euler angle component.
317 Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
318 stores the vehicle orientation relative to the Local frame. The order of
319 rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
320 Phi. Various convenience enumerators are defined in FGJSBBase. The
321 relevant enumerators for the Euler angle returned by this call are,
322 ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
324 @return An Euler angle.
326 double GetEuler(int axis) const { return VState.vQtrn.GetEuler(axis); }
328 /** Retrieves the cosine of a vehicle Euler angle component.
329 Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
330 quaternion that stores the vehicle orientation relative to the Local frame.
331 The order of rotations used is Yaw-Pitch-Roll. The Euler angle
332 with subscript (1) is Phi. Various convenience enumerators are defined in
333 FGJSBBase. The relevant enumerators for the Euler angle referred to in this
334 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
336 @return The cosine of an Euler angle.
338 double GetCosEuler(int idx) const { return VState.vQtrn.GetCosEuler(idx); }
340 /** Retrieves the sine of a vehicle Euler angle component.
341 Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
342 quaternion that stores the vehicle orientation relative to the Local frame.
343 The order of rotations used is Yaw-Pitch-Roll. The Euler angle
344 with subscript (1) is Phi. Various convenience enumerators are defined in
345 FGJSBBase. The relevant enumerators for the Euler angle referred to in this
346 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
348 @return The sine of an Euler angle.
350 double GetSinEuler(int idx) const { return VState.vQtrn.GetSinEuler(idx); }
352 /** Returns the current altitude rate.
353 Returns the current altitude rate (rate of climb).
355 @return The current rate of change in altitude.
357 double Gethdot(void) const { return -vVel(eDown); }
359 /** Returns the "constant" RunwayRadius.
360 The RunwayRadius parameter is set by the calling application or set to
361 sea level if JSBSim is running in standalone mode.
363 @return distance of the runway from the center of the earth.
365 double GetRunwayRadius(void) const;
367 double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
368 double GetTerrainElevationASL(void) const;
369 double GetDistanceAGL(void) const;
370 double GetRadius(void) const {
371 if (VState.vLocation.GetRadius() == 0) return 1.0;
372 else return VState.vLocation.GetRadius();
374 double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
375 double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
377 double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
378 double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
380 double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
382 double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
383 double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
384 const FGLocation& GetLocation(void) const { return VState.vLocation; }
386 /** Retrieves the local-to-body transformation matrix.
387 The quaternion class, being the means by which the orientation of the
388 vehicle is stored, manages the local-to-body transformation matrix.
389 @return a reference to the local-to-body transformation matrix. */
390 const FGMatrix33& GetTl2b(void) const { return VState.vQtrn.GetT(); }
392 /** Retrieves the body-to-local transformation matrix.
393 The quaternion class, being the means by which the orientation of the
394 vehicle is stored, manages the body-to-local transformation matrix.
395 @return a reference to the body-to-local matrix. */
396 const FGMatrix33& GetTb2l(void) const { return VState.vQtrn.GetTInv(); }
398 /** Retrieves the ECEF-to-body transformation matrix.
399 @return a reference to the ECEF-to-body transformation matrix. */
400 const FGMatrix33& GetTec2b(void) const { return Tec2b; }
402 /** Retrieves the body-to-ECEF transformation matrix.
403 @return a reference to the body-to-ECEF matrix. */
404 const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
406 /** Retrieves the ECEF-to-ECI transformation matrix.
407 @return a reference to the ECEF-to-ECI transformation matrix. */
408 const FGMatrix33& GetTec2i(void);
410 /** Retrieves the ECI-to-ECEF transformation matrix.
411 @return a reference to the ECI-to-ECEF matrix. */
412 const FGMatrix33& GetTi2ec(void);
414 /** Retrieves the ECEF-to-local transformation matrix.
415 Retrieves the ECEF-to-local transformation matrix. Note that the so-called
416 local from is also know as the NED frame (for North, East, Down).
417 @return a reference to the ECEF-to-local matrix. */
418 const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); }
420 /** Retrieves the local-to-ECEF transformation matrix.
421 Retrieves the local-to-ECEF transformation matrix. Note that the so-called
422 local from is also know as the NED frame (for North, East, Down).
423 @return a reference to the local-to-ECEF matrix. */
424 const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
426 const VehicleState GetVState(void) const { return VState; }
428 void SetVState(VehicleState vstate) {
429 VState.vLocation = vstate.vLocation;
430 VState.vUVW = vstate.vUVW;
431 VState.vPQR = vstate.vPQR;
432 VState.vQtrn = vstate.vQtrn; // ... mmh
435 const FGQuaternion GetQuaternion(void) const { return VState.vQtrn; }
437 void SetPQR(unsigned int i, double val) {
438 if ((i>=1) && (i<=3) )
439 VState.vPQR(i) = val;
442 void SetUVW(unsigned int i, double val) {
443 if ((i>=1) && (i<=3) )
444 VState.vUVW(i) = val;
449 void SetLongitude(double lon) { VState.vLocation.SetLongitude(lon); }
450 void SetLongitudeDeg(double lon) {SetLongitude(lon*degtorad);}
451 void SetLatitude(double lat) { VState.vLocation.SetLatitude(lat); }
452 void SetLatitudeDeg(double lat) {SetLatitude(lat*degtorad);}
453 void SetRadius(double r) { VState.vLocation.SetRadius(r); }
454 void SetLocation(const FGLocation& l) { VState.vLocation = l; }
455 void Seth(double tt);
456 void Sethmeters(double tt) {Seth(tt/fttom);}
457 void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
458 void SetTerrainElevationASL(double tt);
459 void SetDistanceAGL(double tt);
460 void SetInitialState(const FGInitialCondition *);
461 void RecomputeRunwayRadius(void);
463 void CalculatePQRdot(void);
464 void CalculateQuatdot(void);
465 void CalculateLocationdot(void);
466 void CalculateUVWdot(void);
472 struct VehicleState VState;
474 FGColumnVector3 vVel;
475 FGColumnVector3 vInertialVelocity;
476 FGColumnVector3 vPQRdot, last_vPQRdot, last2_vPQRdot;
477 FGColumnVector3 vUVWdot, last_vUVWdot, last2_vUVWdot;
478 FGColumnVector3 vLocationDot, last_vLocationDot, last2_vLocationDot;
479 FGColumnVector3 vLocation;
480 FGColumnVector3 vPQRi; // Inertial frame angular velocity
481 FGColumnVector3 vOmega; // The Earth angular velocity vector
482 FGColumnVector3 vOmegaLocal; // The local frame angular velocity vector
483 FGQuaternion vQtrndot, last_vQtrndot, last2_vQtrndot;
486 FGMatrix33 Tl2b; // local to body frame matrix copy for immediate local use
487 FGMatrix33 Tb2l; // body to local frame matrix copy for immediate local use
488 FGMatrix33 Tl2ec; // local to ECEF matrix copy for immediate local use
489 FGMatrix33 Tec2l; // ECEF to local frame matrix copy for immediate local use
490 FGMatrix33 Tec2i; // ECEF to ECI frame matrix copy for immediate local use
491 FGMatrix33 Ti2ec; // ECI to ECEF frame matrix copy for immediate local use
492 FGMatrix33 Ti2b; // ECI to body frame rotation matrix
493 FGMatrix33 Tb2i; // body to ECI frame rotation matrix
495 double RunwayRadius, SeaLevelRadius, VehicleRadius;
497 int integrator_rotational_rate;
498 int integrator_translational_rate;
499 int integrator_rotational_position;
500 int integrator_translational_position;
503 void Debug(int from);
506 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%