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