fgaerodynamics.cpp

来自「6 DOF Missle Simulation」· C++ 代码 · 共 580 行 · 第 1/2 页

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

 Module:       FGAerodynamics.cpp
 Author:       Jon S. Berndt
 Date started: 09/13/00
 Purpose:      Encapsulates the aerodynamic forces

 ------------- Copyright (C) 2000  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
--------------------------------------------------------------------------------

HISTORY
--------------------------------------------------------------------------------
09/13/00   JSB   Created
04/22/01   JSB   Moved code into here from FGAircraft

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

#include "FGAerodynamics.h"
#include "FGPropagate.h"
#include "FGAircraft.h"
#include "FGAuxiliary.h"
#include "FGMassBalance.h"
#include <input_output/FGPropertyManager.h>

namespace JSBSim {

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

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


FGAerodynamics::FGAerodynamics(FGFDMExec* FDMExec) : FGModel(FDMExec)
{
  Name = "FGAerodynamics";

  AxisIdx["DRAG"]   = 0;
  AxisIdx["SIDE"]   = 1;
  AxisIdx["LIFT"]   = 2;
  AxisIdx["ROLL"]   = 3;
  AxisIdx["PITCH"]  = 4;
  AxisIdx["YAW"]    = 5;

  AxisIdx["AXIAL"]  = 0;
  AxisIdx["NORMAL"] = 2;

  AxisIdx["X"] = 0;
  AxisIdx["Y"] = 1;
  AxisIdx["Z"] = 2;

  axisType = atNone;

  Coeff = new CoeffArray[6];

  impending_stall = stall_hyst = 0.0;
  alphaclmin = alphaclmax = 0.0;
  alphahystmin = alphahystmax = 0.0;
  clsq = lod = 0.0;
  alphaw = 0.0;
  bi2vel = ci2vel = 0.0;
  AeroRPShift = 0;
  vDeltaRP.InitMatrix();

  bind();

  Debug(0);
}

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

FGAerodynamics::~FGAerodynamics()
{
  unsigned int i,j;

  for (i=0; i<6; i++)
    for (j=0; j<Coeff[i].size(); j++)
      delete Coeff[i][j];

  delete[] Coeff;

  for (i=0; i<variables.size(); i++)
    delete variables[i];

  delete AeroRPShift;

  Debug(1);
}

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

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

  impending_stall = stall_hyst = 0.0;
  alphaclmin = alphaclmax = 0.0;
  alphahystmin = alphahystmax = 0.0;
  clsq = lod = 0.0;
  alphaw = 0.0;
  bi2vel = ci2vel = 0.0;
  AeroRPShift = 0;
  vDeltaRP.InitMatrix();

  return true;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGAerodynamics::Run(void)
{
  unsigned int axis_ctr, ctr, i;
  double alpha, twovel;

  if (FGModel::Run()) return true;
  if (FDMExec->Holding()) return false; // if paused don't execute

  // calculate some oft-used quantities for speed

  twovel = 2*Auxiliary->GetVt();
  if (twovel != 0) {
    bi2vel = Aircraft->GetWingSpan() / twovel;
    ci2vel = Aircraft->Getcbar() / twovel;
  }
  alphaw = Auxiliary->Getalpha() + Aircraft->GetWingIncidence();
  alpha = Auxiliary->Getalpha();
  qbar_area = Aircraft->GetWingArea() * Auxiliary->Getqbar();

  if (alphaclmax != 0) {
    if (alpha > 0.85*alphaclmax) {
      impending_stall = 10*(alpha/alphaclmax - 0.85);
    } else {
      impending_stall = 0;
    }
  }

  if (alphahystmax != 0.0 && alphahystmin != 0.0) {
    if (alpha > alphahystmax) {
       stall_hyst = 1;
    } else if (alpha < alphahystmin) {
       stall_hyst = 0;
    }
  }

  vFw.InitMatrix();
  vFnative.InitMatrix();

  // Tell the variable functions to cache their values, so while the aerodynamic
  // functions are being calculated for each axis, these functions do not get
  // calculated each time, but instead use the values that have already
  // been calculated for this frame.

  for (i=0; i<variables.size(); i++) variables[i]->cacheValue(true);

  for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) {
    for (ctr=0; ctr < Coeff[axis_ctr].size(); ctr++) {
      vFnative(axis_ctr+1) += Coeff[axis_ctr][ctr]->GetValue();
    }
  }

  // Note that we still need to convert to wind axes here, because it is
  // used in the L/D calculation, and we still may want to look at Lift
  // and Drag.

  switch (axisType) {
    case atBodyXYZ:       // Forces already in body axes; no manipulation needed
      vFw = GetTb2w()*vFnative;
      vForces = vFnative;
      break;
    case atLiftDrag:      // Copy forces into wind axes
      vFw = vFnative;
      vFw(eDrag)*=-1; vFw(eLift)*=-1;
      vForces = GetTw2b()*vFw;
      break;
    case atAxialNormal:   // Convert native forces into Axial|Normal|Side system
      vFw = GetTb2w()*vFnative;
      vFnative(eX)*=-1; vFnative(eZ)*=-1;
      vForces = vFnative;
      break;
    default:
      cerr << endl << "  A proper axis type has NOT been selected. Check "
                   << "your aerodynamics definition." << endl;
      exit(-1);
  }

  // Calculate aerodynamic reference point shift, if any
  if (AeroRPShift) vDeltaRP(eX) = AeroRPShift->GetValue()*Aircraft->Getcbar()*12.0;

  // Calculate lift coefficient squared
  if ( Auxiliary->Getqbar() > 0) {
    clsq = vFw(eLift) / (Aircraft->GetWingArea()*Auxiliary->Getqbar());
    clsq *= clsq;
  }

  // Calculate lift Lift over Drag
  if ( fabs(vFw(eDrag)) > 0.0) lod = fabs( vFw(eLift) / vFw(eDrag) );

  vDXYZcg = MassBalance->StructuralToBody(Aircraft->GetXYZrp() + vDeltaRP);

  vMoments = vDXYZcg*vForces; // M = r X F

  for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) {
    for (ctr = 0; ctr < Coeff[axis_ctr+3].size(); ctr++) {
      vMoments(axis_ctr+1) += Coeff[axis_ctr+3][ctr]->GetValue();
    }
  }

  return false;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
// From Stevens and Lewis, "Aircraft Control and Simulation", 3rd Ed., the
// transformation from body to wind axes is defined (where "a" is alpha and "B"
// is beta):
//
//   cos(a)*cos(B)     sin(B)    sin(a)*cos(B)
//  -cos(a)*sin(B)     cos(B)   -sin(a)*sin(B)
//  -sin(a)              0       cos(a)
//
// The transform from wind to body axes is then,
//
//   cos(a)*cos(B)  -cos(a)*sin(B)  -sin(a)
//          sin(B)          cos(B)     0
//   sin(a)*cos(B)  -sin(a)*sin(B)   cos(a)

FGMatrix33& FGAerodynamics::GetTw2b(void)
{
  double ca, cb, sa, sb;

  double alpha = Auxiliary->Getalpha();
  double beta  = Auxiliary->Getbeta();

  ca = cos(alpha);
  sa = sin(alpha);
  cb = cos(beta);
  sb = sin(beta);

  mTw2b(1,1) = ca*cb;
  mTw2b(1,2) = -ca*sb;
  mTw2b(1,3) = -sa;
  mTw2b(2,1) = sb;
  mTw2b(2,2) = cb;
  mTw2b(2,3) = 0.0;
  mTw2b(3,1) = sa*cb;
  mTw2b(3,2) = -sa*sb;
  mTw2b(3,3) = ca;

  return mTw2b;
}

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

FGMatrix33& FGAerodynamics::GetTb2w(void)
{
  double alpha,beta;
  double ca, cb, sa, sb;

  alpha = Auxiliary->Getalpha();
  beta  = Auxiliary->Getbeta();

  ca = cos(alpha);
  sa = sin(alpha);
  cb = cos(beta);
  sb = sin(beta);

  mTb2w(1,1) = ca*cb;
  mTb2w(1,2) = sb;
  mTb2w(1,3) = sa*cb;