fgpropagate.h

6 DOF Missle Simulation

  /** Retrieves a Local frame velocity component.
      Retrieves a Local frame velocity component. The velocity returned is
      extracted from the vVel vector (an FGColumnVector). The vector for the
      velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
      is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience
      enumerators are defined in FGJSBBase. The relevant enumerators for the
      velocity returned by this call are, eNorth=1, eEast=2, eDown=3.
      units ft/sec
      @param idx the index of the velocity component desired (1-based).
      @return The body frame velocity component.
  */
  double GetVel(int idx) const { return vVel(idx); }

  /** Retrieves the total inertial velocity in ft/sec.
  */
  double GetInertialVelocityMagnitude(void) const { return vInertialVelocity.Magnitude(); }

  /** Returns the current altitude.
      Returns the current altitude. Specifically, this function returns the
      difference between the distance to the center of the Earth, and sea level.
      units ft
      @return The current altitude above sea level in feet.
  */
  double Geth(void)   const { return VState.vLocation.GetRadius() - SeaLevelRadius; }

  /** Returns the current altitude.
      Returns the curren altitude. Specifically, this function returns the
      difference between the distance to the center of the Earth, and sea level.
      units meters
      @return The current altitude above sea level in meters.
  */
  double Gethmeters(void) const { return Geth()*fttom;}

  /** Retrieves a body frame angular velocity component.
      Retrieves a body frame angular velocity component. The angular velocity
      returned is extracted from the vPQR vector (an FGColumnVector). The vector
      for the angular velocity in Body frame is organized (P, Q, R). The vector
      is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
      convenience enumerators are defined in FGJSBBase. The relevant enumerators
      for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
      units rad/sec
      @param axis the index of the angular velocity component desired (1-based).
      @return The body frame angular velocity component.
  */
  double GetPQR(int axis) const {return VState.vPQR(axis);}

  /** Retrieves a body frame angular acceleration component.
      Retrieves a body frame angular acceleration component. The angular
      acceleration returned is extracted from the vPQRdot vector (an
      FGColumnVector). The vector for the angular acceleration in Body frame
      is organized (Pdot, Qdot, Rdot). The vector is 1-based. In other words,
      GetPQRdot(1) returns Pdot (roll acceleration). Various convenience
      enumerators are defined in FGJSBBase. The relevant enumerators for the
      angular acceleration returned by this call are, eP=1, eQ=2, eR=3.
      units rad/sec^2
      @param axis the index of the angular acceleration component desired (1-based).
      @return The body frame angular acceleration component.
  */
  double GetPQRdot(int axis) const {return vPQRdot(axis);}

  /** Retrieves a vehicle Euler angle component.
      Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
      stores the vehicle orientation relative to the Local frame. The order of
      rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
      Phi. Various convenience enumerators are defined in FGJSBBase. The
      relevant enumerators for the Euler angle returned by this call are,
      ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
      units radians
      @return An Euler angle.
  */
  double GetEuler(int axis) const { return VState.vQtrn.GetEuler(axis); }

  /** Retrieves the cosine of a vehicle Euler angle component.
      Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
      quaternion that stores the vehicle orientation relative to the Local frame.
      The order of rotations used is Yaw-Pitch-Roll. The Euler angle
      with subscript (1) is Phi. Various convenience enumerators are defined in
      FGJSBBase. The relevant enumerators for the Euler angle referred to in this
      call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
      units none
      @return The cosine of an Euler angle.
  */
  double GetCosEuler(int idx) const { return VState.vQtrn.GetCosEuler(idx); }

  /** Retrieves the sine of a vehicle Euler angle component.
      Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
      quaternion that stores the vehicle orientation relative to the Local frame.
      The order of rotations used is Yaw-Pitch-Roll. The Euler angle
      with subscript (1) is Phi. Various convenience enumerators are defined in
      FGJSBBase. The relevant enumerators for the Euler angle referred to in this
      call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
      units none
      @return The sine of an Euler angle.
  */
  double GetSinEuler(int idx) const { return VState.vQtrn.GetSinEuler(idx); }

  /** Returns the current altitude rate.
      Returns the current altitude rate (rate of climb).
      units ft/sec
      @return The current rate of change in altitude.
  */
  double Gethdot(void) const { return -vVel(eDown); }

  /** Returns the "constant" RunwayRadius.
      The RunwayRadius parameter is set by the calling application or set to
      sea level if JSBSim is running in standalone mode.
      units feet
      @return distance of the runway from the center of the earth.
      */
  double GetRunwayRadius(void) const;

  double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
  double GetTerrainElevationASL(void) const;
  double GetDistanceAGL(void)  const;
  double GetRadius(void) const {
      if (VState.vLocation.GetRadius() == 0) return 1.0;
      else return VState.vLocation.GetRadius();
  }
  double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
  double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }

  double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
  double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }

  double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }

  double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
  double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
  const FGLocation& GetLocation(void) const { return VState.vLocation; }

  /** Retrieves the local-to-body transformation matrix.
      The quaternion class, being the means by which the orientation of the
      vehicle is stored, manages the local-to-body transformation matrix.
      @return a reference to the local-to-body transformation matrix.  */
  const FGMatrix33& GetTl2b(void) const { return VState.vQtrn.GetT(); }

  /** Retrieves the body-to-local transformation matrix.
      The quaternion class, being the means by which the orientation of the
      vehicle is stored, manages the body-to-local transformation matrix.
      @return a reference to the body-to-local matrix.  */
  const FGMatrix33& GetTb2l(void) const { return VState.vQtrn.GetTInv(); }

  /** Retrieves the ECEF-to-body transformation matrix.
      @return a reference to the ECEF-to-body transformation matrix.  */
  const FGMatrix33& GetTec2b(void) const { return Tec2b; }

  /** Retrieves the body-to-ECEF transformation matrix.
      @return a reference to the body-to-ECEF matrix.  */
  const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }

  /** Retrieves the ECEF-to-ECI transformation matrix.
      @return a reference to the ECEF-to-ECI transformation matrix.  */
  const FGMatrix33& GetTec2i(void);

  /** Retrieves the ECI-to-ECEF transformation matrix.
      @return a reference to the ECI-to-ECEF matrix.  */
  const FGMatrix33& GetTi2ec(void);

  /** Retrieves the ECEF-to-local transformation matrix.
      Retrieves the ECEF-to-local transformation matrix. Note that the so-called
      local from is also know as the NED frame (for North, East, Down).
      @return a reference to the ECEF-to-local matrix.  */
  const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); }

  /** Retrieves the local-to-ECEF transformation matrix.
      Retrieves the local-to-ECEF transformation matrix. Note that the so-called
      local from is also know as the NED frame (for North, East, Down).
      @return a reference to the local-to-ECEF matrix.  */
  const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }

  const VehicleState GetVState(void) const { return VState; }

  void SetVState(VehicleState vstate) {
      VState.vLocation = vstate.vLocation;
      VState.vUVW = vstate.vUVW;
      VState.vPQR = vstate.vPQR;
      VState.vQtrn = vstate.vQtrn; // ... mmh
  }

  const FGQuaternion GetQuaternion(void) const { return VState.vQtrn; }

  void SetPQR(unsigned int i, double val) {
      if ((i>=1) && (i<=3) )
          VState.vPQR(i) = val;
  }

  void SetUVW(unsigned int i, double val) {
      if ((i>=1) && (i<=3) )
          VState.vUVW(i) = val;
  }

// SET functions

  void SetLongitude(double lon) { VState.vLocation.SetLongitude(lon); }
  void SetLongitudeDeg(double lon) {SetLongitude(lon*degtorad);}
  void SetLatitude(double lat) { VState.vLocation.SetLatitude(lat); }
  void SetLatitudeDeg(double lat) {SetLatitude(lat*degtorad);}
  void SetRadius(double r) { VState.vLocation.SetRadius(r); }
  void SetLocation(const FGLocation& l) { VState.vLocation = l; }
  void Seth(double tt);
  void Sethmeters(double tt) {Seth(tt/fttom);}
  void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
  void SetTerrainElevationASL(double tt);
  void SetDistanceAGL(double tt);
  void SetInitialState(const FGInitialCondition *);
  void RecomputeRunwayRadius(void);

  void CalculatePQRdot(void);
  void CalculateQuatdot(void);
  void CalculateLocationdot(void);
  void CalculateUVWdot(void);

private:

// state vector

  struct VehicleState VState;

  FGColumnVector3 vVel;
  FGColumnVector3 vInertialVelocity;
  FGColumnVector3 vPQRdot, last_vPQRdot, last2_vPQRdot;
  FGColumnVector3 vUVWdot, last_vUVWdot, last2_vUVWdot;
  FGColumnVector3 vLocationDot, last_vLocationDot, last2_vLocationDot;
  FGColumnVector3 vLocation;
  FGColumnVector3 vPQRi;   // Inertial frame angular velocity
  FGColumnVector3 vOmega;  // The Earth angular velocity vector
  FGColumnVector3 vOmegaLocal;  // The local frame angular velocity vector
  FGQuaternion vQtrndot, last_vQtrndot, last2_vQtrndot;
  FGMatrix33 Tec2b;
  FGMatrix33 Tb2ec;
  FGMatrix33 Tl2b;   // local to body frame matrix copy for immediate local use
  FGMatrix33 Tb2l;   // body to local frame matrix copy for immediate local use
  FGMatrix33 Tl2ec;  // local to ECEF matrix copy for immediate local use
  FGMatrix33 Tec2l;  // ECEF to local frame matrix copy for immediate local use
  FGMatrix33 Tec2i;  // ECEF to ECI frame matrix copy for immediate local use
  FGMatrix33 Ti2ec;  // ECI to ECEF frame matrix copy for immediate local use
  FGMatrix33 Ti2b;   // ECI to body frame rotation matrix
  FGMatrix33 Tb2i;   // body to ECI frame rotation matrix
  
  double RunwayRadius, SeaLevelRadius, VehicleRadius;
  double radInv;
  int integrator_rotational_rate;
  int integrator_translational_rate;
  int integrator_rotational_position;
  int integrator_translational_position;

  void bind(void);
  void Debug(int from);
};
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endif