src/FDM/JSBSim/models/propulsion/FGForce.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Source:       FGForce.cpp
   4  Author:       Tony Peden
   5  Date started: 6/10/00
   6
   7  ------------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) -------------
   8
   9  This program is free software; you can redistribute it and/or modify it under
  10  the terms of the GNU Lesser General Public License as published by the Free Software
  11  Foundation; either version 2 of the License, or (at your option) any later
  12  version.
  13
  14  This program is distributed in the hope that it will be useful, but WITHOUT
  15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  16  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU Lesser General Public License along with
  20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  21  Place - Suite 330, Boston, MA  02111-1307, USA.
  22
  23  Further information about the GNU Lesser General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26
  27  HISTORY
  28 --------------------------------------------------------------------------------
  29 6/10/00  TP   Created
  30
  31
  32 FUNCTIONAL DESCRIPTION
  33 --------------------------------------------------------------------------------
  34
  35 The purpose of this class is to provide storage for computed forces and
  36 encapsulate all the functionality associated with transforming those
  37 forces from their native coord system to the body system.  This includes
  38 computing the moments due to the difference between the point of application
  39 and the cg.
  40
  41 */
  42
  43 #include "FGForce.h"
  44 #include "FGFDMExec.h"
  45 #include "models/FGAircraft.h"
  46 #include "models/FGPropagate.h"
  47 #include "models/FGMassBalance.h"
  48 #include "models/FGAerodynamics.h"
  49
  50 namespace JSBSim {
  51
  52 static const char *IdSrc = "$Id$";
  53 static const char *IdHdr = ID_FORCE;
  54
  55 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  56
  57 FGForce::FGForce(FGFDMExec *FDMExec) :
  58                  fdmex(FDMExec),
  59                  ttype(tNone)
  60 {
  61   mT(1,1) = 1; //identity matrix
  62   mT(2,2) = 1;
  63   mT(3,3) = 1;
  64
  65   Debug(0);
  66 }
  67
  68 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  69
  70 FGForce::~FGForce()
  71 {
  72   Debug(1);
  73 }
  74
  75 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  76
  77 FGColumnVector3& FGForce::GetBodyForces(void)
  78 {
  79   vFb = Transform()*vFn;
  80
  81   // Find the distance from this vector's acting location to the cg; this
  82   // needs to be done like this to convert from structural to body coords.
  83   // CG and RP values are in inches
  84
  85   vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn);
  86
  87   vM = vMn + vDXYZ*vFb;
  88
  89   return vFb;
  90 }
  91
  92 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  93
  94 FGMatrix33 FGForce::Transform(void)
  95 {
  96   switch(ttype) {
  97   case tWindBody:
  98     return fdmex->GetAerodynamics()->GetTw2b();
  99   case tLocalBody:
 100     return fdmex->GetPropagate()->GetTl2b();
 101   case tCustom:
 102   case tNone:
 103     return mT;
 104   default:
 105     cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
 106     exit(1);
 107   }
 108 }
 109
 110 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 111
 112 void FGForce::UpdateCustomTransformMatrix(void)
 113 {
 114   double cp,sp,cr,sr,cy,sy;
 115   double srsp, crcy, crsy;
 116
 117   cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
 118   cr=cos(vOrient(eRoll));  sr=sin(vOrient(eRoll));
 119   cy=cos(vOrient(eYaw));   sy=sin(vOrient(eYaw));
 120
 121   srsp = sr*sp;
 122   crcy = cr*cy;
 123   crsy = cr*sy;
 124
 125   mT(1,1) =  cp*cy;
 126   mT(2,1) =  cp*sy;
 127   mT(3,1) = -sp;
 128
 129   mT(1,2) = srsp*cy - crsy;
 130   mT(2,2) = srsp*sy + crcy;
 131   mT(3,2) = sr*cp;
 132
 133   mT(1,3) = crcy*sp + sr*sy;
 134   mT(2,3) = crsy*sp - sr*cy;
 135   mT(3,3) = cr*cp;
 136 }
 137
 138 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 139
 140 void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)
 141 {
 142   if (ttype == tCustom) {
 143     vOrient(ePitch) = bpitch;
 144     vOrient(eRoll) = broll;
 145     vOrient(eYaw) = byaw;
 146
 147     UpdateCustomTransformMatrix();
 148   }
 149 }
 150
 151 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 152 //    The bitmasked value choices are as follows:
 153 //    unset: In this case (the default) JSBSim would only print
 154 //       out the normally expected messages, essentially echoing
 155 //       the config files as they are read. If the environment
 156 //       variable is not set, debug_lvl is set to 1 internally
 157 //    0: This requests JSBSim not to output any messages
 158 //       whatsoever.
 159 //    1: This value explicity requests the normal JSBSim
 160 //       startup messages
 161 //    2: This value asks for a message to be printed out when
 162 //       a class is instantiated
 163 //    4: When this value is set, a message is displayed when a
 164 //       FGModel object executes its Run() method
 165 //    8: When this value is set, various runtime state variables
 166 //       are printed out periodically
 167 //    16: When set various parameters are sanity checked and
 168 //       a message is printed out when they go out of bounds
 169
 170 void FGForce::Debug(int from)
 171 {
 172   if (debug_lvl <= 0) return;
 173
 174   if (debug_lvl & 1) { // Standard console startup message output
 175     if (from == 0) { // Constructor
 176
 177     }
 178   }
 179   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 180     if (from == 0) cout << "Instantiated: FGForce" << endl;
 181     if (from == 1) cout << "Destroyed:    FGForce" << endl;
 182   }
 183   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 184   }
 185   if (debug_lvl & 8 ) { // Runtime state variables
 186   }
 187   if (debug_lvl & 16) { // Sanity checking
 188   }
 189   if (debug_lvl & 64) {
 190     if (from == 0) { // Constructor
 191       cout << IdSrc << endl;
 192       cout << IdHdr << endl;
 193     }
 194   }
 195 }
 196 }