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 <input_output/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()
80 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
82 bool FGMassBalance::Load(Element* el)
85 string element_name = "";
86 double bixx, biyy, bizz, bixy, bixz, biyz;
88 bixx = biyy = bizz = bixy = bixz = biyz = 0.0;
89 if (el->FindElement("ixx"))
90 bixx = el->FindElementValueAsNumberConvertTo("ixx", "SLUG*FT2");
91 if (el->FindElement("iyy"))
92 biyy = el->FindElementValueAsNumberConvertTo("iyy", "SLUG*FT2");
93 if (el->FindElement("izz"))
94 bizz = el->FindElementValueAsNumberConvertTo("izz", "SLUG*FT2");
95 if (el->FindElement("ixy"))
96 bixy = el->FindElementValueAsNumberConvertTo("ixy", "SLUG*FT2");
97 if (el->FindElement("ixz"))
98 bixz = el->FindElementValueAsNumberConvertTo("ixz", "SLUG*FT2");
99 if (el->FindElement("iyz"))
100 biyz = el->FindElementValueAsNumberConvertTo("iyz", "SLUG*FT2");
101 SetAircraftBaseInertias(FGMatrix33( bixx, -bixy, -bixz,
103 -bixz, -biyz, bizz ));
104 EmptyWeight = el->FindElementValueAsNumberConvertTo("emptywt", "LBS");
106 element = el->FindElement("location");
108 element_name = element->GetAttributeValue("name");
109 if (element_name == "CG") vbaseXYZcg = element->FindElementTripletConvertTo("IN");
110 element = el->FindNextElement("location");
113 // Find all POINTMASS elements that descend from this METRICS branch of the
116 element = el->FindElement("pointmass");
118 AddPointMass(element);
119 element = el->FindNextElement("pointmass");
126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
128 bool FGMassBalance::Run(void)
130 double denom, k1, k2, k3, k4, k5, k6;
131 double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
133 if (FGModel::Run()) return true;
134 if (FDMExec->Holding()) return false;
136 Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetPointMassWeight();
138 Mass = lbtoslug*Weight;
142 // printf("%s:%i\n", __FILE__, __LINE__);
143 vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg
144 + GetPointMassMoment() ) / Weight;
146 // Calculate new total moments of inertia
148 // At first it is the base configuration inertia matrix ...
150 // ... with the additional term originating from the parallel axis theorem.
151 mJ += GetPointmassInertia( lbtoslug * EmptyWeight, vbaseXYZcg );
152 // Then add the contributions from the additional pointmasses.
153 mJ += CalculatePMInertias();
154 mJ += Propulsion->CalculateTankInertias();
163 // Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
165 k1 = (Iyy*Izz - Iyz*Iyz);
166 k2 = (Iyz*Ixz + Ixy*Izz);
167 k3 = (Ixy*Iyz + Iyy*Ixz);
169 denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 );
173 k4 = (Izz*Ixx - Ixz*Ixz)*denom;
174 k5 = (Ixy*Ixz + Iyz*Ixx)*denom;
175 k6 = (Ixx*Iyy - Ixy*Ixy)*denom;
177 mJinv.InitMatrix( k1, k2, k3,
186 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
188 void FGMassBalance::AddPointMass(Element* el)
190 Element* loc_element = el->FindElement("location");
191 string pointmass_name = el->GetAttributeValue("name");
193 cerr << "Pointmass " << pointmass_name << "has no location." << endl;
196 string loc_unit = loc_element->GetAttributeValue("unit");
199 w = el->FindElementValueAsNumberConvertTo("weight", "LBS");
200 x = loc_element->FindElementValueAsNumberConvertTo("x", loc_unit);
201 y = loc_element->FindElementValueAsNumberConvertTo("y", loc_unit);
202 z = loc_element->FindElementValueAsNumberConvertTo("z", loc_unit);
204 PointMasses.push_back(PointMass(w, x, y, z));
207 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
209 double FGMassBalance::GetPointMassWeight(void)
211 double PM_total_weight = 0.0;
213 for (unsigned int i=0; i<PointMasses.size(); i++) {
214 PM_total_weight += PointMasses[i].Weight;
216 return PM_total_weight;
219 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
221 FGColumnVector3& FGMassBalance::GetPointMassMoment(void)
223 PointMassCG.InitMatrix();
225 for (unsigned int i=0; i<PointMasses.size(); i++) {
226 PointMassCG += PointMasses[i].Weight*PointMasses[i].Location;
231 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
233 FGMatrix33& FGMassBalance::CalculatePMInertias(void)
237 size = PointMasses.size();
238 if (size == 0) return pmJ;
242 for (unsigned int i=0; i<size; i++)
243 pmJ += GetPointmassInertia( lbtoslug * PointMasses[i].Weight, PointMasses[i].Location );
248 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
250 FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const
252 // Under the assumption that in the structural frame the:
254 // - X-axis is directed afterwards,
255 // - Y-axis is directed towards the right,
256 // - Z-axis is directed upwards,
258 // (as documented in http://jsbsim.sourceforge.net/JSBSimCoordinates.pdf)
259 // we have to subtract first the center of gravity of the plane which
260 // is also defined in the structural frame:
262 // FGColumnVector3 cgOff = r - vXYZcg;
264 // Next, we do a change of units:
266 // cgOff *= inchtoft;
268 // And then a 180 degree rotation is done about the Y axis so that the:
270 // - X-axis is directed forward,
271 // - Y-axis is directed towards the right,
272 // - Z-axis is directed downward.
274 // This is needed because the structural and body frames are 180 degrees apart.
276 return FGColumnVector3(inchtoft*(vXYZcg(1)-r(1)),
277 inchtoft*(r(2)-vXYZcg(2)),
278 inchtoft*(vXYZcg(3)-r(3)));
281 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
283 void FGMassBalance::bind(void)
285 typedef double (FGMassBalance::*PMF)(int) const;
286 PropertyManager->Tie("inertia/mass-slugs", this,
287 &FGMassBalance::GetMass);
288 PropertyManager->Tie("inertia/weight-lbs", this,
289 &FGMassBalance::GetWeight);
290 PropertyManager->Tie("inertia/cg-x-ft", this,1,
291 (PMF)&FGMassBalance::GetXYZcg);
292 PropertyManager->Tie("inertia/cg-y-ft", this,2,
293 (PMF)&FGMassBalance::GetXYZcg);
294 PropertyManager->Tie("inertia/cg-z-ft", this,3,
295 (PMF)&FGMassBalance::GetXYZcg);
298 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
300 void FGMassBalance::unbind(void)
302 PropertyManager->Untie("inertia/mass-slugs");
303 PropertyManager->Untie("inertia/weight-lbs");
304 PropertyManager->Untie("inertia/cg-x-ft");
305 PropertyManager->Untie("inertia/cg-y-ft");
306 PropertyManager->Untie("inertia/cg-z-ft");
309 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
310 // The bitmasked value choices are as follows:
311 // unset: In this case (the default) JSBSim would only print
312 // out the normally expected messages, essentially echoing
313 // the config files as they are read. If the environment
314 // variable is not set, debug_lvl is set to 1 internally
315 // 0: This requests JSBSim not to output any messages
317 // 1: This value explicity requests the normal JSBSim
319 // 2: This value asks for a message to be printed out when
320 // a class is instantiated
321 // 4: When this value is set, a message is displayed when a
322 // FGModel object executes its Run() method
323 // 8: When this value is set, various runtime state variables
324 // are printed out periodically
325 // 16: When set various parameters are sanity checked and
326 // a message is printed out when they go out of bounds
328 void FGMassBalance::Debug(int from)
330 if (debug_lvl <= 0) return;
332 if (debug_lvl & 1) { // Standard console startup message output
333 if (from == 2) { // Loading
334 cout << endl << " Mass and Balance:" << endl;
335 cout << " baseIxx: " << baseJ(1,1) << " slug-ft2" << endl;
336 cout << " baseIyy: " << baseJ(2,2) << " slug-ft2" << endl;
337 cout << " baseIzz: " << baseJ(3,3) << " slug-ft2" << endl;
338 cout << " baseIxy: " << baseJ(1,2) << " slug-ft2" << endl;
339 cout << " baseIxz: " << baseJ(1,3) << " slug-ft2" << endl;
340 cout << " baseIyz: " << baseJ(2,3) << " slug-ft2" << endl;
341 cout << " EmptyWeight: " << EmptyWeight << " lbm" << endl;
342 cout << " CG (x, y, z): " << vbaseXYZcg << endl;
343 // ToDo: Need to add point mass outputs here
344 for (int i=0; i<PointMasses.size(); i++) {
345 cout << " Point Mass Object: " << PointMasses[i].Weight << " lbs. at "
346 << "X, Y, Z (in.): " << PointMasses[i].Location(eX) << " "
347 << PointMasses[i].Location(eY) << " "
348 << PointMasses[i].Location(eZ) << endl;
352 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
353 if (from == 0) cout << "Instantiated: FGMassBalance" << endl;
354 if (from == 1) cout << "Destroyed: FGMassBalance" << endl;
356 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
358 if (debug_lvl & 8 ) { // Runtime state variables
360 if (debug_lvl & 16) { // Sanity checking
362 if (EmptyWeight <= 0.0 || EmptyWeight > 1e9)
363 cout << "MassBalance::EmptyWeight out of bounds: " << EmptyWeight << endl;
364 if (Weight <= 0.0 || Weight > 1e9)
365 cout << "MassBalance::Weight out of bounds: " << Weight << endl;
366 if (Mass <= 0.0 || Mass > 1e9)
367 cout << "MassBalance::Mass out of bounds: " << Mass << endl;
370 if (debug_lvl & 64) {
371 if (from == 0) { // Constructor
372 cout << IdSrc << endl;
373 cout << IdHdr << endl;