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 (jon@jsbsim.org) --------------
10 This program is free software; you can redistribute it and/or modify it under
11 the terms of the GNU Lesser 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 Lesser General Public License for more
20 You should have received a copy of the GNU Lesser 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 Lesser 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 "FGBuoyantForces.h"
44 #include "input_output/FGPropertyManager.h"
52 static const char *IdSrc = "$Id$";
53 static const char *IdHdr = ID_MASSBALANCE;
55 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
57 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
60 FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex)
62 Name = "FGMassBalance";
63 Weight = EmptyWeight = Mass = 0.0;
65 vbaseXYZcg.InitMatrix(0.0);
66 vXYZcg.InitMatrix(0.0);
67 vLastXYZcg.InitMatrix(0.0);
68 vDeltaXYZcg.InitMatrix(0.0);
79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
81 FGMassBalance::~FGMassBalance()
83 for (unsigned int i=0; i<PointMasses.size(); i++) delete PointMasses[i];
89 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
91 bool FGMassBalance::InitModel(void)
93 if (!FGModel::InitModel()) return false;
95 vLastXYZcg.InitMatrix(0.0);
96 vDeltaXYZcg.InitMatrix(0.0);
101 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
103 bool FGMassBalance::Load(Element* el)
106 string element_name = "";
107 double bixx, biyy, bizz, bixy, bixz, biyz;
109 FGModel::Load(el); // Perform base class Load.
111 bixx = biyy = bizz = bixy = bixz = biyz = 0.0;
112 if (el->FindElement("ixx"))
113 bixx = el->FindElementValueAsNumberConvertTo("ixx", "SLUG*FT2");
114 if (el->FindElement("iyy"))
115 biyy = el->FindElementValueAsNumberConvertTo("iyy", "SLUG*FT2");
116 if (el->FindElement("izz"))
117 bizz = el->FindElementValueAsNumberConvertTo("izz", "SLUG*FT2");
118 if (el->FindElement("ixy"))
119 bixy = el->FindElementValueAsNumberConvertTo("ixy", "SLUG*FT2");
120 if (el->FindElement("ixz"))
121 bixz = el->FindElementValueAsNumberConvertTo("ixz", "SLUG*FT2");
122 if (el->FindElement("iyz"))
123 biyz = el->FindElementValueAsNumberConvertTo("iyz", "SLUG*FT2");
124 SetAircraftBaseInertias(FGMatrix33( bixx, -bixy, bixz,
126 bixz, -biyz, bizz ));
127 EmptyWeight = el->FindElementValueAsNumberConvertTo("emptywt", "LBS");
129 element = el->FindElement("location");
131 element_name = element->GetAttributeValue("name");
132 if (element_name == "CG") vbaseXYZcg = element->FindElementTripletConvertTo("IN");
133 element = el->FindNextElement("location");
136 // Find all POINTMASS elements that descend from this METRICS branch of the
139 element = el->FindElement("pointmass");
141 AddPointMass(element);
142 element = el->FindNextElement("pointmass");
145 double ChildFDMWeight = 0.0;
146 for (int fdm=0; fdm<FDMExec->GetFDMCount(); fdm++) {
147 if (FDMExec->GetChildFDM(fdm)->mated) ChildFDMWeight += FDMExec->GetChildFDM(fdm)->exec->GetMassBalance()->GetWeight();
150 Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetTotalPointMassWeight()
151 + BuoyantForces->GetGasMass()*slugtolb + ChildFDMWeight;
153 Mass = lbtoslug*Weight;
155 FGModel::PostLoad(el);
161 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
163 bool FGMassBalance::Run(void)
165 double denom, k1, k2, k3, k4, k5, k6;
166 double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
168 if (FGModel::Run()) return true;
169 if (FDMExec->Holding()) return false;
173 double ChildFDMWeight = 0.0;
174 for (int fdm=0; fdm<FDMExec->GetFDMCount(); fdm++) {
175 if (FDMExec->GetChildFDM(fdm)->mated) ChildFDMWeight += FDMExec->GetChildFDM(fdm)->exec->GetMassBalance()->GetWeight();
178 Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetTotalPointMassWeight()
179 + BuoyantForces->GetGasMass()*slugtolb + ChildFDMWeight;
181 Mass = lbtoslug*Weight;
185 vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg
186 + GetPointMassMoment()
187 + BuoyantForces->GetGasMassMoment()) / Weight;
189 // Track frame-by-frame delta CG, and move the EOM-tracked location
191 if (vLastXYZcg.Magnitude() == 0.0) vLastXYZcg = vXYZcg;
192 vDeltaXYZcg = vXYZcg - vLastXYZcg;
193 vDeltaXYZcgBody = StructuralToBody(vLastXYZcg) - StructuralToBody(vXYZcg);
195 Propagate->NudgeBodyLocation(vDeltaXYZcgBody);
197 // Calculate new total moments of inertia
199 // At first it is the base configuration inertia matrix ...
201 // ... with the additional term originating from the parallel axis theorem.
202 mJ += GetPointmassInertia( lbtoslug * EmptyWeight, vbaseXYZcg );
203 // Then add the contributions from the additional pointmasses.
204 mJ += CalculatePMInertias();
205 mJ += Propulsion->CalculateTankInertias();
206 mJ += BuoyantForces->GetGasMassInertia();
215 // Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
217 k1 = (Iyy*Izz - Iyz*Iyz);
218 k2 = (Iyz*Ixz + Ixy*Izz);
219 k3 = (Ixy*Iyz + Iyy*Ixz);
221 denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 );
225 k4 = (Izz*Ixx - Ixz*Ixz)*denom;
226 k5 = (Ixy*Ixz + Iyz*Ixx)*denom;
227 k6 = (Ixx*Iyy - Ixy*Ixy)*denom;
229 mJinv.InitMatrix( k1, k2, k3,
240 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
242 void FGMassBalance::AddPointMass(Element* el)
244 Element* loc_element = el->FindElement("location");
245 string pointmass_name = el->GetAttributeValue("name");
247 cerr << "Pointmass " << pointmass_name << " has no location." << endl;
251 double w = el->FindElementValueAsNumberConvertTo("weight", "LBS");
252 FGColumnVector3 vXYZ = loc_element->FindElementTripletConvertTo("IN");
254 PointMasses.push_back(new PointMass(w, vXYZ));
255 PointMasses.back()->bind(PropertyManager, PointMasses.size()-1);
258 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
260 double FGMassBalance::GetTotalPointMassWeight(void)
262 double PM_total_weight = 0.0;
264 for (unsigned int i=0; i<PointMasses.size(); i++) {
265 PM_total_weight += PointMasses[i]->Weight;
267 return PM_total_weight;
270 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
272 FGColumnVector3& FGMassBalance::GetPointMassMoment(void)
274 PointMassCG.InitMatrix();
276 for (unsigned int i=0; i<PointMasses.size(); i++) {
277 PointMassCG += PointMasses[i]->Weight*PointMasses[i]->Location;
282 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
284 FGMatrix33& FGMassBalance::CalculatePMInertias(void)
288 size = PointMasses.size();
289 if (size == 0) return pmJ;
293 for (unsigned int i=0; i<size; i++)
294 pmJ += GetPointmassInertia( lbtoslug * PointMasses[i]->Weight, PointMasses[i]->Location );
299 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
301 FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const
303 // Under the assumption that in the structural frame the:
305 // - X-axis is directed afterwards,
306 // - Y-axis is directed towards the right,
307 // - Z-axis is directed upwards,
309 // (as documented in http://jsbsim.sourceforge.net/JSBSimCoordinates.pdf)
310 // we have to subtract first the center of gravity of the plane which
311 // is also defined in the structural frame:
313 // FGColumnVector3 cgOff = r - vXYZcg;
315 // Next, we do a change of units:
317 // cgOff *= inchtoft;
319 // And then a 180 degree rotation is done about the Y axis so that the:
321 // - X-axis is directed forward,
322 // - Y-axis is directed towards the right,
323 // - Z-axis is directed downward.
325 // This is needed because the structural and body frames are 180 degrees apart.
327 return FGColumnVector3(inchtoft*(vXYZcg(1)-r(1)),
328 inchtoft*(r(2)-vXYZcg(2)),
329 inchtoft*(vXYZcg(3)-r(3)));
332 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
334 void FGMassBalance::bind(void)
336 typedef double (FGMassBalance::*PMF)(int) const;
337 PropertyManager->Tie("inertia/mass-slugs", this,
338 &FGMassBalance::GetMass);
339 PropertyManager->Tie("inertia/weight-lbs", this,
340 &FGMassBalance::GetWeight);
341 PropertyManager->Tie("inertia/empty-weight-lbs", this,
342 &FGMassBalance::GetWeight, &FGMassBalance::SetEmptyWeight);
343 PropertyManager->Tie("inertia/cg-x-in", this,1,
344 (PMF)&FGMassBalance::GetXYZcg);
345 PropertyManager->Tie("inertia/cg-y-in", this,2,
346 (PMF)&FGMassBalance::GetXYZcg);
347 PropertyManager->Tie("inertia/cg-z-in", this,3,
348 (PMF)&FGMassBalance::GetXYZcg);
351 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
353 void FGMassBalance::PointMass::bind(FGPropertyManager* PropertyManager, int num) {
354 string tmp = CreateIndexedPropertyName("inertia/pointmass-weight-lbs", num);
355 PropertyManager->Tie( tmp.c_str(), this, &PointMass::GetPointMassWeight,
356 &PointMass::SetPointMassWeight);
358 tmp = CreateIndexedPropertyName("inertia/pointmass-location-X-inches", num);
359 PropertyManager->Tie( tmp.c_str(), this, eX, &PointMass::GetPointMassLocation,
360 &PointMass::SetPointMassLocation);
361 tmp = CreateIndexedPropertyName("inertia/pointmass-location-Y-inches", num);
362 PropertyManager->Tie( tmp.c_str(), this, eY, &PointMass::GetPointMassLocation,
363 &PointMass::SetPointMassLocation);
364 tmp = CreateIndexedPropertyName("inertia/pointmass-location-Z-inches", num);
365 PropertyManager->Tie( tmp.c_str(), this, eZ, &PointMass::GetPointMassLocation,
366 &PointMass::SetPointMassLocation);
369 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
370 // The bitmasked value choices are as follows:
371 // unset: In this case (the default) JSBSim would only print
372 // out the normally expected messages, essentially echoing
373 // the config files as they are read. If the environment
374 // variable is not set, debug_lvl is set to 1 internally
375 // 0: This requests JSBSim not to output any messages
377 // 1: This value explicity requests the normal JSBSim
379 // 2: This value asks for a message to be printed out when
380 // a class is instantiated
381 // 4: When this value is set, a message is displayed when a
382 // FGModel object executes its Run() method
383 // 8: When this value is set, various runtime state variables
384 // are printed out periodically
385 // 16: When set various parameters are sanity checked and
386 // a message is printed out when they go out of bounds
388 void FGMassBalance::Debug(int from)
390 if (debug_lvl <= 0) return;
392 if (debug_lvl & 1) { // Standard console startup message output
393 if (from == 2) { // Loading
394 cout << endl << " Mass and Balance:" << endl;
395 cout << " baseIxx: " << baseJ(1,1) << " slug-ft2" << endl;
396 cout << " baseIyy: " << baseJ(2,2) << " slug-ft2" << endl;
397 cout << " baseIzz: " << baseJ(3,3) << " slug-ft2" << endl;
398 cout << " baseIxy: " << baseJ(1,2) << " slug-ft2" << endl;
399 cout << " baseIxz: " << baseJ(1,3) << " slug-ft2" << endl;
400 cout << " baseIyz: " << baseJ(2,3) << " slug-ft2" << endl;
401 cout << " EmptyWeight: " << EmptyWeight << " lbm" << endl;
402 cout << " CG (x, y, z): " << vbaseXYZcg << endl;
403 // ToDo: Need to add point mass outputs here
404 for (unsigned int i=0; i<PointMasses.size(); i++) {
405 cout << " Point Mass Object: " << PointMasses[i]->Weight << " lbs. at "
406 << "X, Y, Z (in.): " << PointMasses[i]->Location(eX) << " "
407 << PointMasses[i]->Location(eY) << " "
408 << PointMasses[i]->Location(eZ) << endl;
412 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
413 if (from == 0) cout << "Instantiated: FGMassBalance" << endl;
414 if (from == 1) cout << "Destroyed: FGMassBalance" << endl;
416 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
418 if (debug_lvl & 8 ) { // Runtime state variables
420 if (debug_lvl & 16) { // Sanity checking
422 if (EmptyWeight <= 0.0 || EmptyWeight > 1e9)
423 cout << "MassBalance::EmptyWeight out of bounds: " << EmptyWeight << endl;
424 if (Weight <= 0.0 || Weight > 1e9)
425 cout << "MassBalance::Weight out of bounds: " << Weight << endl;
426 if (Mass <= 0.0 || Mass > 1e9)
427 cout << "MassBalance::Mass out of bounds: " << Mass << endl;
430 if (debug_lvl & 64) {
431 if (from == 0) { // Constructor
432 cout << IdSrc << endl;
433 cout << IdHdr << endl;