fgpropagate.cpp

6 DOF Missle Simulation

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

 Module:       FGPropagate.cpp
 Author:       Jon S. Berndt
 Date started: 01/05/99
 Purpose:      Integrate the EOM to determine instantaneous position
 Called by:    FGFDMExec

 ------------- 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.

FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
This class encapsulates the integration of rates and accelerations to get the
current position of the aircraft.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
COMMENTS, REFERENCES,  and NOTES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
    Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420  Naval Postgraduate
    School, January 1994
[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
    JSC 12960, July 1977
[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
    NASA-Ames", NASA CR-2497, January 1975
[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
    Wiley & Sons, 1979 ISBN 0-471-03032-5
[5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
    1982 ISBN 0-471-08936-2

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

#include <cmath>
#include <iomanip>

#include "FGPropagate.h"
#include <FGState.h>
#include <FGFDMExec.h>
#include "FGAircraft.h"
#include "FGMassBalance.h"
#include "FGInertial.h"
#include <input_output/FGPropertyManager.h>

namespace JSBSim {

static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_PROPAGATE;

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
{
  Debug(0);
  Name = "FGPropagate";

  last2_vPQRdot.InitMatrix();
  last_vPQRdot.InitMatrix();
  vPQRdot.InitMatrix();
  
  last2_vUVWdot.InitMatrix();
  last_vUVWdot.InitMatrix();
  vUVWdot.InitMatrix();
  
  last2_vLocationDot.InitMatrix();
  last_vLocationDot.InitMatrix();
  vLocationDot.InitMatrix();

  vOmegaLocal.InitMatrix();

  integrator_rotational_rate = eAdamsBashforth2;
  integrator_translational_rate = eAdamsBashforth2;
  integrator_rotational_position = eTrapezoidal;
  integrator_translational_position = eTrapezoidal;

  bind();
  Debug(0);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

FGPropagate::~FGPropagate(void)
{
  Debug(1);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGPropagate::InitModel(void)
{
  if (!FGModel::InitModel()) return false;

  SeaLevelRadius = Inertial->GetRefRadius();          // For initialization ONLY
  RunwayRadius   = SeaLevelRadius;

  VState.vLocation.SetRadius( SeaLevelRadius + 4.0 ); // Todo Add terrain elevation?
  VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
  vOmega = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector

  last2_vPQRdot.InitMatrix();
  last_vPQRdot.InitMatrix();
  vPQRdot.InitMatrix();
  
  last2_vUVWdot.InitMatrix();
  last_vUVWdot.InitMatrix();
  vUVWdot.InitMatrix();
  
  last2_vLocationDot.InitMatrix();
  last_vLocationDot.InitMatrix();
  vLocationDot.InitMatrix();

  vOmegaLocal.InitMatrix();

  integrator_rotational_rate = eAdamsBashforth2;
  integrator_translational_rate = eAdamsBashforth2;
  integrator_rotational_position = eTrapezoidal;
  integrator_translational_position = eTrapezoidal;

  return true;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
{
  SeaLevelRadius = FGIC->GetSeaLevelRadiusFtIC();
  RunwayRadius = SeaLevelRadius;

  // Set the position lat/lon/radius
  VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
                          FGIC->GetLatitudeRadIC(),
                          FGIC->GetAltitudeFtIC() + FGIC->GetSeaLevelRadiusFtIC() );

  VehicleRadius = GetRadius();
  radInv = 1.0/VehicleRadius;

  // Set the Orientation from the euler angles
  VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
                        FGIC->GetThetaRadIC(),
                        FGIC->GetPsiRadIC() );

  // Set the velocities in the instantaneus body frame
  VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
                          FGIC->GetVBodyFpsIC(),
                          FGIC->GetWBodyFpsIC() );

  // Set the angular velocities in the instantaneus body frame.
  VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
                          FGIC->GetQRadpsIC(),
                          FGIC->GetRRadpsIC() );

  // Compute the local frame ECEF velocity
  vVel = GetTb2l()*VState.vUVW;

  // Finally, make sure that the quaternion stays normalized.
  VState.vQtrn.Normalize();

  // Recompute the RunwayRadius level.
  RecomputeRunwayRadius();
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/*
Purpose: Called on a schedule to perform EOM integration
Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
         In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.

At the top of this Run() function, see several "shortcuts" (or, aliases) being
set up for use later, rather than using the longer class->function() notation.

This propagation is done using the current state values
and current derivatives. Based on these values we compute an approximation to the
state values for (now + dt).

In the code below, variables named beginning with a small "v" refer to a 
a column vector, variables named beginning with a "T" refer to a transformation
matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
Inertial.

*/

bool FGPropagate::Run(void)
{
  if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
  if (FDMExec->Holding()) return false;

  RecomputeRunwayRadius();

  // Calculate current aircraft radius from center of planet

  VehicleRadius = GetRadius();
  radInv = 1.0/VehicleRadius;

  // These local copies of the transformation matrices are for use this
  // pass through Run() only.

  Tl2b = GetTl2b();           // local to body frame transform
  Tb2l = Tl2b.Transposed();   // body to local frame transform
  Tl2ec = GetTl2ec();         // local to ECEF transform
  Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
  Tec2b = Tl2b * Tec2l;       // ECEF to body frame transform
  Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
  Ti2ec = GetTi2ec();         // ECI to ECEF transform
  Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
  Ti2b  = Tec2b*Ti2ec;        // ECI to body frame transform
  Tb2i  = Ti2b.Transposed();  // body to ECI frame transform

  // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
  vVel = Tb2l * VState.vUVW;

  // Inertial angular velocity measured in the body frame.
  vPQRi = VState.vPQR + Tec2b*vOmega;

  // Calculate state derivatives
  CalculatePQRdot();      // Angular rate derivative
  CalculateUVWdot();      // Translational rate derivative
  CalculateQuatdot();     // Angular orientation derivative
  CalculateLocationdot(); // Translational position derivative

  // Integrate to propagate the state

  double dt = State->Getdt()*rate;  // The 'stepsize'

  // Propagate rotational velocity

  switch(integrator_rotational_rate) {
  case eRectEuler:       VState.vPQR += dt*vPQRdot;
    break;
  case eTrapezoidal:     VState.vPQR += 0.5*dt*(vPQRdot + last_vPQRdot);
    break;
  case eAdamsBashforth2: VState.vPQR += dt*(1.5*vPQRdot - 0.5*last_vPQRdot);
    break;
  case eAdamsBashforth3: VState.vPQR += (1/12.0)*dt*(23.0*vPQRdot - 16.0*last_vPQRdot + 5.0*last2_vPQRdot);
    break;
  case eNone: // do nothing, freeze angular rate
    break;
  }
  
  // Propagate translational velocity

  switch(integrator_translational_rate) {
  case eRectEuler:       VState.vUVW += dt*vUVWdot;
    break;
  case eTrapezoidal:     VState.vUVW += 0.5*dt*(vUVWdot + last_vUVWdot);
    break;
  case eAdamsBashforth2: VState.vUVW += dt*(1.5*vUVWdot - 0.5*last_vUVWdot);
    break;
  case eAdamsBashforth3: VState.vUVW += (1/12.0)*dt*(23.0*vUVWdot - 16.0*last_vUVWdot + 5.0*last2_vUVWdot);
    break;
  case eNone: // do nothing, freeze translational rate
    break;
  }

  // Propagate angular position

  switch(integrator_rotational_position) {
  case eRectEuler:       VState.vQtrn += dt*vQtrndot;
    break;
  case eTrapezoidal:     VState.vQtrn += 0.5*dt*(vQtrndot + last_vQtrndot);
    break;
  case eAdamsBashforth2: VState.vQtrn += dt*(1.5*vQtrndot - 0.5*last_vQtrndot);
    break;
  case eAdamsBashforth3: VState.vQtrn += (1/12.0)*dt*(23.0*vQtrndot - 16.0*last_vQtrndot + 5.0*last2_vQtrndot);
    break;
  case eNone: // do nothing, freeze angular position
    break;
  }

  // Propagate translational position

  switch(integrator_translational_position) {
  case eRectEuler:       VState.vLocation += dt*vLocationDot;
    break;
  case eTrapezoidal:     VState.vLocation += 0.5*dt*(vLocationDot + last_vLocationDot);
    break;
  case eAdamsBashforth2: VState.vLocation += dt*(1.5*vLocationDot - 0.5*last_vLocationDot);
    break;
  case eAdamsBashforth3: VState.vLocation += (1/12.0)*dt*(23.0*vLocationDot - 16.0*last_vLocationDot + 5.0*last2_vLocationDot);
    break;
  case eNone: // do nothing, freeze translational position