]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/FGState.h
putting back the pilot-g used by accel.xml gadget
[flightgear.git] / src / FDM / JSBSim / FGState.h
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2  
3  Header:       FGState.h
4  Author:       Jon S. Berndt
5  Date started: 11/17/98
6  
7  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
8  
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
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 General Public License for more
17  details.
18  
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.
22  
23  Further information about the GNU General Public License can also be found on
24  the world wide web at http://www.gnu.org.
25  
26 FUNCTIONAL DESCRIPTION
27 --------------------------------------------------------------------------------
28  
29 HISTORY
30 --------------------------------------------------------------------------------
31 11/17/98   JSB   Created
32  
33 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
34 SENTRY
35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
36
37 #ifndef FGSTATE_H
38 #define FGSTATE_H
39
40 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
41 INCLUDES
42 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
43
44 #ifdef FGFS
45 #  include <simgear/compiler.h>
46 #  ifdef SG_HAVE_STD_INCLUDES
47 #    include <fstream>
48 #  else
49 #    include <fstream.h>
50 #  endif
51 #else
52 #  if defined(sgi) && !defined(__GNUC__)
53 #    include <fstream.h>
54 #  else
55 #    include <fstream>
56 #  endif
57 #endif
58
59 #include <string>
60 #include <map>
61 #include "FGJSBBase.h"
62 #include "FGInitialCondition.h"
63 #include "FGMatrix33.h"
64 #include "FGColumnVector3.h"
65 #include "FGColumnVector4.h"
66
67 #include "FGFDMExec.h"
68 #include "FGAtmosphere.h"
69 #include "FGFCS.h"
70 #include "FGTranslation.h"
71 #include "FGRotation.h"
72 #include "FGPosition.h"
73 #include "FGAerodynamics.h"
74 #include "FGOutput.h"
75 #include "FGAircraft.h"
76 #include "FGGroundReactions.h"
77 #include "FGPropulsion.h"
78
79
80 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
81 DEFINITIONS
82 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
83
84 #define ID_STATE "$Id$"
85
86 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
87 FORWARD DECLARATIONS
88 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
89
90 namespace JSBSim {
91
92 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
93 COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs]
94 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
95
96 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
97 CLASS DOCUMENTATION
98 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
99
100 /** Encapsulates the calculation of aircraft state.
101     @author Jon S. Berndt
102     @version $Id$
103     @see <a href="http://cvs.sourceforge.net/cgi-bin/viewcvs.cgi/jsbsim/JSBSim/FGState.h?rev=HEAD&content-type=text/vnd.viewcvs-markup">
104          Header File </a>
105     @see <a href="http://cvs.sourceforge.net/cgi-bin/viewcvs.cgi/jsbsim/JSBSim/FGState.cpp?rev=HEAD&content-type=text/vnd.viewcvs-markup">
106          Source File </a>
107 */
108
109 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
110 CLASS DECLARATION
111 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
112
113 class FGState : public FGJSBBase
114 {
115 public:
116   /** Constructor
117       @param Executive a pointer to the parent executive object */
118   FGState(FGFDMExec*);
119   /// Destructor
120   ~FGState();
121
122  /** Initializes the simulation state based on the passed-in parameters.
123       @param U the body X-Axis velocity in fps.
124       @param V the body Y-Axis velocity in fps.
125       @param W the body Z-Axis velocity in fps.
126       @param lat latitude measured in radians from the equator, negative values are south.
127       @param lon longitude, measured in radians from the Greenwich meridian, negative values are west.
128       @param phi the roll angle in radians.
129       @param tht the pitch angle in radians.
130       @param psi the heading angle in radians measured clockwise from north.
131       @param h altitude in feet.
132       @param wnorth north velocity in feet per second
133       @param weast eastward velocity in feet per second
134       @param wdown downward velocity in feet per second
135       */
136   void Initialize(double U,
137                   double V,
138                   double W,
139                   double lat,
140                   double lon,
141                   double phi,
142                   double tht,
143                   double psi,
144                   double h,
145                   double wnorth,
146                   double weast,
147                   double wdown);
148
149   /** Initializes the simulation state based on parameters from an Initial Conditions object.
150       @param FGIC pointer to an initial conditions object.
151       @see FGInitialConditions.
152       */
153   void Initialize(FGInitialCondition *FGIC);
154
155   /// returns the speed of sound in feet per second.
156   inline double Geta(void) { return a; }
157
158   /// Returns the simulation time in seconds.
159   inline double Getsim_time(void) const { return sim_time; }
160   /// Returns the simulation delta T.
161   inline double Getdt(void) { return dt; }
162
163   /// Suspends the simulation and sets the delta T to zero.
164   inline void Suspend(void) {saved_dt = dt; dt = 0.0;}
165   /// Resumes the simulation by resetting delta T to the correct value.
166   inline void Resume(void)  {dt = saved_dt;}
167
168   /** Sets the speed of sound.
169       @param speed the speed of sound in feet per second.
170       */
171   inline void Seta(double speed) { a = speed; }
172
173   /** Sets the current sim time.
174       @param cur_time the current time
175       @return the current time.
176       */
177   inline double Setsim_time(double cur_time) {
178     sim_time = cur_time;
179     return sim_time;
180   }
181   
182   /** Sets the integration time step for the simulation executive.
183       @param delta_t the time step in seconds.
184       */
185   inline void  Setdt(double delta_t) { dt = delta_t; }
186
187   /** Increments the simulation time.
188       @return the new simulation time.
189       */
190   inline double IncrTime(void) {
191     sim_time+=dt;
192     return sim_time;
193   }
194
195   /** Initializes the transformation matrices.
196       @param phi the roll angle in radians.
197       @param tht the pitch angle in radians.
198       @param psi the heading angle in radians
199       */
200   void InitMatrices(double phi, double tht, double psi);
201
202   /** Calculates the local-to-body and body-to-local conversion matrices.
203       */
204   void CalcMatrices(void);
205
206   /** Integrates the quaternion.
207       Given the supplied rotational rate vector and integration rate, the quaternion
208       is integrated. The quaternion is later used to update the transformation
209       matrices.
210       @param vPQR the body rotational rate column vector.
211       @param rate the integration rate in seconds.
212       */
213   void IntegrateQuat(FGColumnVector3 vPQR, int rate);
214   
215   // ======================================= General Purpose INTEGRATOR
216
217   enum iType {AB4, AB3, AB2, AM3, EULER, TRAPZ};
218   
219   /** Multi-method integrator.
220       @param type Type of intergation scheme to use. Can be one of:
221              <ul>
222              <li>AB4 - Adams-Bashforth, fourth order</li>
223              <li>AB3 - Adams-Bashforth, third order</li>
224              <li>AB2 - Adams-Bashforth, second order</li>
225              <li>AM3 - Adams Moulton, third order</li>
226              <li>EULER - Euler</li>
227              <li>TRAPZ - Trapezoidal</li>
228              </ul>
229       @param delta_t the integration time step in seconds
230       @param vTDeriv a reference to the current value of the time derivative of
231              the quantity being integrated (i.e. if vUVW is being integrated
232              vTDeriv is the current value of vUVWdot)
233       @param vLastArray an array of previously calculated and saved values of 
234              the quantity being integrated (i.e. if vUVW is being integrated
235              vLastArray[0] is the past value of vUVWdot, vLastArray[1] is the value of
236              vUVWdot prior to that, etc.)
237       @return the current, incremental value of the item integrated to add to the
238               previous value. */
239   
240   template <class T> T Integrate(iType type, double delta_t, T& vTDeriv, T *vLastArray)
241   {
242     T vResult;
243
244     switch (type) {
245     case AB4:
246       vResult = (delta_t/24.0)*(  55.0 * vTDeriv
247                                 - 59.0 * vLastArray[0]
248                                 + 37.0 * vLastArray[1]
249                                 -  9.0 * vLastArray[2] );
250       vLastArray[2] = vLastArray[1];
251       vLastArray[1] = vLastArray[0];
252       vLastArray[0] = vTDeriv;
253       break;
254     case AB3:
255       vResult = (delta_t/12.0)*(  23.0 * vTDeriv
256                                 - 16.0 * vLastArray[0]
257                                 +  5.0 * vLastArray[1] );
258       vLastArray[1] = vLastArray[0];
259       vLastArray[0] = vTDeriv;
260       break;
261     case AB2:
262       vResult = (delta_t/2.0)*( 3.0 * vTDeriv - vLastArray[0] );
263       vLastArray[0] = vTDeriv;
264       break;
265     case AM3:
266       vResult = (delta_t/12.0)*(  5.0 * vTDeriv
267                                 + 8.0 * vLastArray[0]
268                                 - 1.0 * vLastArray[1] );
269       vLastArray[1] = vLastArray[0];
270       vLastArray[0] = vTDeriv;
271       break;
272     case EULER:
273       vResult = delta_t * vTDeriv;
274       break;
275     case TRAPZ:
276       vResult = (delta_t*0.5) * (vTDeriv + vLastArray[0]);
277       vLastArray[0] = vTDeriv;
278       break;
279     }
280
281     return vResult;
282   }
283
284   // =======================================
285
286   /** Calculates Euler angles from the local-to-body matrix.
287       @return a reference to the vEuler column vector.
288       */
289   FGColumnVector3& CalcEuler(void);
290
291   /** Calculates and returns the stability-to-body axis transformation matrix.
292       @return a reference to the stability-to-body transformation matrix.
293       */
294   FGMatrix33& GetTs2b(void);
295   
296   /** Calculates and returns the body-to-stability axis transformation matrix.
297       @return a reference to the stability-to-body transformation matrix.
298       */
299   FGMatrix33& GetTb2s(void);
300
301   /** Retrieves the local-to-body transformation matrix.
302       @return a reference to the local-to-body transformation matrix.
303       */
304   FGMatrix33& GetTl2b(void) { return mTl2b; }
305
306   /** Retrieves a specific local-to-body matrix element.
307       @param r matrix row index.
308       @param c matrix column index.
309       @return the matrix element described by the row and column supplied.
310       */
311   double GetTl2b(int r, int c) { return mTl2b(r,c);}
312
313   /** Retrieves the body-to-local transformation matrix.
314       @return a reference to the body-to-local matrix.
315       */
316   FGMatrix33& GetTb2l(void) { return mTb2l; }
317
318   /** Retrieves a specific body-to-local matrix element.
319       @param r matrix row index.
320       @param c matrix column index.
321       @return the matrix element described by the row and column supplied.
322       */
323   double GetTb2l(int i, int j) { return mTb2l(i,j);}
324   
325   /** Prints a summary of simulator state (speed, altitude, 
326       configuration, etc.)
327   */
328   void ReportState(void);
329   
330   void bind();
331   void unbind();
332
333 private:
334   double a;                          // speed of sound
335   double sim_time, dt;
336   double saved_dt;
337
338   FGFDMExec* FDMExec;
339   FGMatrix33 mTb2l;
340   FGMatrix33 mTl2b;
341   FGMatrix33 mTs2b;
342   FGMatrix33 mTb2s;
343   FGColumnVector4 vQtrn;
344   FGColumnVector4 vQdot_prev[3];
345   FGColumnVector4 vQdot;
346   FGColumnVector3 vUVW;
347   FGColumnVector3 vLocalVelNED;
348   FGColumnVector3 vLocalEuler;
349   
350   FGColumnVector4 vTmp;
351   FGColumnVector3 vEuler;
352
353   FGAircraft* Aircraft;
354   FGPosition* Position;
355   FGTranslation* Translation;
356   FGRotation* Rotation;
357   FGOutput* Output;
358   FGAtmosphere* Atmosphere;
359   FGFCS* FCS;
360   FGAerodynamics* Aerodynamics;
361   FGGroundReactions* GroundReactions;
362   FGPropulsion* Propulsion;
363   FGPropertyManager* PropertyManager;
364
365   void Debug(int from);
366 };
367 }
368 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
369
370
371 #endif
372