Branch data Line data Source code
1 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 : :
3 : : Header: FGPropagate.h
4 : : Author: Jon S. Berndt
5 : : Date started: 1/5/99
6 : :
7 : : ------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) -------------
8 : :
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
12 : : version.
13 : :
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
17 : : details.
18 : :
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.
22 : :
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.
25 : :
26 : : HISTORY
27 : : --------------------------------------------------------------------------------
28 : : 01/05/99 JSB Created
29 : :
30 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
31 : : SENTRY
32 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
33 : :
34 : : #ifndef FGPROPAGATE_H
35 : : #define FGPROPAGATE_H
36 : :
37 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38 : : INCLUDES
39 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
40 : :
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"
46 : : #include <deque>
47 : :
48 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
49 : : DEFINITIONS
50 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
51 : :
52 : : #define ID_PROPAGATE "$Id: FGPropagate.h,v 1.43 2010/07/25 15:35:11 jberndt Exp $"
53 : :
54 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
55 : : FORWARD DECLARATIONS
56 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
57 : :
58 : : namespace JSBSim {
59 : :
60 : : using std::deque;
61 : : class FGInitialCondition;
62 : :
63 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
64 : : CLASS DOCUMENTATION
65 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
66 : :
67 : : /** Models the EOM and integration/propagation of state.
68 : : The Equations of Motion (EOM) for JSBSim are integrated to propagate the
69 : : state of the vehicle given the forces and moments that act on it. The
70 : : integration accounts for a rotating Earth.
71 : :
72 : : The general execution of this model follows this process:
73 : :
74 : : -Calculate the angular accelerations
75 : : -Calculate the translational accelerations
76 : : -Calculate the angular rate
77 : : -Calculate the translational velocity
78 : :
79 : : -Integrate accelerations and rates
80 : :
81 : : Integration of rotational and translation position and rate can be
82 : : customized as needed or frozen by the selection of no integrator. The
83 : : selection of which integrator to use is done through the setting of
84 : : the associated property. There are four properties which can be set:
85 : :
86 : : @code
87 : : simulation/integrator/rate/rotational
88 : : simulation/integrator/rate/translational
89 : : simulation/integrator/position/rotational
90 : : simulation/integrator/position/translational
91 : : @endcode
92 : :
93 : : Each of the integrators listed above can be set to one of the following values:
94 : :
95 : : @code
96 : : 0: No integrator (Freeze)
97 : : 1: Rectangular Euler
98 : : 2: Trapezoidal
99 : : 3: Adams Bashforth 2
100 : : 4: Adams Bashforth 3
101 : : 5: Adams Bashforth 4
102 : : @endcode
103 : :
104 : : @author Jon S. Berndt, Mathias Froehlich
105 : : @version $Id: FGPropagate.h,v 1.43 2010/07/25 15:35:11 jberndt Exp $
106 : : */
107 : :
108 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
109 : : CLASS DECLARATION
110 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
111 : :
112 : : class FGPropagate : public FGModel {
113 : : public:
114 : :
115 : : /** The current vehicle state vector structure contains the translational and
116 : : angular position, and the translational and angular velocity. */
117 : 4 : struct VehicleState {
118 : : /** Represents the current location of the vehicle in Earth centered Earth
119 : : fixed (ECEF) frame.
120 : : units ft */
121 : : FGLocation vLocation;
122 : :
123 : : /** The velocity vector of the vehicle with respect to the ECEF frame,
124 : : expressed in the body system.
125 : : units ft/sec */
126 : : FGColumnVector3 vUVW;
127 : :
128 : : /** The angular velocity vector for the vehicle relative to the ECEF frame,
129 : : expressed in the body frame.
130 : : units rad/sec */
131 : : FGColumnVector3 vPQR;
132 : :
133 : : /** The angular velocity vector for the vehicle body frame relative to the
134 : : ECI frame, expressed in the body frame.
135 : : units rad/sec */
136 : : FGColumnVector3 vPQRi;
137 : :
138 : : /** The current orientation of the vehicle, that is, the orientation of the
139 : : body frame relative to the local, NED frame. */
140 : : FGQuaternion qAttitudeLocal;
141 : :
142 : : /** The current orientation of the vehicle, that is, the orientation of the
143 : : body frame relative to the inertial (ECI) frame. */
144 : : FGQuaternion qAttitudeECI;
145 : :
146 : : FGColumnVector3 vInertialVelocity;
147 : :
148 : : FGColumnVector3 vInertialPosition;
149 : :
150 : : deque <FGColumnVector3> dqPQRdot;
151 : : deque <FGColumnVector3> dqUVWdot;
152 : : deque <FGColumnVector3> dqInertialVelocity;
153 : : deque <FGQuaternion> dqQtrndot;
154 : : };
155 : :
156 : : /** Constructor.
157 : : The constructor initializes several variables, and sets the initial set
158 : : of integrators to use as follows:
159 : : - integrator, rotational rate = Adams Bashforth 2
160 : : - integrator, translational rate = Adams Bashforth 2
161 : : - integrator, rotational position = Trapezoidal
162 : : - integrator, translational position = Trapezoidal
163 : : @param Executive a pointer to the parent executive object */
164 : : FGPropagate(FGFDMExec* Executive);
165 : :
166 : : /// Destructor
167 : : ~FGPropagate();
168 : :
169 : : /// These define the indices use to select the various integrators.
170 : : enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
171 : :
172 : : /// These define the indices use to select the gravitation models.
173 : : enum eGravType {gtStandard, gtWGS84};
174 : :
175 : : /** Initializes the FGPropagate class after instantiation and prior to first execution.
176 : : The base class FGModel::InitModel is called first, initializing pointers to the
177 : : other FGModel objects (and others). */
178 : : bool InitModel(void);
179 : :
180 : : /** Runs the Propagate model; called by the Executive.
181 : : @return false if no error */
182 : : bool Run(void);
183 : :
184 : : const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
185 : :
186 : : /** Retrieves the velocity vector.
187 : : The vector returned is represented by an FGColumnVector reference. The vector
188 : : for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
189 : : is 1-based, so that the first element can be retrieved using the "()" operator.
190 : : In other words, vVel(1) is Vnorth. Various convenience enumerators are defined
191 : : in FGJSBBase. The relevant enumerators for the vector returned by this call are,
192 : : eNorth=1, eEast=2, eDown=3.
193 : : units ft/sec
194 : : @return The vehicle velocity vector with respect to the Earth centered frame,
195 : : expressed in Local horizontal frame.
196 : : */
197 : 58914 : const FGColumnVector3& GetVel(void) const { return vVel; }
198 : :
199 : : /** Retrieves the body frame vehicle velocity vector.
200 : : The vector returned is represented by an FGColumnVector reference. The vector
201 : : for the velocity in Body frame is organized (Vx, Vy, Vz). The vector
202 : : is 1-based, so that the first element can be retrieved using the "()" operator.
203 : : In other words, vUVW(1) is Vx. Various convenience enumerators are defined
204 : : in FGJSBBase. The relevant enumerators for the vector returned by this call are,
205 : : eX=1, eY=2, eZ=3.
206 : : units ft/sec
207 : : @return The body frame vehicle velocity vector in ft/sec.
208 : : */
209 : 58915 : const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
210 : :
211 : : /** Retrieves the body axis acceleration.
212 : : Retrieves the computed body axis accelerations based on the
213 : : applied forces and accounting for a rotating body frame.
214 : : The vector returned is represented by an FGColumnVector reference. The vector
215 : : for the acceleration in Body frame is organized (Ax, Ay, Az). The vector
216 : : is 1-based, so that the first element can be retrieved using the "()" operator.
217 : : In other words, vUVWdot(1) is Ax. Various convenience enumerators are defined
218 : : in FGJSBBase. The relevant enumerators for the vector returned by this call are,
219 : : eX=1, eY=2, eZ=3.
220 : : units ft/sec^2
221 : : @return Body axis translational acceleration in ft/sec^2.
222 : : */
223 : 54005 : const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; }
224 : :
225 : : /** Retrieves the body angular rates vector, relative to the ECEF frame.
226 : : Retrieves the body angular rates (p, q, r), which are calculated by integration
227 : : of the angular acceleration.
228 : : The vector returned is represented by an FGColumnVector reference. The vector
229 : : for the angular velocity in Body frame is organized (P, Q, R). The vector
230 : : is 1-based, so that the first element can be retrieved using the "()" operator.
231 : : In other words, vPQR(1) is P. Various convenience enumerators are defined
232 : : in FGJSBBase. The relevant enumerators for the vector returned by this call are,
233 : : eP=1, eQ=2, eR=3.
234 : : units rad/sec
235 : : @return The body frame angular rates in rad/sec.
236 : : */
237 : 58914 : const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
238 : :
239 : : /** Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
240 : : Retrieves the body angular rates (p, q, r), which are calculated by integration
241 : : of the angular acceleration.
242 : : The vector returned is represented by an FGColumnVector reference. The vector
243 : : for the angular velocity in Body frame is organized (P, Q, R). The vector
244 : : is 1-based, so that the first element can be retrieved using the "()" operator.
245 : : In other words, vPQR(1) is P. Various convenience enumerators are defined
246 : : in FGJSBBase. The relevant enumerators for the vector returned by this call are,
247 : : eP=1, eQ=2, eR=3.
248 : : units rad/sec
249 : : @return The body frame inertial angular rates in rad/sec.
250 : : */
251 : 4909 : const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
252 : :
253 : : /** Retrieves the body axis angular acceleration vector.
254 : : Retrieves the body axis angular acceleration vector in rad/sec^2. The
255 : : angular acceleration vector is determined from the applied forces and
256 : : accounts for a rotating frame.
257 : : The vector returned is represented by an FGColumnVector reference. The vector
258 : : for the angular acceleration in Body frame is organized (Pdot, Qdot, Rdot). The vector
259 : : is 1-based, so that the first element can be retrieved using the "()" operator.
260 : : In other words, vPQRdot(1) is Pdot. Various convenience enumerators are defined
261 : : in FGJSBBase. The relevant enumerators for the vector returned by this call are,
262 : : eP=1, eQ=2, eR=3.
263 : : units rad/sec^2
264 : : @return The angular acceleration vector.
265 : : */
266 : 58012 : const FGColumnVector3& GetPQRdot(void) const {return vPQRdot;}
267 : :
268 : : /** Retrieves the Euler angles that define the vehicle orientation.
269 : : Extracts the Euler angles from the quaternion that stores the orientation
270 : : in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
271 : : vector returned is represented by an FGColumnVector reference. The vector
272 : : for the Euler angles is organized (Phi, Theta, Psi). The vector
273 : : is 1-based, so that the first element can be retrieved using the "()" operator.
274 : : In other words, the returned vector item with subscript (1) is Phi.
275 : : Various convenience enumerators are defined in FGJSBBase. The relevant
276 : : enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
277 : : units radians
278 : : @return The Euler angle vector, where the first item in the
279 : : vector is the angle about the X axis, the second is the
280 : : angle about the Y axis, and the third item is the angle
281 : : about the Z axis (Phi, Theta, Psi).
282 : : */
283 : 9818 : const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
284 : :
285 : : /** Retrieves a body frame velocity component.
286 : : Retrieves a body frame velocity component. The velocity returned is
287 : : extracted from the vUVW vector (an FGColumnVector). The vector for the
288 : : velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based.
289 : : In other words, GetUVW(1) returns Vx. Various convenience enumerators
290 : : are defined in FGJSBBase. The relevant enumerators for the velocity
291 : : returned by this call are, eX=1, eY=2, eZ=3.
292 : : units ft/sec
293 : : @param idx the index of the velocity component desired (1-based).
294 : : @return The body frame velocity component.
295 : : */
296 : 3 : double GetUVW (int idx) const { return VState.vUVW(idx); }
297 : :
298 : : /** Retrieves a body frame acceleration component.
299 : : Retrieves a body frame acceleration component. The acceleration returned
300 : : is extracted from the vUVWdot vector (an FGColumnVector). The vector for
301 : : the acceleration in Body frame is organized (Ax, Ay, Az). The vector is
302 : : 1-based. In other words, GetUVWdot(1) returns Ax. Various convenience
303 : : enumerators are defined in FGJSBBase. The relevant enumerators for the
304 : : acceleration returned by this call are, eX=1, eY=2, eZ=3.
305 : : units ft/sec^2
306 : : @param idx the index of the acceleration component desired (1-based).
307 : : @return The body frame acceleration component.
308 : : */
309 : 3 : double GetUVWdot(int idx) const { return vUVWdot(idx); }
310 : :
311 : : /** Retrieves a Local frame velocity component.
312 : : Retrieves a Local frame velocity component. The velocity returned is
313 : : extracted from the vVel vector (an FGColumnVector). The vector for the
314 : : velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
315 : : is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience
316 : : enumerators are defined in FGJSBBase. The relevant enumerators for the
317 : : velocity returned by this call are, eNorth=1, eEast=2, eDown=3.
318 : : units ft/sec
319 : : @param idx the index of the velocity component desired (1-based).
320 : : @return The body frame velocity component.
321 : : */
322 : 3 : double GetVel(int idx) const { return vVel(idx); }
323 : :
324 : : /** Retrieves the total inertial velocity in ft/sec.
325 : : */
326 : 166925 : double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
327 : :
328 : : /** Retrieves the inertial velocity vector in ft/sec.
329 : : */
330 : 4909 : const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
331 : :
332 : : /** Retrieves the inertial position vector.
333 : : */
334 : 4909 : const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
335 : :
336 : : /** Returns the current altitude above sea level.
337 : : This function returns the altitude above sea level.
338 : : units ft
339 : : @return The current altitude above sea level in feet.
340 : : */
341 : 117836 : double GetAltitudeASL(void) const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
342 : :
343 : : /** Returns the current altitude above sea level.
344 : : This function returns the altitude above sea level.
345 : : units meters
346 : : @return The current altitude above sea level in meters.
347 : : */
348 : 1 : double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
349 : :
350 : : /** Retrieves a body frame angular velocity component relative to the ECEF frame.
351 : : Retrieves a body frame angular velocity component. The angular velocity
352 : : returned is extracted from the vPQR vector (an FGColumnVector). The vector
353 : : for the angular velocity in Body frame is organized (P, Q, R). The vector
354 : : is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
355 : : convenience enumerators are defined in FGJSBBase. The relevant enumerators
356 : : for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
357 : : units rad/sec
358 : : @param axis the index of the angular velocity component desired (1-based).
359 : : @return The body frame angular velocity component.
360 : : */
361 : 216023 : double GetPQR(int axis) const {return VState.vPQR(axis);}
362 : :
363 : : /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame.
364 : : Retrieves a body frame angular velocity component. The angular velocity
365 : : returned is extracted from the vPQR vector (an FGColumnVector). The vector
366 : : for the angular velocity in Body frame is organized (P, Q, R). The vector
367 : : is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
368 : : convenience enumerators are defined in FGJSBBase. The relevant enumerators
369 : : for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
370 : : units rad/sec
371 : : @param axis the index of the angular velocity component desired (1-based).
372 : : @return The body frame angular velocity component.
373 : : */
374 : 3 : double GetPQRi(int axis) const {return VState.vPQRi(axis);}
375 : :
376 : : /** Retrieves a body frame angular acceleration component.
377 : : Retrieves a body frame angular acceleration component. The angular
378 : : acceleration returned is extracted from the vPQRdot vector (an
379 : : FGColumnVector). The vector for the angular acceleration in Body frame
380 : : is organized (Pdot, Qdot, Rdot). The vector is 1-based. In other words,
381 : : GetPQRdot(1) returns Pdot (roll acceleration). Various convenience
382 : : enumerators are defined in FGJSBBase. The relevant enumerators for the
383 : : angular acceleration returned by this call are, eP=1, eQ=2, eR=3.
384 : : units rad/sec^2
385 : : @param axis the index of the angular acceleration component desired (1-based).
386 : : @return The body frame angular acceleration component.
387 : : */
388 : 3 : double GetPQRdot(int axis) const {return vPQRdot(axis);}
389 : :
390 : : /** Retrieves a vehicle Euler angle component.
391 : : Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
392 : : stores the vehicle orientation relative to the Local frame. The order of
393 : : rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
394 : : Phi. Various convenience enumerators are defined in FGJSBBase. The
395 : : relevant enumerators for the Euler angle returned by this call are,
396 : : ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
397 : : units radians
398 : : @return An Euler angle.
399 : : */
400 : 54146 : double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
401 : :
402 : : /** Retrieves the cosine of a vehicle Euler angle component.
403 : : Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
404 : : quaternion that stores the vehicle orientation relative to the Local frame.
405 : : The order of rotations used is Yaw-Pitch-Roll. The Euler angle
406 : : with subscript (1) is Phi. Various convenience enumerators are defined in
407 : : FGJSBBase. The relevant enumerators for the Euler angle referred to in this
408 : : call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
409 : : units none
410 : : @return The cosine of an Euler angle.
411 : : */
412 : 216020 : double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
413 : :
414 : : /** Retrieves the sine of a vehicle Euler angle component.
415 : : Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
416 : : quaternion that stores the vehicle orientation relative to the Local frame.
417 : : The order of rotations used is Yaw-Pitch-Roll. The Euler angle
418 : : with subscript (1) is Phi. Various convenience enumerators are defined in
419 : : FGJSBBase. The relevant enumerators for the Euler angle referred to in this
420 : : call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
421 : : units none
422 : : @return The sine of an Euler angle.
423 : : */
424 : 216020 : double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
425 : :
426 : : /** Returns the current altitude rate.
427 : : Returns the current altitude rate (rate of climb).
428 : : units ft/sec
429 : : @return The current rate of change in altitude.
430 : : */
431 : 1 : double Gethdot(void) const { return -vVel(eDown); }
432 : :
433 : : /** Returns the "constant" LocalTerrainRadius.
434 : : The LocalTerrainRadius parameter is set by the calling application or set to
435 : : sea level + terrain elevation if JSBSim is running in standalone mode.
436 : : units feet
437 : : @return distance of the local terrain from the center of the earth.
438 : : */
439 : : double GetLocalTerrainRadius(void) const;
440 : :
441 : : double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
442 : : double GetTerrainElevation(void) const;
443 : : double GetDistanceAGL(void) const;
444 : 324032 : double GetRadius(void) const {
445 [ - + ][ - + ]: 378037 : if (VState.vLocation.GetRadius() == 0) return 1.0;
446 : 378037 : else return VState.vLocation.GetRadius();
447 : : }
448 : 108011 : double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
449 : 216023 : double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
450 : :
451 : 1 : double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
452 : 1 : double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
453 : :
454 : 1 : double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
455 : :
456 : 1 : double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
457 : 1 : double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
458 : 68732 : const FGLocation& GetLocation(void) const { return VState.vLocation; }
459 : :
460 : : /** Retrieves the local-to-body transformation matrix.
461 : : The quaternion class, being the means by which the orientation of the
462 : : vehicle is stored, manages the local-to-body transformation matrix.
463 : : @return a reference to the local-to-body transformation matrix. */
464 : 112724 : const FGMatrix33& GetTl2b(void) const { return VState.qAttitudeLocal.GetT(); }
465 : :
466 : : /** Retrieves the body-to-local transformation matrix.
467 : : The quaternion class, being the means by which the orientation of the
468 : : vehicle is stored, manages the body-to-local transformation matrix.
469 : : @return a reference to the body-to-local matrix. */
470 : 216020 : const FGMatrix33& GetTb2l(void) const { return VState.qAttitudeLocal.GetTInv(); }
471 : :
472 : : /** Retrieves the ECEF-to-body transformation matrix.
473 : : @return a reference to the ECEF-to-body transformation matrix. */
474 : 0 : const FGMatrix33& GetTec2b(void) const { return Tec2b; }
475 : :
476 : : /** Retrieves the body-to-ECEF transformation matrix.
477 : : @return a reference to the body-to-ECEF matrix. */
478 : 54006 : const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
479 : :
480 : : /** Retrieves the ECI-to-body transformation matrix.
481 : : @return a reference to the ECI-to-body transformation matrix. */
482 : 108012 : const FGMatrix33& GetTi2b(void) const { return VState.qAttitudeECI.GetT(); }
483 : :
484 : : /** Retrieves the body-to-ECI transformation matrix.
485 : : @return a reference to the body-to-ECI matrix. */
486 : 0 : const FGMatrix33& GetTb2i(void) const { return VState.qAttitudeECI.GetTInv(); }
487 : :
488 : : /** Retrieves the ECEF-to-ECI transformation matrix.
489 : : @return a reference to the ECEF-to-ECI transformation matrix. */
490 : : const FGMatrix33& GetTec2i(void);
491 : :
492 : : /** Retrieves the ECI-to-ECEF transformation matrix.
493 : : @return a reference to the ECI-to-ECEF matrix. */
494 : : const FGMatrix33& GetTi2ec(void);
495 : :
496 : : /** Retrieves the ECEF-to-local transformation matrix.
497 : : Retrieves the ECEF-to-local transformation matrix. Note that the so-called
498 : : local from is also know as the NED frame (for North, East, Down).
499 : : @return a reference to the ECEF-to-local matrix. */
500 : 0 : const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); }
501 : :
502 : : /** Retrieves the local-to-ECEF transformation matrix.
503 : : Retrieves the local-to-ECEF transformation matrix. Note that the so-called
504 : : local from is also know as the NED frame (for North, East, Down).
505 : : @return a reference to the local-to-ECEF matrix. */
506 : 108012 : const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
507 : :
508 : : /** Retrieves the local-to-inertial transformation matrix.
509 : : @return a reference to the local-to-inertial transformation matrix. */
510 : 0 : const FGMatrix33& GetTl2i(void) { return VState.vLocation.GetTl2i(); }
511 : :
512 : : /** Retrieves the inertial-to-local transformation matrix.
513 : : @return a reference to the inertial-to-local matrix. */
514 : 108012 : const FGMatrix33& GetTi2l(void) { return VState.vLocation.GetTi2l(); }
515 : :
516 : 0 : VehicleState* GetVState(void) { return &VState; }
517 : :
518 : 0 : void SetVState(VehicleState* vstate) {
519 : 0 : VState.vLocation = vstate->vLocation;
520 : 0 : VState.vUVW = vstate->vUVW;
521 : 0 : VState.vPQR = vstate->vPQR;
522 : 0 : VState.qAttitudeLocal = vstate->qAttitudeLocal;
523 : 0 : VState.qAttitudeECI = vstate->qAttitudeECI;
524 : :
525 : 0 : VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
526 : 0 : VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
527 : 0 : VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
528 : 0 : VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
529 : 0 : }
530 : :
531 : : void SetInertialOrientation(FGQuaternion Qi);
532 : : void SetInertialVelocity(FGColumnVector3 Vi);
533 : :
534 : : const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
535 : :
536 : : void SetPQR(unsigned int i, double val) {
537 : : if ((i>=1) && (i<=3) )
538 : : VState.vPQR(i) = val;
539 : : }
540 : :
541 : : void SetUVW(unsigned int i, double val) {
542 : : if ((i>=1) && (i<=3) )
543 : : VState.vUVW(i) = val;
544 : : }
545 : :
546 : : // SET functions
547 : :
548 : 0 : void SetLongitude(double lon) { VState.vLocation.SetLongitude(lon); }
549 : 0 : void SetLongitudeDeg(double lon) {SetLongitude(lon*degtorad);}
550 : 0 : void SetLatitude(double lat) { VState.vLocation.SetLatitude(lat); }
551 : 0 : void SetLatitudeDeg(double lat) {SetLatitude(lat*degtorad);}
552 : : void SetRadius(double r) { VState.vLocation.SetRadius(r); }
553 : : void SetLocation(const FGLocation& l) { VState.vLocation = l; }
554 : : void SetAltitudeASL(double altASL);
555 : 0 : void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
556 : 1 : void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
557 : : void SetTerrainElevation(double tt);
558 : : void SetDistanceAGL(double tt);
559 : : void SetInitialState(const FGInitialCondition *);
560 : : void RecomputeLocalTerrainRadius(void);
561 : :
562 : : void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
563 : 54006 : vDeltaXYZEC = GetTb2ec()*deltaLoc;
564 : 162018 : VState.vLocation -= vDeltaXYZEC;
565 : : }
566 : :
567 : 0 : struct LagrangeMultiplier {
568 : : FGColumnVector3 ForceJacobian;
569 : : FGColumnVector3 MomentJacobian;
570 : : double Min;
571 : : double Max;
572 : : double value;
573 : : };
574 : :
575 : : private:
576 : :
577 : : // state vector
578 : :
579 : : struct VehicleState VState;
580 : :
581 : : FGColumnVector3 vVel;
582 : : FGColumnVector3 vPQRdot;
583 : : FGColumnVector3 vUVWdot;
584 : : FGColumnVector3 vInertialVelocity;
585 : : FGColumnVector3 vLocation;
586 : : FGColumnVector3 vDeltaXYZEC;
587 : : FGColumnVector3 vGravAccel;
588 : : FGColumnVector3 vOmegaEarth; // The Earth angular velocity vector
589 : : FGQuaternion vQtrndot;
590 : : FGMatrix33 Tec2b;
591 : : FGMatrix33 Tb2ec;
592 : : FGMatrix33 Tl2b; // local to body frame matrix copy for immediate local use
593 : : FGMatrix33 Tb2l; // body to local frame matrix copy for immediate local use
594 : : FGMatrix33 Tl2ec; // local to ECEF matrix copy for immediate local use
595 : : FGMatrix33 Tec2l; // ECEF to local frame matrix copy for immediate local use
596 : : FGMatrix33 Tec2i; // ECEF to ECI frame matrix copy for immediate local use
597 : : FGMatrix33 Ti2ec; // ECI to ECEF frame matrix copy for immediate local use
598 : : FGMatrix33 Ti2b; // ECI to body frame rotation matrix
599 : : FGMatrix33 Tb2i; // body to ECI frame rotation matrix
600 : : FGMatrix33 Ti2l;
601 : : FGMatrix33 Tl2i;
602 : : FGLocation contactloc;
603 : : FGColumnVector3 dv;
604 : :
605 : : double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
606 : : double radInv;
607 : : eIntegrateType integrator_rotational_rate;
608 : : eIntegrateType integrator_translational_rate;
609 : : eIntegrateType integrator_rotational_position;
610 : : eIntegrateType integrator_translational_position;
611 : : int gravType;
612 : :
613 : : void CalculatePQRdot(void);
614 : : void CalculateQuatdot(void);
615 : : void CalculateInertialVelocity(void);
616 : : void CalculateUVWdot(void);
617 : :
618 : : void Integrate( FGColumnVector3& Integrand,
619 : : FGColumnVector3& Val,
620 : : deque <FGColumnVector3>& ValDot,
621 : : double dt,
622 : : eIntegrateType integration_type);
623 : :
624 : : void Integrate( FGQuaternion& Integrand,
625 : : FGQuaternion& Val,
626 : : deque <FGQuaternion>& ValDot,
627 : : double dt,
628 : : eIntegrateType integration_type);
629 : :
630 : : void ResolveFrictionForces(double dt);
631 : :
632 : : void bind(void);
633 : : void Debug(int from);
634 : : };
635 : : }
636 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
637 : :
638 : : #include "initialization/FGInitialCondition.h"
639 : :
640 : : #endif
|
