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 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 General Public License for more
19 You should have received a copy of the GNU 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 General Public License can also be found on
24 the world wide web at http://www.gnu.org.
26 FUNCTIONAL DESCRIPTION
27 --------------------------------------------------------------------------------
30 --------------------------------------------------------------------------------
33 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
40 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
42 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
45 # include <simgear/compiler.h>
46 # ifdef SG_HAVE_STD_INCLUDES
52 # if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
61 #include "FGJSBBase.h"
62 #include "FGInitialCondition.h"
63 #include "FGMatrix33.h"
64 #include "FGColumnVector3.h"
65 #include "FGColumnVector4.h"
67 #include "FGFDMExec.h"
68 #include "FGAtmosphere.h"
70 #include "FGTranslation.h"
71 #include "FGRotation.h"
72 #include "FGPosition.h"
73 #include "FGAerodynamics.h"
75 #include "FGAircraft.h"
76 #include "FGGroundReactions.h"
77 #include "FGPropulsion.h"
79 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
81 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
83 #define ID_STATE "$Id$"
85 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
87 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
91 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
93 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
95 /** Encapsulates the calculation of aircraft state.
100 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
102 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
104 class FGState : public FGJSBBase
108 @param Executive a pointer to the parent executive object */
113 /** Initializes the simulation state based on the passed-in parameters.
114 @param U the body X-Axis velocity in fps.
115 @param V the body Y-Axis velocity in fps.
116 @param W the body Z-Axis velocity in fps.
117 @param lat latitude measured in radians from the equator, negative values are south.
118 @param lon longitude, measured in radians from the Greenwich meridian, negative values are west.
119 @param phi the roll angle in radians.
120 @param tht the pitch angle in radians.
121 @param psi the heading angle in radians measured clockwise from north.
122 @param h altitude in feet.
123 @param wnorth north velocity in feet per second
124 @param weast eastward velocity in feet per second
125 @param wdown downward velocity in feet per second
127 void Initialize(double U,
140 /** Initializes the simulation state based on parameters from an Initial Conditions object.
141 @param FGIC pointer to an initial conditions object.
142 @see FGInitialConditions.
144 void Initialize(FGInitialCondition *FGIC);
146 /// Returns the simulation time in seconds.
147 inline double Getsim_time(void) const { return sim_time; }
148 /// Returns the simulation delta T.
149 inline double Getdt(void) { return dt; }
151 /// Suspends the simulation and sets the delta T to zero.
152 inline void Suspend(void) {saved_dt = dt; dt = 0.0;}
153 /// Resumes the simulation by resetting delta T to the correct value.
154 inline void Resume(void) {dt = saved_dt;}
156 /** Sets the current sim time.
157 @param cur_time the current time
158 @return the current time.
160 inline double Setsim_time(double cur_time) {
165 /** Sets the integration time step for the simulation executive.
166 @param delta_t the time step in seconds.
168 inline void Setdt(double delta_t) { dt = delta_t; }
170 /** Increments the simulation time.
171 @return the new simulation time.
173 inline double IncrTime(void) {
178 /** Initializes the transformation matrices.
179 @param phi the roll angle in radians.
180 @param tht the pitch angle in radians.
181 @param psi the heading angle in radians
183 void InitMatrices(double phi, double tht, double psi);
185 /** Calculates the local-to-body and body-to-local conversion matrices.
187 void CalcMatrices(void);
189 /** Integrates the quaternion.
190 Given the supplied rotational rate vector and integration rate, the quaternion
191 is integrated. The quaternion is later used to update the transformation
193 @param vPQR the body rotational rate column vector.
194 @param rate the integration rate in seconds.
196 void IntegrateQuat(FGColumnVector3 vPQR, int rate);
198 // ======================================= General Purpose INTEGRATOR
200 enum iType {AB4, AB3, AB2, AM3, AM4, EULER, TRAPZ};
202 /** Multi-method integrator.
203 @param type Type of intergation scheme to use. Can be one of:
205 <li>AB4 - Adams-Bashforth, fourth order</li>
206 <li>AB3 - Adams-Bashforth, third order</li>
207 <li>AB2 - Adams-Bashforth, second order</li>
208 <li>AM3 - Adams Moulton, third order</li>
209 <li>AM4 - Adams Moulton, fourth order</li>
210 <li>EULER - Euler</li>
211 <li>TRAPZ - Trapezoidal</li>
213 @param delta_t the integration time step in seconds
214 @param vTDeriv a reference to the current value of the time derivative of
215 the quantity being integrated (i.e. if vUVW is being integrated
216 vTDeriv is the current value of vUVWdot)
217 @param vLastArray an array of previously calculated and saved values of
218 the quantity being integrated (i.e. if vUVW is being integrated
219 vLastArray[0] is the past value of vUVWdot, vLastArray[1] is the value of
220 vUVWdot prior to that, etc.)
221 @return the current, incremental value of the item integrated to add to the
224 template <class T> T Integrate(iType type, double delta_t, T& vTDeriv, T *vLastArray)
230 vResult = (delta_t/24.0)*( 55.0 * vLastArray[0]
231 - 59.0 * vLastArray[1]
232 + 37.0 * vLastArray[2]
233 - 9.0 * vLastArray[3] );
234 vLastArray[3] = vLastArray[2];
235 vLastArray[2] = vLastArray[1];
236 vLastArray[1] = vLastArray[0];
237 vLastArray[0] = vTDeriv;
240 vResult = (delta_t/12.0)*( 23.0 * vLastArray[0]
241 - 16.0 * vLastArray[1]
242 + 5.0 * vLastArray[2] );
243 vLastArray[2] = vLastArray[1];
244 vLastArray[1] = vLastArray[0];
245 vLastArray[0] = vTDeriv;
248 vResult = (delta_t/2.0)*( 3.0 * vLastArray[0] - vLastArray[1] );
249 vLastArray[1] = vLastArray[0];
250 vLastArray[0] = vTDeriv;
253 vResult = (delta_t/24.0)*( 9.0 * vTDeriv
254 + 19.0 * vLastArray[0]
255 - 5.0 * vLastArray[1]
256 + 1.0 * vLastArray[2] );
257 vLastArray[2] = vLastArray[1];
258 vLastArray[1] = vLastArray[0];
259 vLastArray[0] = vTDeriv;
262 vResult = (delta_t/12.0)*( 5.0 * vTDeriv
263 + 8.0 * vLastArray[0]
264 - 1.0 * vLastArray[1] );
265 vLastArray[1] = vLastArray[0];
266 vLastArray[0] = vTDeriv;
269 vResult = delta_t * vTDeriv;
272 vResult = (delta_t*0.5) * (vTDeriv + vLastArray[0]);
273 vLastArray[0] = vTDeriv;
280 // =======================================
282 /** Calculates Euler angles from the local-to-body matrix.
283 @return a reference to the vEuler column vector.
285 FGColumnVector3& CalcEuler(void);
287 /** Calculates and returns the stability-to-body axis transformation matrix.
288 @return a reference to the stability-to-body transformation matrix.
290 FGMatrix33& GetTs2b(void);
292 /** Calculates and returns the body-to-stability axis transformation matrix.
293 @return a reference to the stability-to-body transformation matrix.
295 FGMatrix33& GetTb2s(void);
297 /** Retrieves the local-to-body transformation matrix.
298 @return a reference to the local-to-body transformation matrix.
300 FGMatrix33& GetTl2b(void) { return mTl2b; }
302 /** Retrieves a specific local-to-body matrix element.
303 @param r matrix row index.
304 @param c matrix column index.
305 @return the matrix element described by the row and column supplied.
307 double GetTl2b(int r, int c) { return mTl2b(r,c);}
309 /** Retrieves the body-to-local transformation matrix.
310 @return a reference to the body-to-local matrix.
312 FGMatrix33& GetTb2l(void) { return mTb2l; }
314 /** Retrieves a specific body-to-local matrix element.
315 @param r matrix row index.
316 @param c matrix column index.
317 @return the matrix element described by the row and column supplied.
319 double GetTb2l(int i, int j) { return mTb2l(i,j);}
321 /** Prints a summary of simulator state (speed, altitude,
324 void ReportState(void);
338 FGColumnVector4 vQtrn;
339 FGColumnVector4 vQdot_prev[4];
340 FGColumnVector4 vQdot;
341 FGColumnVector3 vLocalVelNED;
342 FGColumnVector3 vLocalEuler;
344 FGColumnVector4 vTmp;
345 FGColumnVector3 vEuler;
347 FGAircraft* Aircraft;
348 FGPosition* Position;
349 FGTranslation* Translation;
350 FGRotation* Rotation;
352 FGAtmosphere* Atmosphere;
354 FGAerodynamics* Aerodynamics;
355 FGGroundReactions* GroundReactions;
356 FGPropulsion* Propulsion;
357 FGPropertyManager* PropertyManager;
359 void Debug(int from);
362 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%