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 #include <iostream>
  50 #include <cstdlib>
  51
  52 using namespace std;
  53
  54 namespace JSBSim {
  55
  56 static const char *IdSrc = "$Id: FGForce.cpp,v 1.14 2009/10/24 22:59:30 jberndt Exp $";
  57 static const char *IdHdr = ID_FORCE;
  58
  59 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  60
  61 FGForce::FGForce(FGFDMExec *FDMExec) :
  62                  fdmex(FDMExec),
  63                  ttype(tNone)
  64 {
  65   mT(1,1) = 1; //identity matrix
  66   mT(2,2) = 1;
  67   mT(3,3) = 1;
  68
  69   Debug(0);
  70 }
  71
  72 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  73
  74 FGForce::~FGForce()
  75 {
  76   Debug(1);
  77 }
  78
  79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  80
  81 FGColumnVector3& FGForce::GetBodyForces(void)
  82 {
  83   vFb = Transform()*vFn;
  84
  85   // Find the distance from this vector's acting location to the cg; this
  86   // needs to be done like this to convert from structural to body coords.
  87   // CG and RP values are in inches
  88
  89   vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn);
  90
  91   vM = vMn + vDXYZ*vFb;
  92
  93   return vFb;
  94 }
  95
  96 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  97
  98 FGMatrix33 FGForce::Transform(void)
  99 {
 100   switch(ttype) {
 101   case tWindBody:
 102     return fdmex->GetAerodynamics()->GetTw2b();
 103   case tLocalBody:
 104     return fdmex->GetPropagate()->GetTl2b();
 105   case tCustom:
 106   case tNone:
 107     return mT;
 108   default:
 109     cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
 110     exit(1);
 111   }
 112 }
 113
 114 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 115
 116 void FGForce::UpdateCustomTransformMatrix(void)
 117 {
 118   double cp,sp,cr,sr,cy,sy;
 119   double srsp, crcy, crsy;
 120
 121   cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
 122   cr=cos(vOrient(eRoll));  sr=sin(vOrient(eRoll));
 123   cy=cos(vOrient(eYaw));   sy=sin(vOrient(eYaw));
 124
 125   srsp = sr*sp;
 126   crcy = cr*cy;
 127   crsy = cr*sy;
 128
 129   mT(1,1) =  cp*cy;
 130   mT(2,1) =  cp*sy;
 131   mT(3,1) = -sp;
 132
 133   mT(1,2) = srsp*cy - crsy;
 134   mT(2,2) = srsp*sy + crcy;
 135   mT(3,2) = sr*cp;
 136
 137   mT(1,3) = crcy*sp + sr*sy;
 138   mT(2,3) = crsy*sp - sr*cy;
 139   mT(3,3) = cr*cp;
 140 }
 141
 142 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 143
 144 void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)
 145 {
 146   if (ttype == tCustom) {
 147     vOrient(ePitch) = bpitch;
 148     vOrient(eRoll) = broll;
 149     vOrient(eYaw) = byaw;
 150
 151     UpdateCustomTransformMatrix();
 152   }
 153 }
 154
 155 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 156 //    The bitmasked value choices are as follows:
 157 //    unset: In this case (the default) JSBSim would only print
 158 //       out the normally expected messages, essentially echoing
 159 //       the config files as they are read. If the environment
 160 //       variable is not set, debug_lvl is set to 1 internally
 161 //    0: This requests JSBSim not to output any messages
 162 //       whatsoever.
 163 //    1: This value explicity requests the normal JSBSim
 164 //       startup messages
 165 //    2: This value asks for a message to be printed out when
 166 //       a class is instantiated
 167 //    4: When this value is set, a message is displayed when a
 168 //       FGModel object executes its Run() method
 169 //    8: When this value is set, various runtime state variables
 170 //       are printed out periodically
 171 //    16: When set various parameters are sanity checked and
 172 //       a message is printed out when they go out of bounds
 173
 174 void FGForce::Debug(int from)
 175 {
 176   if (debug_lvl <= 0) return;
 177
 178   if (debug_lvl & 1) { // Standard console startup message output
 179     if (from == 0) { // Constructor
 180
 181     }
 182   }
 183   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 184     if (from == 0) cout << "Instantiated: FGForce" << endl;
 185     if (from == 1) cout << "Destroyed:    FGForce" << endl;
 186   }
 187   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 188   }
 189   if (debug_lvl & 8 ) { // Runtime state variables
 190   }
 191   if (debug_lvl & 16) { // Sanity checking
 192   }
 193   if (debug_lvl & 64) {
 194     if (from == 0) { // Constructor
 195       cout << IdSrc << endl;
 196       cout << IdHdr << endl;
 197     }
 198   }
 199 }
 200 }