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.
290 Returns the current altitude. Specifically, this function returns the
291 difference between the distance to the center of the Earth, and sea level.
293 @return The current altitude above sea level in feet.
295 double Geth(void) const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
297 /** Returns the current altitude.
298 Returns the curren altitude. Specifically, this function returns the
299 difference between the distance to the center of the Earth, and sea level.
301 @return The current altitude above sea level in meters.
303 double Gethmeters(void) const { return Geth()*fttom;}
305 /** Retrieves a body frame angular velocity component relative to the ECEF frame.
306 Retrieves a body frame angular velocity component. The angular velocity
307 returned is extracted from the vPQR vector (an FGColumnVector). The vector
308 for the angular velocity in Body frame is organized (P, Q, R). The vector
309 is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
310 convenience enumerators are defined in FGJSBBase. The relevant enumerators
311 for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
313 @param axis the index of the angular velocity component desired (1-based).
314 @return The body frame angular velocity component.
316 double GetPQR(int axis) const {return VState.vPQR(axis);}
318 /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame.
319 Retrieves a body frame angular velocity component. The angular velocity
320 returned is extracted from the vPQR vector (an FGColumnVector). The vector
321 for the angular velocity in Body frame is organized (P, Q, R). The vector
322 is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
323 convenience enumerators are defined in FGJSBBase. The relevant enumerators
324 for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
326 @param axis the index of the angular velocity component desired (1-based).
327 @return The body frame angular velocity component.
329 double GetPQRi(int axis) const {return vPQRi(axis);}
331 /** Retrieves a body frame angular acceleration component.
332 Retrieves a body frame angular acceleration component. The angular
333 acceleration returned is extracted from the vPQRdot vector (an
334 FGColumnVector). The vector for the angular acceleration in Body frame
335 is organized (Pdot, Qdot, Rdot). The vector is 1-based. In other words,
336 GetPQRdot(1) returns Pdot (roll acceleration). Various convenience
337 enumerators are defined in FGJSBBase. The relevant enumerators for the
338 angular acceleration returned by this call are, eP=1, eQ=2, eR=3.
340 @param axis the index of the angular acceleration component desired (1-based).
341 @return The body frame angular acceleration component.
343 double GetPQRdot(int axis) const {return vPQRdot(axis);}
345 /** Retrieves a vehicle Euler angle component.
346 Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
347 stores the vehicle orientation relative to the Local frame. The order of
348 rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
349 Phi. Various convenience enumerators are defined in FGJSBBase. The
350 relevant enumerators for the Euler angle returned by this call are,
351 ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
353 @return An Euler angle.
355 double GetEuler(int axis) const { return VState.vQtrn.GetEuler(axis); }
357 /** Retrieves the cosine of a vehicle Euler angle component.
358 Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
359 quaternion that stores the vehicle orientation relative to the Local frame.
360 The order of rotations used is Yaw-Pitch-Roll. The Euler angle
361 with subscript (1) is Phi. Various convenience enumerators are defined in
362 FGJSBBase. The relevant enumerators for the Euler angle referred to in this
363 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
365 @return The cosine of an Euler angle.
367 double GetCosEuler(int idx) const { return VState.vQtrn.GetCosEuler(idx); }
369 /** Retrieves the sine of a vehicle Euler angle component.
370 Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
371 quaternion that stores the vehicle orientation relative to the Local frame.
372 The order of rotations used is Yaw-Pitch-Roll. The Euler angle
373 with subscript (1) is Phi. Various convenience enumerators are defined in
374 FGJSBBase. The relevant enumerators for the Euler angle referred to in this
375 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
377 @return The sine of an Euler angle.
379 double GetSinEuler(int idx) const { return VState.vQtrn.GetSinEuler(idx); }
381 /** Returns the current altitude rate.
382 Returns the current altitude rate (rate of climb).
384 @return The current rate of change in altitude.
386 double Gethdot(void) const { return -vVel(eDown); }
388 /** Returns the "constant" RunwayRadius.
389 The RunwayRadius parameter is set by the calling application or set to
390 sea level if JSBSim is running in standalone mode.
392 @return distance of the runway from the center of the earth.
394 double GetRunwayRadius(void) const;
396 double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
397 double GetTerrainElevationASL(void) const;
398 double GetDistanceAGL(void) const;
399 double GetRadius(void) const {
400 if (VState.vLocation.GetRadius() == 0) return 1.0;
401 else return VState.vLocation.GetRadius();
403 double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
404 double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
406 double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
407 double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
409 double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
411 double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
412 double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
413 const FGLocation& GetLocation(void) const { return VState.vLocation; }
415 /** Retrieves the local-to-body transformation matrix.
416 The quaternion class, being the means by which the orientation of the
417 vehicle is stored, manages the local-to-body transformation matrix.
418 @return a reference to the local-to-body transformation matrix. */
419 const FGMatrix33& GetTl2b(void) const { return VState.vQtrn.GetT(); }
421 /** Retrieves the body-to-local transformation matrix.
422 The quaternion class, being the means by which the orientation of the
423 vehicle is stored, manages the body-to-local transformation matrix.
424 @return a reference to the body-to-local matrix. */
425 const FGMatrix33& GetTb2l(void) const { return VState.vQtrn.GetTInv(); }
427 /** Retrieves the ECEF-to-body transformation matrix.
428 @return a reference to the ECEF-to-body transformation matrix. */
429 const FGMatrix33& GetTec2b(void) const { return Tec2b; }
431 /** Retrieves the body-to-ECEF transformation matrix.
432 @return a reference to the body-to-ECEF matrix. */
433 const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
435 /** Retrieves the ECEF-to-ECI transformation matrix.
436 @return a reference to the ECEF-to-ECI transformation matrix. */
437 const FGMatrix33& GetTec2i(void);
439 /** Retrieves the ECI-to-ECEF transformation matrix.
440 @return a reference to the ECI-to-ECEF matrix. */
441 const FGMatrix33& GetTi2ec(void);
443 /** Retrieves the ECEF-to-local transformation matrix.
444 Retrieves the ECEF-to-local transformation matrix. Note that the so-called
445 local from is also know as the NED frame (for North, East, Down).
446 @return a reference to the ECEF-to-local matrix. */
447 const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); }
449 /** Retrieves the local-to-ECEF transformation matrix.
450 Retrieves the local-to-ECEF transformation matrix. Note that the so-called
451 local from is also know as the NED frame (for North, East, Down).
452 @return a reference to the local-to-ECEF matrix. */
453 const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
455 VehicleState* GetVState(void) { return &VState; }
457 void SetVState(VehicleState* vstate) {
458 VState.vLocation = vstate->vLocation;
459 VState.vUVW = vstate->vUVW;
460 VState.vPQR = vstate->vPQR;
461 VState.vQtrn = vstate->vQtrn; // ... mmh
464 const FGQuaternion GetQuaternion(void) const { return VState.vQtrn; }
466 void SetPQR(unsigned int i, double val) {
467 if ((i>=1) && (i<=3) )
468 VState.vPQR(i) = val;
471 void SetUVW(unsigned int i, double val) {
472 if ((i>=1) && (i<=3) )
473 VState.vUVW(i) = val;
478 void SetLongitude(double lon) { VState.vLocation.SetLongitude(lon); }
479 void SetLongitudeDeg(double lon) {SetLongitude(lon*degtorad);}
480 void SetLatitude(double lat) { VState.vLocation.SetLatitude(lat); }
481 void SetLatitudeDeg(double lat) {SetLatitude(lat*degtorad);}
482 void SetRadius(double r) { VState.vLocation.SetRadius(r); }
483 void SetLocation(const FGLocation& l) { VState.vLocation = l; }
484 void Seth(double tt);
485 void Sethmeters(double tt) {Seth(tt/fttom);}
486 void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
487 void SetTerrainElevationASL(double tt);
488 void SetDistanceAGL(double tt);
489 void SetInitialState(const FGInitialCondition *);
490 void RecomputeRunwayRadius(void);
492 void CalculatePQRdot(void);
493 void CalculateQuatdot(void);
494 void CalculateLocationdot(void);
495 void CalculateUVWdot(void);
501 struct VehicleState VState;
503 FGColumnVector3 vVel;
504 FGColumnVector3 vInertialVelocity;
505 FGColumnVector3 vPQRdot, last_vPQRdot, last2_vPQRdot;
506 FGColumnVector3 vUVWdot, last_vUVWdot, last2_vUVWdot;
507 FGColumnVector3 vLocationDot, last_vLocationDot, last2_vLocationDot;
508 FGColumnVector3 vLocation;
509 FGColumnVector3 vPQRi; // Inertial frame angular velocity
510 FGColumnVector3 vOmega; // The Earth angular velocity vector
511 FGColumnVector3 vOmegaLocal; // The local frame angular velocity vector
512 FGQuaternion vQtrndot, last_vQtrndot, last2_vQtrndot;
515 FGMatrix33 Tl2b; // local to body frame matrix copy for immediate local use
516 FGMatrix33 Tb2l; // body to local frame matrix copy for immediate local use
517 FGMatrix33 Tl2ec; // local to ECEF matrix copy for immediate local use
518 FGMatrix33 Tec2l; // ECEF to local frame matrix copy for immediate local use
519 FGMatrix33 Tec2i; // ECEF to ECI frame matrix copy for immediate local use
520 FGMatrix33 Ti2ec; // ECI to ECEF frame matrix copy for immediate local use
521 FGMatrix33 Ti2b; // ECI to body frame rotation matrix
522 FGMatrix33 Tb2i; // body to ECI frame rotation matrix
524 double RunwayRadius, SeaLevelRadius, VehicleRadius;
526 int integrator_rotational_rate;
527 int integrator_translational_rate;
528 int integrator_rotational_position;
529 int integrator_translational_position;
532 void Debug(int from);
535 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
537 #include <initialization/FGInitialCondition.h>