fgpropagate.h

6 DOF Missle Simulation

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Header:       FGPropagate.h
 Author:       Jon S. Berndt
 Date started: 1/5/99

 ------------- Copyright (C) 1999  Jon S. Berndt ([e-mail]) -------------

 This program is free software; you can redistribute it and/or modify it under
 the terms of the GNU Lesser 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 Lesser General Public License for more
 details.

 You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on
 the world wide web at http://www.gnu.org.

HISTORY
--------------------------------------------------------------------------------
01/05/99   JSB   Created

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#ifndef FGPROPAGATE_H
#define FGPROPAGATE_H

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#include <models/FGModel.h>
#include <math/FGColumnVector3.h>
#include <initialization/FGInitialCondition.h>
#include <math/FGLocation.h>
#include <math/FGQuaternion.h>
#include <math/FGMatrix33.h>

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#define ID_PROPAGATE "$Id$"

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

namespace JSBSim {

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

/** Models the EOM and integration/propagation of state.
    The Equations of Motion (EOM) for JSBSim are integrated to propagate the
    state of the vehicle given the forces and moments that act on it. The
    integration accounts for a rotating Earth.
    Integration of rotational and translation position and rate can be 
    customized as needed or frozen by the selection of no integrator. The
    selection of which integrator to use is done through the setting of 
    the associated property. There are four properties which can be set:
    
    @code
    simulation/integrator/rate/rotational
    simulation/integrator/rate/translational
    simulation/integrator/position/rotational
    simulation/integrator/position/translational
    @endcode
    
    Each of the integrators listed above can be set to one of the following values:

    @code
    0: No integrator (Freeze)
    1: Rectangular Euler
    2: Trapezoidal
    3: Adams Bashforth 2
    4: Adams Bashforth 3
    @endcode

    @author Jon S. Berndt, Mathias Froehlich
    @version $Id$
  */

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

class FGPropagate : public FGModel {
public:
  /** Constructor.
      The constructor initializes several variables, and sets the initial set
      of integrators to use as follows:
      - integrator, rotational rate = Adams Bashforth 2
      - integrator, translational rate = Adams Bashforth 2
      - integrator, rotational position = Trapezoidal
      - integrator, translational position = Trapezoidal
      @param Executive a pointer to the parent executive object */
  FGPropagate(FGFDMExec* Executive);

  /// Destructor
  ~FGPropagate();
  
  /// These define the indices use to select the various integrators.
  enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3};

/** The current vehicle state vector structure contains the translational and
    angular position, and the translational and angular velocity. */
struct VehicleState {
  /** Represents the current location of the vehicle in Earth centered Earth
      fixed (ECEF) frame.
      units ft */
  FGLocation vLocation;
  /** The velocity vector of the vehicle with respect to the ECEF frame,
      expressed in the body system.
      units ft/sec */
  FGColumnVector3 vUVW;
  /** The angular velocity vector for the vehicle relative to the ECEF frame,
      expressed in the body frame.
      units rad/sec */
  FGColumnVector3 vPQR;
  /** The current orientation of the vehicle, that is, the orientation of the
      body frame relative to the local, vehilce-carried, NED frame. */
  FGQuaternion vQtrn;
};

  /** Initializes the FGPropagate class after instantiation and prior to first execution.
      The base class FGModel::InitModel is called first, initializing pointers to the 
      other FGModel objects (and others).  */
  bool InitModel(void);

  /** Runs the Propagate model; called by the Executive.
      @return false if no error */
  bool Run(void);

  /** Retrieves the velocity vector.
      The vector returned is represented by an FGColumnVector reference. The vector
      for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
      is 1-based, so that the first element can be retrieved using the "()" operator.
      In other words, vVel(1) is Vnorth. Various convenience enumerators are defined
      in FGJSBBase. The relevant enumerators for the vector returned by this call are,
      eNorth=1, eEast=2, eDown=3.
      units ft/sec
      @return The vehicle velocity vector with respect to the Earth centered frame,
              expressed in Local horizontal frame.
  */
  const FGColumnVector3& GetVel(void) const { return vVel; }
  
  /** Retrieves the body frame vehicle velocity vector.
      The vector returned is represented by an FGColumnVector reference. The vector
      for the velocity in Body frame is organized (Vx, Vy, Vz). The vector
      is 1-based, so that the first element can be retrieved using the "()" operator.
      In other words, vUVW(1) is Vx. Various convenience enumerators are defined
      in FGJSBBase. The relevant enumerators for the vector returned by this call are,
      eX=1, eY=2, eZ=3.
      units ft/sec
      @return The body frame vehicle velocity vector in ft/sec.
  */
  const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
  
  /** Retrieves the body axis acceleration.
      Retrieves the computed body axis accelerations based on the
      applied forces and accounting for a rotating body frame.
      The vector returned is represented by an FGColumnVector reference. The vector
      for the acceleration in Body frame is organized (Ax, Ay, Az). The vector
      is 1-based, so that the first element can be retrieved using the "()" operator.
      In other words, vUVWdot(1) is Ax. Various convenience enumerators are defined
      in FGJSBBase. The relevant enumerators for the vector returned by this call are,
      eX=1, eY=2, eZ=3.
      units ft/sec^2
      @return Body axis translational acceleration in ft/sec^2.
  */
  const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; }
  
  /** Retrieves the body angular rates vector.
      Retrieves the body angular rates (p, q, r), which are calculated by integration
      of the angular acceleration.
      The vector returned is represented by an FGColumnVector reference. The vector
      for the angular velocity in Body frame is organized (P, Q, R). The vector
      is 1-based, so that the first element can be retrieved using the "()" operator.
      In other words, vPQR(1) is P. Various convenience enumerators are defined
      in FGJSBBase. The relevant enumerators for the vector returned by this call are,
      eP=1, eQ=2, eR=3.
      units rad/sec
      @return The body frame angular rates in rad/sec.
  */
  const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
  
  /** Retrieves the body axis angular acceleration vector.
      Retrieves the body axis angular acceleration vector in rad/sec^2. The
      angular acceleration vector is determined from the applied forces and
      accounts for a rotating frame.
      The vector returned is represented by an FGColumnVector reference. The vector
      for the angular acceleration in Body frame is organized (Pdot, Qdot, Rdot). The vector
      is 1-based, so that the first element can be retrieved using the "()" operator.
      In other words, vPQRdot(1) is Pdot. Various convenience enumerators are defined
      in FGJSBBase. The relevant enumerators for the vector returned by this call are,
      eP=1, eQ=2, eR=3.
      units rad/sec^2
      @return The angular acceleration vector.
  */
  const FGColumnVector3& GetPQRdot(void) const {return vPQRdot;}
  
  /** Retrieves the Euler angles that define the vehicle orientation.
      Extracts the Euler angles from the quaternion that stores the orientation
      in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
      vector returned is represented by an FGColumnVector reference. The vector
      for the Euler angles is organized (Phi, Theta, Psi). The vector
      is 1-based, so that the first element can be retrieved using the "()" operator.
      In other words, the returned vector item with subscript (1) is Phi.
      Various convenience enumerators are defined in FGJSBBase. The relevant
      enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
      units radians
      @return The Euler angle vector, where the first item in the
              vector is the angle about the X axis, the second is the
              angle about the Y axis, and the third item is the angle
              about the Z axis (Phi, Theta, Psi).
  */
  const FGColumnVector3& GetEuler(void) const { return VState.vQtrn.GetEuler(); }

  /** Retrieves a body frame velocity component.
      Retrieves a body frame velocity component. The velocity returned is
      extracted from the vUVW vector (an FGColumnVector). The vector for the
      velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based.
      In other words, GetUVW(1) returns Vx. Various convenience enumerators
      are defined in FGJSBBase. The relevant enumerators for the velocity
      returned by this call are, eX=1, eY=2, eZ=3.
      units ft/sec
      @param idx the index of the velocity component desired (1-based).
      @return The body frame velocity component.
  */
  double GetUVW   (int idx) const { return VState.vUVW(idx); }

  /** Retrieves a body frame acceleration component.
      Retrieves a body frame acceleration component. The acceleration returned
      is extracted from the vUVWdot vector (an FGColumnVector). The vector for
      the acceleration in Body frame is organized (Ax, Ay, Az). The vector is
      1-based. In other words, GetUVWdot(1) returns Ax. Various convenience
      enumerators are defined in FGJSBBase. The relevant enumerators for the
      acceleration returned by this call are, eX=1, eY=2, eZ=3.
      units ft/sec^2
      @param idx the index of the acceleration component desired (1-based).
      @return The body frame acceleration component.
  */
  double GetUVWdot(int idx) const { return vUVWdot(idx); }