FGPropagate.h

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 Header:       FGPropagate.h
 Author:       Jon S. Berndt
 Date started: 1/5/99
 
 ------------- Copyright (C) 1999  Jon S. Berndt (jon@jsbsim.org) -------------
 
 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/FGLocation.h"
#include "math/FGQuaternion.h"
 
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
namespace JSBSim {
 
class FGInitialCondition;
class FGInertial;
 
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
class FGPropagate : public FGModel {
public:
 
  struct VehicleState {
    FGLocation vLocation;
 
    FGColumnVector3 vUVW;
 
    FGColumnVector3 vPQR;
 
    FGColumnVector3 vPQRi;
 
    FGQuaternion qAttitudeLocal;
 
    FGQuaternion qAttitudeECI;
 
    FGQuaternion vQtrndot;
 
    FGColumnVector3 vInertialVelocity;
 
    FGColumnVector3 vInertialPosition;
 
    std::deque <FGColumnVector3> dqPQRidot;
    std::deque <FGColumnVector3> dqUVWidot;
    std::deque <FGColumnVector3> dqInertialVelocity;
    std::deque <FGQuaternion>    dqQtrndot;
  };
 
  explicit FGPropagate(FGFDMExec* Executive);
 
  ~FGPropagate();
 
  enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2,
                       eAdamsBashforth3, eAdamsBashforth4, eBuss1, eBuss2, eLocalLinearization, eAdamsBashforth5};
 
  bool InitModel(void);
 
  void InitializeDerivatives();
 
  bool Run(bool Holding);
 
  const FGColumnVector3& GetVel(void) const { return vVel; }
 
  const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
 
  const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
 
  const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
 
  const FGQuaternion& GetQuaterniondot(void) const {return VState.vQtrndot;}
 
  const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
 
  FGColumnVector3 GetEulerDeg(void) const;
 
  double GetUVW(int idx) const { return VState.vUVW(idx); }
 
  double GetVel(int idx) const { return vVel(idx); }
 
  double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
 
  double GetNEDVelocityMagnitude(void) const { return VState.vUVW.Magnitude(); }
 
  const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
  double GetInertialVelocity(int i) const { return VState.vInertialVelocity(i); }
 
  const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
  double GetInertialPosition(int i) const { return VState.vInertialPosition(i); }
 
  FGColumnVector3 GetECEFVelocity(void) const {return Tb2ec * VState.vUVW; }
 
  double GetECEFVelocity(int idx) const {return (Tb2ec * VState.vUVW)(idx); }
 
  double GetAltitudeASL(void) const;
 
  double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
 
  double GetPQR(int axis) const {return VState.vPQR(axis);}
 
  double GetPQRi(int axis) const {return VState.vPQRi(axis);}
 
  double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
 
  double GetEulerDeg(int axis) const { return VState.qAttitudeLocal.GetEuler(axis) * radtodeg; }
 
  double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
 
  double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
 
  double Gethdot(void) const { return -vVel(eDown); }
 
  double GetLocalTerrainRadius(void) const;
 
  double GetEarthPositionAngle(void) const { return epa; }
 
  double GetEarthPositionAngleDeg(void) const { return epa*radtodeg;}
 
  const FGColumnVector3& GetTerrainVelocity(void) const { return LocalTerrainVelocity; }
  const FGColumnVector3& GetTerrainAngularVelocity(void) const { return LocalTerrainAngularVelocity; }
  void RecomputeLocalTerrainVelocity();
 
  double GetTerrainElevation(void) const;
  double GetDistanceAGL(void)  const;
  double GetDistanceAGLKm(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 GetGeodeticAltitudeKm(void) const { return VState.vLocation.GetGeodAltitude()*0.0003048; }
 
  double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
  double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
  const FGLocation& GetLocation(void) const { return VState.vLocation; }
  double GetLocation(int i) const { return VState.vLocation(i); }
 
  const FGMatrix33& GetTl2b(void) const { return Tl2b; }
 
  const FGMatrix33& GetTb2l(void) const { return Tb2l; }
 
  const FGMatrix33& GetTec2b(void) const { return Tec2b; }
 
  const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
 
  const FGMatrix33& GetTi2b(void) const { return Ti2b; }
 
  const FGMatrix33& GetTb2i(void) const { return Tb2i; }
 
  const FGMatrix33& GetTec2i(void) const { return Tec2i; }
 
  const FGMatrix33& GetTi2ec(void) const { return Ti2ec; }
 
  const FGMatrix33& GetTec2l(void) const { return Tec2l; }
 
  const FGMatrix33& GetTl2ec(void) const { return Tl2ec; }
 
  const FGMatrix33& GetTl2i(void) const { return Tl2i; }
 
  const FGMatrix33& GetTi2l(void) const { return Ti2l; }
 
  const VehicleState& GetVState(void) const { return VState; }
 
  void SetVState(const VehicleState& vstate);
 
  void SetEarthPositionAngle(double EPA) {epa = EPA;}
 
  void SetInertialOrientation(const FGQuaternion& Qi);
  void SetInertialVelocity(const FGColumnVector3& Vi);
  void SetInertialRates(const FGColumnVector3& vRates);
 
  const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
 
  const FGQuaternion GetQuaternionECI(void) const { return VState.qAttitudeECI; }
 
  const FGQuaternion GetQuaternionECEF(void) const { return Qec2b; }
 
  void SetPQR(unsigned int i, double val) {
    VState.vPQR(i) = val;
    VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
  }
 
  void SetUVW(unsigned int i, double val) {
    VState.vUVW(i) = val;
    CalculateInertialVelocity();
  }
 
// SET functions
 
  void SetLongitude(double lon)
  {
    VState.vLocation.SetLongitude(lon);
    UpdateVehicleState();
  }
  void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
  void SetLatitude(double lat)
  {
    VState.vLocation.SetLatitude(lat);
    UpdateVehicleState();
  }
  void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
  void SetRadius(double r)
  {
    VState.vLocation.SetRadius(r);
    VState.vInertialPosition = Tec2i * VState.vLocation;
  }
 
  void SetAltitudeASL(double altASL);
  void SetAltitudeASLmeters(double altASL) { SetAltitudeASL(altASL/fttom); }
 
  void SetTerrainElevation(double tt);
  void SetDistanceAGL(double tt);
  void SetDistanceAGLKm(double tt);
 
  void SetInitialState(const FGInitialCondition*);
  void SetLocation(const FGLocation& l);
  void SetLocation(const FGColumnVector3& lv)
  {
      FGLocation l = FGLocation(lv);
      SetLocation(l);
  }
  void SetPosition(const double Lon, const double Lat, const double Radius)
  {
      FGLocation l = FGLocation(Lon, Lat, Radius);
      SetLocation(l);
  }
 
  void NudgeBodyLocation(const FGColumnVector3& deltaLoc) {
    VState.vInertialPosition -= Tb2i*deltaLoc;
    VState.vLocation -= Tb2ec*deltaLoc;
  }
 
  void SetHoldDown(bool hd);
 
  void DumpState(void);
 
  struct Inputs {
    FGColumnVector3 vPQRidot;
    FGColumnVector3 vUVWidot;
    FGColumnVector3 vOmegaPlanet;
    double SemiMajor;
    double SemiMinor;
    double DeltaT;
  } in;
 
private:
 
// state vector
 
  struct VehicleState VState;
 
  FGInertial* Inertial = nullptr;
  FGColumnVector3 vVel;
  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
  FGMatrix33 Ti2l;
  FGMatrix33 Tl2i;
  double epa;        // Earth Position Angle
 
  FGQuaternion Qec2b;
 
  FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
 
  eIntegrateType integrator_rotational_rate;
  eIntegrateType integrator_translational_rate;
  eIntegrateType integrator_rotational_position;
  eIntegrateType integrator_translational_position;
 
  void CalculateInertialVelocity(void);
  void CalculateUVW(void);
  void CalculateQuatdot(void);
 
  void Integrate( FGColumnVector3& Integrand,
                  FGColumnVector3& Val,
                  std::deque <FGColumnVector3>& ValDot,
                  double dt,
                  eIntegrateType integration_type);
 
  void Integrate( FGQuaternion& Integrand,
                  FGQuaternion& Val,
                  std::deque <FGQuaternion>& ValDot,
                  double dt,
                  eIntegrateType integration_type);
 
  void UpdateLocationMatrices(void);
  void UpdateBodyMatrices(void);
  void UpdateVehicleState(void);
 
  void WriteStateFile(int num);
  void bind(void);
  void Debug(int from);
};
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endif