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 <math/FGLocation.h>
44 #include <math/FGQuaternion.h>
45 #include <math/FGMatrix33.h>
47 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
51 #define ID_PROPAGATE "$Id$"
53 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
59 class FGInitialCondition;
61 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
63 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
65 /** Models the EOM and integration/propagation of state.
66 The Equations of Motion (EOM) for JSBSim are integrated to propagate the
67 state of the vehicle given the forces and moments that act on it. The
68 integration accounts for a rotating Earth.
69 Integration of rotational and translation position and rate can be
70 customized as needed or frozen by the selection of no integrator. The
71 selection of which integrator to use is done through the setting of
72 the associated property. There are four properties which can be set:
75 simulation/integrator/rate/rotational
76 simulation/integrator/rate/translational
77 simulation/integrator/position/rotational
78 simulation/integrator/position/translational
81 Each of the integrators listed above can be set to one of the following values:
84 0: No integrator (Freeze)
91 @author Jon S. Berndt, Mathias Froehlich
95 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
97 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
99 class FGPropagate : public FGModel {
102 /** The current vehicle state vector structure contains the translational and
103 angular position, and the translational and angular velocity. */
104 struct VehicleState {
105 /** Represents the current location of the vehicle in Earth centered Earth
108 FGLocation vLocation;
109 /** The velocity vector of the vehicle with respect to the ECEF frame,
110 expressed in the body system.
112 FGColumnVector3 vUVW;
113 /** The angular velocity vector for the vehicle relative to the ECEF frame,
114 expressed in the body frame.
116 FGColumnVector3 vPQR;
117 /** The current orientation of the vehicle, that is, the orientation of the
118 body frame relative to the local, vehilce-carried, NED frame. */
123 The constructor initializes several variables, and sets the initial set
124 of integrators to use as follows:
125 - integrator, rotational rate = Adams Bashforth 2
126 - integrator, translational rate = Adams Bashforth 2
127 - integrator, rotational position = Trapezoidal
128 - integrator, translational position = Trapezoidal
129 @param Executive a pointer to the parent executive object */
130 FGPropagate(FGFDMExec* Executive);
135 /// These define the indices use to select the various integrators.
136 enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3};
138 /** Initializes the FGPropagate class after instantiation and prior to first execution.
139 The base class FGModel::InitModel is called first, initializing pointers to the
140 other FGModel objects (and others). */
141 bool InitModel(void);
143 /** Runs the Propagate model; called by the Executive.
144 @return false if no error */
147 /** Retrieves the velocity vector.
148 The vector returned is represented by an FGColumnVector reference. The vector
149 for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
150 is 1-based, so that the first element can be retrieved using the "()" operator.
151 In other words, vVel(1) is Vnorth. Various convenience enumerators are defined
152 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
153 eNorth=1, eEast=2, eDown=3.
155 @return The vehicle velocity vector with respect to the Earth centered frame,
156 expressed in Local horizontal frame.
158 const FGColumnVector3& GetVel(void) const { return vVel; }
160 /** Retrieves the body frame vehicle velocity vector.
161 The vector returned is represented by an FGColumnVector reference. The vector
162 for the velocity in Body frame is organized (Vx, Vy, Vz). The vector
163 is 1-based, so that the first element can be retrieved using the "()" operator.
164 In other words, vUVW(1) is Vx. Various convenience enumerators are defined
165 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
168 @return The body frame vehicle velocity vector in ft/sec.
170 const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
172 /** Retrieves the body axis acceleration.
173 Retrieves the computed body axis accelerations based on the
174 applied forces and accounting for a rotating body frame.
175 The vector returned is represented by an FGColumnVector reference. The vector
176 for the acceleration in Body frame is organized (Ax, Ay, Az). The vector
177 is 1-based, so that the first element can be retrieved using the "()" operator.
178 In other words, vUVWdot(1) is Ax. Various convenience enumerators are defined
179 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
182 @return Body axis translational acceleration in ft/sec^2.
184 const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; }
186 /** Retrieves the body angular rates vector, relative to the ECEF frame.
187 Retrieves the body angular rates (p, q, r), which are calculated by integration
188 of the angular acceleration.
189 The vector returned is represented by an FGColumnVector reference. The vector
190 for the angular velocity in Body frame is organized (P, Q, R). The vector
191 is 1-based, so that the first element can be retrieved using the "()" operator.
192 In other words, vPQR(1) is P. Various convenience enumerators are defined
193 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
196 @return The body frame angular rates in rad/sec.
198 const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
200 /** Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
201 Retrieves the body angular rates (p, q, r), which are calculated by integration
202 of the angular acceleration.
203 The vector returned is represented by an FGColumnVector reference. The vector
204 for the angular velocity in Body frame is organized (P, Q, R). The vector
205 is 1-based, so that the first element can be retrieved using the "()" operator.
206 In other words, vPQR(1) is P. Various convenience enumerators are defined
207 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
210 @return The body frame angular rates in rad/sec.
212 const FGColumnVector3& GetPQRi(void) const {return vPQRi;}
214 /** Retrieves the body axis angular acceleration vector.
215 Retrieves the body axis angular acceleration vector in rad/sec^2. The
216 angular acceleration vector is determined from the applied forces and
217 accounts for a rotating frame.
218 The vector returned is represented by an FGColumnVector reference. The vector
219 for the angular acceleration in Body frame is organized (Pdot, Qdot, Rdot). The vector
220 is 1-based, so that the first element can be retrieved using the "()" operator.
221 In other words, vPQRdot(1) is Pdot. Various convenience enumerators are defined
222 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
225 @return The angular acceleration vector.
227 const FGColumnVector3& GetPQRdot(void) const {return vPQRdot;}
229 /** Retrieves the Euler angles that define the vehicle orientation.
230 Extracts the Euler angles from the quaternion that stores the orientation
231 in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
232 vector returned is represented by an FGColumnVector reference. The vector
233 for the Euler angles is organized (Phi, Theta, Psi). The vector
234 is 1-based, so that the first element can be retrieved using the "()" operator.
235 In other words, the returned vector item with subscript (1) is Phi.
236 Various convenience enumerators are defined in FGJSBBase. The relevant
237 enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
239 @return The Euler angle vector, where the first item in the
240 vector is the angle about the X axis, the second is the
241 angle about the Y axis, and the third item is the angle
242 about the Z axis (Phi, Theta, Psi).
244 const FGColumnVector3& GetEuler(void) const { return VState.vQtrn.GetEuler(); }
246 /** Retrieves a body frame velocity component.
247 Retrieves a body frame velocity component. The velocity returned is
248 extracted from the vUVW vector (an FGColumnVector). The vector for the
249 velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based.
250 In other words, GetUVW(1) returns Vx. Various convenience enumerators
251 are defined in FGJSBBase. The relevant enumerators for the velocity
252 returned by this call are, eX=1, eY=2, eZ=3.
254 @param idx the index of the velocity component desired (1-based).
255 @return The body frame velocity component.
257 double GetUVW (int idx) const { return VState.vUVW(idx); }
259 /** Retrieves a body frame acceleration component.
260 Retrieves a body frame acceleration component. The acceleration returned
261 is extracted from the vUVWdot vector (an FGColumnVector). The vector for
262 the acceleration in Body frame is organized (Ax, Ay, Az). The vector is
263 1-based. In other words, GetUVWdot(1) returns Ax. Various convenience
264 enumerators are defined in FGJSBBase. The relevant enumerators for the
265 acceleration returned by this call are, eX=1, eY=2, eZ=3.
267 @param idx the index of the acceleration component desired (1-based).
268 @return The body frame acceleration component.
270 double GetUVWdot(int idx) const { return vUVWdot(idx); }
272 /** Retrieves a Local frame velocity component.
273 Retrieves a Local frame velocity component. The velocity returned is
274 extracted from the vVel vector (an FGColumnVector). The vector for the
275 velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
276 is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience
277 enumerators are defined in FGJSBBase. The relevant enumerators for the
278 velocity returned by this call are, eNorth=1, eEast=2, eDown=3.
280 @param idx the index of the velocity component desired (1-based).
281 @return The body frame velocity component.
283 double GetVel(int idx) const { return vVel(idx); }
285 /** Retrieves the total inertial velocity in ft/sec.
287 double GetInertialVelocityMagnitude(void) const { return vInertialVelocity.Magnitude(); }
289 /** Returns the current altitude above sea level.
290 This function returns the altitude above sea level.
292 @return The current altitude above sea level in feet.
294 double GetAltitudeASL(void) const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
296 /** Returns the current altitude above sea level.
297 This function returns the altitude above sea level.
299 @return The current altitude above sea level in meters.
301 double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
303 /** Retrieves a body frame angular velocity component relative to the ECEF frame.
304 Retrieves a body frame angular velocity component. The angular velocity
305 returned is extracted from the vPQR vector (an FGColumnVector). The vector
306 for the angular velocity in Body frame is organized (P, Q, R). The vector
307 is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
308 convenience enumerators are defined in FGJSBBase. The relevant enumerators
309 for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
311 @param axis the index of the angular velocity component desired (1-based).
312 @return The body frame angular velocity component.
314 double GetPQR(int axis) const {return VState.vPQR(axis);}
316 /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame.
317 Retrieves a body frame angular velocity component. The angular velocity
318 returned is extracted from the vPQR vector (an FGColumnVector). The vector
319 for the angular velocity in Body frame is organized (P, Q, R). The vector
320 is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
321 convenience enumerators are defined in FGJSBBase. The relevant enumerators
322 for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
324 @param axis the index of the angular velocity component desired (1-based).
325 @return The body frame angular velocity component.
327 double GetPQRi(int axis) const {return vPQRi(axis);}
329 /** Retrieves a body frame angular acceleration component.
330 Retrieves a body frame angular acceleration component. The angular
331 acceleration returned is extracted from the vPQRdot vector (an
332 FGColumnVector). The vector for the angular acceleration in Body frame
333 is organized (Pdot, Qdot, Rdot). The vector is 1-based. In other words,
334 GetPQRdot(1) returns Pdot (roll acceleration). Various convenience
335 enumerators are defined in FGJSBBase. The relevant enumerators for the
336 angular acceleration returned by this call are, eP=1, eQ=2, eR=3.
338 @param axis the index of the angular acceleration component desired (1-based).
339 @return The body frame angular acceleration component.
341 double GetPQRdot(int axis) const {return vPQRdot(axis);}
343 /** Retrieves a vehicle Euler angle component.
344 Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
345 stores the vehicle orientation relative to the Local frame. The order of
346 rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
347 Phi. Various convenience enumerators are defined in FGJSBBase. The
348 relevant enumerators for the Euler angle returned by this call are,
349 ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
351 @return An Euler angle.
353 double GetEuler(int axis) const { return VState.vQtrn.GetEuler(axis); }
355 /** Retrieves the cosine of a vehicle Euler angle component.
356 Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
357 quaternion that stores the vehicle orientation relative to the Local frame.
358 The order of rotations used is Yaw-Pitch-Roll. The Euler angle
359 with subscript (1) is Phi. Various convenience enumerators are defined in
360 FGJSBBase. The relevant enumerators for the Euler angle referred to in this
361 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
363 @return The cosine of an Euler angle.
365 double GetCosEuler(int idx) const { return VState.vQtrn.GetCosEuler(idx); }
367 /** Retrieves the sine of a vehicle Euler angle component.
368 Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
369 quaternion that stores the vehicle orientation relative to the Local frame.
370 The order of rotations used is Yaw-Pitch-Roll. The Euler angle
371 with subscript (1) is Phi. Various convenience enumerators are defined in
372 FGJSBBase. The relevant enumerators for the Euler angle referred to in this
373 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
375 @return The sine of an Euler angle.
377 double GetSinEuler(int idx) const { return VState.vQtrn.GetSinEuler(idx); }
379 /** Returns the current altitude rate.
380 Returns the current altitude rate (rate of climb).
382 @return The current rate of change in altitude.
384 double Gethdot(void) const { return -vVel(eDown); }
386 /** Returns the "constant" LocalTerrainRadius.
387 The LocalTerrainRadius parameter is set by the calling application or set to
388 sea level + terrain elevation if JSBSim is running in standalone mode.
390 @return distance of the local terrain from the center of the earth.
392 double GetLocalTerrainRadius(void) const;
394 double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
395 double GetTerrainElevation(void) const;
396 double GetDistanceAGL(void) const;
397 double GetRadius(void) const {
398 if (VState.vLocation.GetRadius() == 0) return 1.0;
399 else return VState.vLocation.GetRadius();
401 double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
402 double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
404 double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
405 double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
407 double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
409 double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
410 double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
411 const FGLocation& GetLocation(void) const { return VState.vLocation; }
413 /** Retrieves the local-to-body transformation matrix.
414 The quaternion class, being the means by which the orientation of the
415 vehicle is stored, manages the local-to-body transformation matrix.
416 @return a reference to the local-to-body transformation matrix. */
417 const FGMatrix33& GetTl2b(void) const { return VState.vQtrn.GetT(); }
419 /** Retrieves the body-to-local transformation matrix.
420 The quaternion class, being the means by which the orientation of the
421 vehicle is stored, manages the body-to-local transformation matrix.
422 @return a reference to the body-to-local matrix. */
423 const FGMatrix33& GetTb2l(void) const { return VState.vQtrn.GetTInv(); }
425 /** Retrieves the ECEF-to-body transformation matrix.
426 @return a reference to the ECEF-to-body transformation matrix. */
427 const FGMatrix33& GetTec2b(void) const { return Tec2b; }
429 /** Retrieves the body-to-ECEF transformation matrix.
430 @return a reference to the body-to-ECEF matrix. */
431 const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
433 /** Retrieves the ECEF-to-ECI transformation matrix.
434 @return a reference to the ECEF-to-ECI transformation matrix. */
435 const FGMatrix33& GetTec2i(void);
437 /** Retrieves the ECI-to-ECEF transformation matrix.
438 @return a reference to the ECI-to-ECEF matrix. */
439 const FGMatrix33& GetTi2ec(void);
441 /** Retrieves the ECEF-to-local transformation matrix.
442 Retrieves the ECEF-to-local transformation matrix. Note that the so-called
443 local from is also know as the NED frame (for North, East, Down).
444 @return a reference to the ECEF-to-local matrix. */
445 const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); }
447 /** Retrieves the local-to-ECEF transformation matrix.
448 Retrieves the local-to-ECEF transformation matrix. Note that the so-called
449 local from is also know as the NED frame (for North, East, Down).
450 @return a reference to the local-to-ECEF matrix. */
451 const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
453 VehicleState* GetVState(void) { return &VState; }
455 void SetVState(VehicleState* vstate) {
456 VState.vLocation = vstate->vLocation;
457 VState.vUVW = vstate->vUVW;
458 VState.vPQR = vstate->vPQR;
459 VState.vQtrn = vstate->vQtrn; // ... mmh
462 const FGQuaternion GetQuaternion(void) const { return VState.vQtrn; }
464 void SetPQR(unsigned int i, double val) {
465 if ((i>=1) && (i<=3) )
466 VState.vPQR(i) = val;
469 void SetUVW(unsigned int i, double val) {
470 if ((i>=1) && (i<=3) )
471 VState.vUVW(i) = val;
476 void SetLongitude(double lon) { VState.vLocation.SetLongitude(lon); }
477 void SetLongitudeDeg(double lon) {SetLongitude(lon*degtorad);}
478 void SetLatitude(double lat) { VState.vLocation.SetLatitude(lat); }
479 void SetLatitudeDeg(double lat) {SetLatitude(lat*degtorad);}
480 void SetRadius(double r) { VState.vLocation.SetRadius(r); }
481 void SetLocation(const FGLocation& l) { VState.vLocation = l; }
482 void SetAltitudeASL(double altASL);
483 void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
484 void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
485 void SetTerrainElevation(double tt);
486 void SetDistanceAGL(double tt);
487 void SetInitialState(const FGInitialCondition *);
488 void RecomputeLocalTerrainRadius(void);
490 void CalculatePQRdot(void);
491 void CalculateQuatdot(void);
492 void CalculateLocationdot(void);
493 void CalculateUVWdot(void);
499 struct VehicleState VState;
501 FGColumnVector3 vVel;
502 FGColumnVector3 vInertialVelocity;
503 FGColumnVector3 vPQRdot, last_vPQRdot, last2_vPQRdot;
504 FGColumnVector3 vUVWdot, last_vUVWdot, last2_vUVWdot;
505 FGColumnVector3 vLocationDot, last_vLocationDot, last2_vLocationDot;
506 FGColumnVector3 vLocation;
507 FGColumnVector3 vPQRi; // Inertial frame angular velocity
508 FGColumnVector3 vOmega; // The Earth angular velocity vector
509 FGColumnVector3 vOmegaLocal; // The local frame angular velocity vector
510 FGQuaternion vQtrndot, last_vQtrndot, last2_vQtrndot;
513 FGMatrix33 Tl2b; // local to body frame matrix copy for immediate local use
514 FGMatrix33 Tb2l; // body to local frame matrix copy for immediate local use
515 FGMatrix33 Tl2ec; // local to ECEF matrix copy for immediate local use
516 FGMatrix33 Tec2l; // ECEF to local frame matrix copy for immediate local use
517 FGMatrix33 Tec2i; // ECEF to ECI frame matrix copy for immediate local use
518 FGMatrix33 Ti2ec; // ECI to ECEF frame matrix copy for immediate local use
519 FGMatrix33 Ti2b; // ECI to body frame rotation matrix
520 FGMatrix33 Tb2i; // body to ECI frame rotation matrix
522 double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
524 int integrator_rotational_rate;
525 int integrator_translational_rate;
526 int integrator_rotational_position;
527 int integrator_translational_position;
530 void Debug(int from);
533 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
535 #include <initialization/FGInitialCondition.h>