/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Header: FGState.h Author: Jon S. Berndt Date started: 11/17/98 ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. Further information about the GNU General Public License can also be found on the world wide web at http://www.gnu.org. FUNCTIONAL DESCRIPTION -------------------------------------------------------------------------------- HISTORY -------------------------------------------------------------------------------- 11/17/98 JSB Created %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SENTRY %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #ifndef FGSTATE_H #define FGSTATE_H /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #ifdef FGFS # include # ifdef SG_HAVE_STD_INCLUDES # include # else # include # endif #else # if defined(sgi) && !defined(__GNUC__) # include # else # include # endif #endif #include #include #include "FGJSBBase.h" #include "FGInitialCondition.h" #include "FGMatrix33.h" #include "FGColumnVector3.h" #include "FGColumnVector4.h" #include "FGFDMExec.h" #include "FGAtmosphere.h" #include "FGFCS.h" #include "FGTranslation.h" #include "FGRotation.h" #include "FGPosition.h" #include "FGAerodynamics.h" #include "FGOutput.h" #include "FGAircraft.h" #include "FGGroundReactions.h" #include "FGPropulsion.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #define ID_STATE "$Id$" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ namespace JSBSim { /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS DOCUMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ /** Encapsulates the calculation of aircraft state. @author Jon S. Berndt @version $Id$ @see Header File @see Source File */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS DECLARATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ class FGState : public FGJSBBase { public: /** Constructor @param Executive a pointer to the parent executive object */ FGState(FGFDMExec*); /// Destructor ~FGState(); /** Initializes the simulation state based on the passed-in parameters. @param U the body X-Axis velocity in fps. @param V the body Y-Axis velocity in fps. @param W the body Z-Axis velocity in fps. @param lat latitude measured in radians from the equator, negative values are south. @param lon longitude, measured in radians from the Greenwich meridian, negative values are west. @param phi the roll angle in radians. @param tht the pitch angle in radians. @param psi the heading angle in radians measured clockwise from north. @param h altitude in feet. @param wnorth north velocity in feet per second @param weast eastward velocity in feet per second @param wdown downward velocity in feet per second */ void Initialize(double U, double V, double W, double lat, double lon, double phi, double tht, double psi, double h, double wnorth, double weast, double wdown); /** Initializes the simulation state based on parameters from an Initial Conditions object. @param FGIC pointer to an initial conditions object. @see FGInitialConditions. */ void Initialize(FGInitialCondition *FGIC); /// returns the speed of sound in feet per second. inline double Geta(void) { return a; } /// Returns the simulation time in seconds. inline double Getsim_time(void) const { return sim_time; } /// Returns the simulation delta T. inline double Getdt(void) { return dt; } /// Suspends the simulation and sets the delta T to zero. inline void Suspend(void) {saved_dt = dt; dt = 0.0;} /// Resumes the simulation by resetting delta T to the correct value. inline void Resume(void) {dt = saved_dt;} /** Sets the speed of sound. @param speed the speed of sound in feet per second. */ inline void Seta(double speed) { a = speed; } /** Sets the current sim time. @param cur_time the current time @return the current time. */ inline double Setsim_time(double cur_time) { sim_time = cur_time; return sim_time; } /** Sets the integration time step for the simulation executive. @param delta_t the time step in seconds. */ inline void Setdt(double delta_t) { dt = delta_t; } /** Increments the simulation time. @return the new simulation time. */ inline double IncrTime(void) { sim_time+=dt; return sim_time; } /** Initializes the transformation matrices. @param phi the roll angle in radians. @param tht the pitch angle in radians. @param psi the heading angle in radians */ void InitMatrices(double phi, double tht, double psi); /** Calculates the local-to-body and body-to-local conversion matrices. */ void CalcMatrices(void); /** Integrates the quaternion. Given the supplied rotational rate vector and integration rate, the quaternion is integrated. The quaternion is later used to update the transformation matrices. @param vPQR the body rotational rate column vector. @param rate the integration rate in seconds. */ void IntegrateQuat(FGColumnVector3 vPQR, int rate); // ======================================= General Purpose INTEGRATOR enum iType {AB4, AB3, AB2, AM3, EULER, TRAPZ}; /** Multi-method integrator. @param type Type of intergation scheme to use. Can be one of:
  • AB4 - Adams-Bashforth, fourth order
  • AB3 - Adams-Bashforth, third order
  • AB2 - Adams-Bashforth, second order
  • AM3 - Adams Moulton, third order
  • EULER - Euler
  • TRAPZ - Trapezoidal
@param delta_t the integration time step in seconds @param vTDeriv a reference to the current value of the time derivative of the quantity being integrated (i.e. if vUVW is being integrated vTDeriv is the current value of vUVWdot) @param vLastArray an array of previously calculated and saved values of the quantity being integrated (i.e. if vUVW is being integrated vLastArray[0] is the past value of vUVWdot, vLastArray[1] is the value of vUVWdot prior to that, etc.) @return the current, incremental value of the item integrated to add to the previous value. */ template T Integrate(iType type, double delta_t, T& vTDeriv, T *vLastArray) { T vResult; switch (type) { case AB4: vResult = (delta_t/24.0)*( 55.0 * vTDeriv - 59.0 * vLastArray[0] + 37.0 * vLastArray[1] - 9.0 * vLastArray[2] ); vLastArray[2] = vLastArray[1]; vLastArray[1] = vLastArray[0]; vLastArray[0] = vTDeriv; break; case AB3: vResult = (delta_t/12.0)*( 23.0 * vTDeriv - 16.0 * vLastArray[0] + 5.0 * vLastArray[1] ); vLastArray[1] = vLastArray[0]; vLastArray[0] = vTDeriv; break; case AB2: vResult = (delta_t/2.0)*( 3.0 * vTDeriv - vLastArray[0] ); vLastArray[0] = vTDeriv; break; case AM3: vResult = (delta_t/12.0)*( 5.0 * vTDeriv + 8.0 * vLastArray[0] - 1.0 * vLastArray[1] ); vLastArray[1] = vLastArray[0]; vLastArray[0] = vTDeriv; break; case EULER: vResult = delta_t * vTDeriv; break; case TRAPZ: vResult = (delta_t*0.5) * (vTDeriv + vLastArray[0]); vLastArray[0] = vTDeriv; break; } return vResult; } // ======================================= /** Calculates Euler angles from the local-to-body matrix. @return a reference to the vEuler column vector. */ FGColumnVector3& CalcEuler(void); /** Calculates and returns the stability-to-body axis transformation matrix. @return a reference to the stability-to-body transformation matrix. */ FGMatrix33& GetTs2b(void); /** Calculates and returns the body-to-stability axis transformation matrix. @return a reference to the stability-to-body transformation matrix. */ FGMatrix33& GetTb2s(void); /** Retrieves the local-to-body transformation matrix. @return a reference to the local-to-body transformation matrix. */ FGMatrix33& GetTl2b(void) { return mTl2b; } /** Retrieves a specific local-to-body matrix element. @param r matrix row index. @param c matrix column index. @return the matrix element described by the row and column supplied. */ double GetTl2b(int r, int c) { return mTl2b(r,c);} /** Retrieves the body-to-local transformation matrix. @return a reference to the body-to-local matrix. */ FGMatrix33& GetTb2l(void) { return mTb2l; } /** Retrieves a specific body-to-local matrix element. @param r matrix row index. @param c matrix column index. @return the matrix element described by the row and column supplied. */ double GetTb2l(int i, int j) { return mTb2l(i,j);} /** Prints a summary of simulator state (speed, altitude, configuration, etc.) */ void ReportState(void); void bind(); void unbind(); private: double a; // speed of sound double sim_time, dt; double saved_dt; FGFDMExec* FDMExec; FGMatrix33 mTb2l; FGMatrix33 mTl2b; FGMatrix33 mTs2b; FGMatrix33 mTb2s; FGColumnVector4 vQtrn; FGColumnVector4 vQdot_prev[3]; FGColumnVector4 vQdot; FGColumnVector3 vUVW; FGColumnVector3 vLocalVelNED; FGColumnVector3 vLocalEuler; FGColumnVector4 vTmp; FGColumnVector3 vEuler; FGAircraft* Aircraft; FGPosition* Position; FGTranslation* Translation; FGRotation* Rotation; FGOutput* Output; FGAtmosphere* Atmosphere; FGFCS* FCS; FGAerodynamics* Aerodynamics; FGGroundReactions* GroundReactions; FGPropulsion* Propulsion; FGPropertyManager* PropertyManager; void Debug(int from); }; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% #endif