1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 Module: FGMassBalance.cpp
5 Date started: 09/12/2000
6 Purpose: This module models weight and balance
8 ------------- Copyright (C) 2000 Jon S. Berndt (jsb@hal-pc.org) --------------
10 This program is free software; you can redistribute it and/or modify it under
11 the terms of the GNU General Public License as published by the Free Software
12 Foundation; either version 2 of the License, or (at your option) any later
15 This program is distributed in the hope that it will be useful, but WITHOUT
16 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
17 FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
20 You should have received a copy of the GNU General Public License along with
21 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
22 Place - Suite 330, Boston, MA 02111-1307, USA.
24 Further information about the GNU General Public License can also be found on
25 the world wide web at http://www.gnu.org.
27 FUNCTIONAL DESCRIPTION
28 --------------------------------------------------------------------------------
30 This class models the change in weight and balance of the aircraft due to fuel
34 --------------------------------------------------------------------------------
35 09/12/2000 JSB Created
37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
41 #include "FGMassBalance.h"
42 #include "FGPropulsion.h"
43 #include "FGPropertyManager.h"
47 static const char *IdSrc = "$Id$";
48 static const char *IdHdr = ID_MASSBALANCE;
50 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
52 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
55 FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex)
57 Name = "FGMassBalance";
58 Weight = EmptyWeight = Mass = 0.0;
60 vbaseXYZcg.InitMatrix(0.0);
71 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
73 FGMassBalance::~FGMassBalance()
79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
81 bool FGMassBalance::Run(void)
83 double denom, k1, k2, k3, k4, k5, k6;
84 double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
86 if (!FGModel::Run()) {
88 Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetPointMassWeight();
90 Mass = lbtoslug*Weight;
94 vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg
95 + GetPointMassMoment() ) / Weight;
97 // Calculate new total moments of inertia
99 // At first it is the base configuration inertia matrix ...
101 // ... with the additional term originating from the parallel axis theorem.
102 mJ += GetPointmassInertia( lbtoslug * EmptyWeight, vbaseXYZcg );
103 // Then add the contributions from the additional pointmasses.
104 mJ += CalculatePMInertias();
105 mJ += Propulsion->CalculateTankInertias();
114 // Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
116 k1 = (Iyy*Izz - Iyz*Iyz);
117 k2 = (Iyz*Ixz + Ixy*Izz);
118 k3 = (Ixy*Iyz + Iyy*Ixz);
120 denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 );
124 k4 = (Izz*Ixx - Ixz*Ixz)*denom;
125 k5 = (Ixy*Ixz + Iyz*Ixx)*denom;
126 k6 = (Ixx*Iyy - Ixy*Ixy)*denom;
128 mJinv.InitMatrix( k1, k2, k3,
140 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
142 void FGMassBalance::AddPointMass(double weight, double X, double Y, double Z)
144 PointMassLoc.push_back(FGColumnVector3(X, Y, Z));
145 PointMassWeight.push_back(weight);
148 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
150 double FGMassBalance::GetPointMassWeight(void)
152 double PM_total_weight = 0.0;
154 for (unsigned int i=0; i<PointMassWeight.size(); i++) {
155 PM_total_weight += PointMassWeight[i];
157 return PM_total_weight;
160 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
162 FGColumnVector3& FGMassBalance::GetPointMassMoment(void)
164 PointMassCG.InitMatrix();
166 for (unsigned int i=0; i<PointMassLoc.size(); i++) {
167 PointMassCG += PointMassWeight[i]*PointMassLoc[i];
172 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
174 FGMatrix33& FGMassBalance::CalculatePMInertias(void)
178 size = PointMassLoc.size();
179 if (size == 0) return pmJ;
183 for (unsigned int i=0; i<size; i++)
184 pmJ += GetPointmassInertia( lbtoslug * PointMassWeight[i], PointMassLoc[i] );
189 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
191 FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const
193 // Under the assumption that in the structural frame the:
195 // - X-axis is directed afterwards,
196 // - Y-axis is directed towards the right,
197 // - Z-axis is directed upwards,
199 // (as documented in http://jsbsim.sourceforge.net/JSBSimCoordinates.pdf)
200 // we have to subtract first the center of gravity of the plane which
201 // is also defined in the structural frame:
203 // FGColumnVector3 cgOff = r - vXYZcg;
205 // Next, we do a change of units:
207 // cgOff *= inchtoft;
209 // And then a 180 degree rotation is done about the Y axis so that the:
211 // - X-axis is directed forward,
212 // - Y-axis is directed towards the right,
213 // - Z-axis is directed downward.
215 // This is needed because the structural and body frames are 180 degrees apart.
217 return FGColumnVector3(inchtoft*(vXYZcg(1)-r(1)),
218 inchtoft*(r(2)-vXYZcg(2)),
219 inchtoft*(vXYZcg(3)-r(3)));
222 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
224 void FGMassBalance::bind(void)
226 typedef double (FGMassBalance::*PMF)(int) const;
227 PropertyManager->Tie("inertia/mass-slugs", this,
228 &FGMassBalance::GetMass);
229 PropertyManager->Tie("inertia/weight-lbs", this,
230 &FGMassBalance::GetWeight);
231 PropertyManager->Tie("inertia/cg-x-ft", this,1,
232 (PMF)&FGMassBalance::GetXYZcg);
233 PropertyManager->Tie("inertia/cg-y-ft", this,2,
234 (PMF)&FGMassBalance::GetXYZcg);
235 PropertyManager->Tie("inertia/cg-z-ft", this,3,
236 (PMF)&FGMassBalance::GetXYZcg);
239 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
241 void FGMassBalance::unbind(void)
243 PropertyManager->Untie("inertia/mass-slugs");
244 PropertyManager->Untie("inertia/weight-lbs");
245 PropertyManager->Untie("inertia/cg-x-ft");
246 PropertyManager->Untie("inertia/cg-y-ft");
247 PropertyManager->Untie("inertia/cg-z-ft");
250 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
251 // The bitmasked value choices are as follows:
252 // unset: In this case (the default) JSBSim would only print
253 // out the normally expected messages, essentially echoing
254 // the config files as they are read. If the environment
255 // variable is not set, debug_lvl is set to 1 internally
256 // 0: This requests JSBSim not to output any messages
258 // 1: This value explicity requests the normal JSBSim
260 // 2: This value asks for a message to be printed out when
261 // a class is instantiated
262 // 4: When this value is set, a message is displayed when a
263 // FGModel object executes its Run() method
264 // 8: When this value is set, various runtime state variables
265 // are printed out periodically
266 // 16: When set various parameters are sanity checked and
267 // a message is printed out when they go out of bounds
269 void FGMassBalance::Debug(int from)
271 if (debug_lvl <= 0) return;
273 if (debug_lvl & 1) { // Standard console startup message output
274 if (from == 0) { // Constructor
278 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
279 if (from == 0) cout << "Instantiated: FGMassBalance" << endl;
280 if (from == 1) cout << "Destroyed: FGMassBalance" << endl;
282 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
284 if (debug_lvl & 8 ) { // Runtime state variables
286 if (debug_lvl & 16) { // Sanity checking
288 if (EmptyWeight <= 0.0 || EmptyWeight > 1e9)
289 cout << "MassBalance::EmptyWeight out of bounds: " << EmptyWeight << endl;
290 if (Weight <= 0.0 || Weight > 1e9)
291 cout << "MassBalance::Weight out of bounds: " << Weight << endl;
292 if (Mass <= 0.0 || Mass > 1e9)
293 cout << "MassBalance::Mass out of bounds: " << Mass << endl;
296 if (debug_lvl & 64) {
297 if (from == 0) { // Constructor
298 cout << IdSrc << endl;
299 cout << IdHdr << endl;