Branch data Line data Source code
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 : 12 : FGForce::FGForce(FGFDMExec *FDMExec) :
62 : : fdmex(FDMExec),
63 : 12 : ttype(tNone)
64 : : {
65 : 24 : mT(1,1) = 1; //identity matrix
66 : 24 : mT(2,2) = 1;
67 : 24 : mT(3,3) = 1;
68 : :
69 : 12 : Debug(0);
70 : 12 : }
71 : :
72 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
73 : :
74 : 12 : FGForce::~FGForce()
75 : : {
76 : 12 : Debug(1);
77 : 12 : }
78 : :
79 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
80 : :
81 : 648060 : FGColumnVector3& FGForce::GetBodyForces(void)
82 : : {
83 : 648060 : 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 : 648060 : vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn);
90 : :
91 : 1944180 : vM = vMn + vDXYZ*vFb;
92 : :
93 : 648060 : return vFb;
94 : : }
95 : :
96 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
97 : :
98 : 648060 : FGMatrix33 FGForce::Transform(void)
99 : : {
100 [ - - + - ]: 648060 : switch(ttype) {
101 : : case tWindBody:
102 : 0 : return fdmex->GetAerodynamics()->GetTw2b();
103 : : case tLocalBody:
104 : 0 : return fdmex->GetPropagate()->GetTl2b();
105 : : case tCustom:
106 : : case tNone:
107 : 648060 : return mT;
108 : : default:
109 : 0 : cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
110 : 648060 : exit(1);
111 : : }
112 : : }
113 : :
114 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
115 : :
116 : 648072 : void FGForce::UpdateCustomTransformMatrix(void)
117 : : {
118 : : double cp,sp,cr,sr,cy,sy;
119 : : double srsp, crcy, crsy;
120 : :
121 : 648072 : cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
122 : 648072 : cr=cos(vOrient(eRoll)); sr=sin(vOrient(eRoll));
123 : 648072 : cy=cos(vOrient(eYaw)); sy=sin(vOrient(eYaw));
124 : :
125 : 648072 : srsp = sr*sp;
126 : 648072 : crcy = cr*cy;
127 : 648072 : crsy = cr*sy;
128 : :
129 : 1296144 : mT(1,1) = cp*cy;
130 : 1296144 : mT(2,1) = cp*sy;
131 : 1296144 : mT(3,1) = -sp;
132 : :
133 : 1296144 : mT(1,2) = srsp*cy - crsy;
134 : 1296144 : mT(2,2) = srsp*sy + crcy;
135 : 1296144 : mT(3,2) = sr*cp;
136 : :
137 : 1296144 : mT(1,3) = crcy*sp + sr*sy;
138 : 1296144 : mT(2,3) = crsy*sp - sr*cy;
139 : 1296144 : mT(3,3) = cr*cp;
140 : 648072 : }
141 : :
142 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
143 : :
144 : 12 : void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)
145 : : {
146 [ + - ]: 12 : if (ttype == tCustom) {
147 : 24 : vOrient(ePitch) = bpitch;
148 : 24 : vOrient(eRoll) = broll;
149 : 24 : vOrient(eYaw) = byaw;
150 : :
151 : 12 : UpdateCustomTransformMatrix();
152 : : }
153 : 12 : }
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 : 24 : void FGForce::Debug(int from)
175 : : {
176 [ + - ]: 24 : if (debug_lvl <= 0) return;
177 : :
178 : 24 : if (debug_lvl & 1) { // Standard console startup message output
179 : : if (from == 0) { // Constructor
180 : :
181 : : }
182 : : }
183 [ - + ]: 24 : if (debug_lvl & 2 ) { // Instantiation/Destruction notification
184 [ # # ]: 0 : if (from == 0) cout << "Instantiated: FGForce" << endl;
185 [ # # ]: 0 : if (from == 1) cout << "Destroyed: FGForce" << endl;
186 : : }
187 : 24 : if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
188 : : }
189 : 24 : if (debug_lvl & 8 ) { // Runtime state variables
190 : : }
191 : 24 : if (debug_lvl & 16) { // Sanity checking
192 : : }
193 [ - + ]: 24 : if (debug_lvl & 64) {
194 [ # # ]: 0 : if (from == 0) { // Constructor
195 : 0 : cout << IdSrc << endl;
196 : 0 : cout << IdHdr << endl;
197 : : }
198 : : }
199 : : }
200 [ + + ][ + - ]: 12 : }
|
