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 "propulsion/FGTank.h"
44 #include "FGBuoyantForces.h"
45 #include "input_output/FGPropertyManager.h"
54 static const char *IdSrc = "$Id: FGMassBalance.cpp,v 1.35 2011/05/20 03:18:36 jberndt Exp $";
55 static const char *IdHdr = ID_MASSBALANCE;
57 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
59 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
62 FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex)
64 Name = "FGMassBalance";
65 Weight = EmptyWeight = Mass = 0.0;
67 vbaseXYZcg.InitMatrix(0.0);
68 vXYZcg.InitMatrix(0.0);
69 vLastXYZcg.InitMatrix(0.0);
70 vDeltaXYZcg.InitMatrix(0.0);
81 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
83 FGMassBalance::~FGMassBalance()
85 for (unsigned int i=0; i<PointMasses.size(); i++) delete PointMasses[i];
91 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
93 bool FGMassBalance::InitModel(void)
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 if (el->FindElement("emptywt")) {
128 EmptyWeight = el->FindElementValueAsNumberConvertTo("emptywt", "LBS");
131 element = el->FindElement("location");
133 element_name = element->GetAttributeValue("name");
134 if (element_name == "CG") vbaseXYZcg = element->FindElementTripletConvertTo("IN");
135 element = el->FindNextElement("location");
138 // Find all POINTMASS elements that descend from this METRICS branch of the
141 element = el->FindElement("pointmass");
143 AddPointMass(element);
144 element = el->FindNextElement("pointmass");
147 double ChildFDMWeight = 0.0;
148 for (int fdm=0; fdm<FDMExec->GetFDMCount(); fdm++) {
149 if (FDMExec->GetChildFDM(fdm)->mated) ChildFDMWeight += FDMExec->GetChildFDM(fdm)->exec->GetMassBalance()->GetWeight();
152 Weight = EmptyWeight + FDMExec->GetPropulsion()->GetTanksWeight() + GetTotalPointMassWeight()
153 + FDMExec->GetBuoyantForces()->GetGasMass()*slugtolb + ChildFDMWeight;
155 Mass = lbtoslug*Weight;
157 PostLoad(el, PropertyManager);
163 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
165 bool FGMassBalance::Run(bool Holding)
167 double denom, k1, k2, k3, k4, k5, k6;
168 double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
170 if (FGModel::Run(Holding)) return true;
171 if (Holding) return false;
175 double ChildFDMWeight = 0.0;
176 for (int fdm=0; fdm<FDMExec->GetFDMCount(); fdm++) {
177 if (FDMExec->GetChildFDM(fdm)->mated) ChildFDMWeight += FDMExec->GetChildFDM(fdm)->exec->GetMassBalance()->GetWeight();
180 Weight = EmptyWeight + FDMExec->GetPropulsion()->GetTanksWeight() + GetTotalPointMassWeight()
181 + FDMExec->GetBuoyantForces()->GetGasMass()*slugtolb + ChildFDMWeight;
183 Mass = lbtoslug*Weight;
187 vXYZcg = (FDMExec->GetPropulsion()->GetTanksMoment() + EmptyWeight*vbaseXYZcg
188 + GetPointMassMoment()
189 + FDMExec->GetBuoyantForces()->GetGasMassMoment()) / Weight;
191 // Track frame-by-frame delta CG, and move the EOM-tracked location
193 if (vLastXYZcg.Magnitude() == 0.0) vLastXYZcg = vXYZcg;
194 vDeltaXYZcg = vXYZcg - vLastXYZcg;
195 vDeltaXYZcgBody = StructuralToBody(vLastXYZcg) - StructuralToBody(vXYZcg);
197 FDMExec->GetPropagate()->NudgeBodyLocation(vDeltaXYZcgBody);
199 // Calculate new total moments of inertia
201 // At first it is the base configuration inertia matrix ...
203 // ... with the additional term originating from the parallel axis theorem.
204 mJ += GetPointmassInertia( lbtoslug * EmptyWeight, vbaseXYZcg );
205 // Then add the contributions from the additional pointmasses.
206 mJ += CalculatePMInertias();
207 mJ += FDMExec->GetPropulsion()->CalculateTankInertias();
208 mJ += FDMExec->GetBuoyantForces()->GetGasMassInertia();
217 // Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
219 k1 = (Iyy*Izz - Iyz*Iyz);
220 k2 = (Iyz*Ixz + Ixy*Izz);
221 k3 = (Ixy*Iyz + Iyy*Ixz);
223 denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 );
227 k4 = (Izz*Ixx - Ixz*Ixz)*denom;
228 k5 = (Ixy*Ixz + Iyz*Ixx)*denom;
229 k6 = (Ixx*Iyy - Ixy*Ixy)*denom;
231 mJinv.InitMatrix( k1, k2, k3,
242 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
244 void FGMassBalance::AddPointMass(Element* el)
246 double radius=0, length=0;
247 Element* loc_element = el->FindElement("location");
248 string pointmass_name = el->GetAttributeValue("name");
250 cerr << "Pointmass " << pointmass_name << " has no location." << endl;
254 double w = el->FindElementValueAsNumberConvertTo("weight", "LBS");
255 FGColumnVector3 vXYZ = loc_element->FindElementTripletConvertTo("IN");
257 PointMass *pm = new PointMass(w, vXYZ);
258 pm->SetName(pointmass_name);
260 Element* form_element = el->FindElement("form");
262 string shape = form_element->GetAttributeValue("shape");
263 Element* radius_element = form_element->FindElement("radius");
264 Element* length_element = form_element->FindElement("length");
265 if (radius_element) radius = form_element->FindElementValueAsNumberConvertTo("radius", "FT");
266 if (length_element) length = form_element->FindElementValueAsNumberConvertTo("length", "FT");
267 if (shape == "tube") {
268 pm->SetPointMassShapeType(PointMass::esTube);
269 pm->SetRadius(radius);
270 pm->SetLength(length);
271 pm->CalculateShapeInertia();
272 } else if (shape == "cylinder") {
273 pm->SetPointMassShapeType(PointMass::esCylinder);
274 pm->SetRadius(radius);
275 pm->SetLength(length);
276 pm->CalculateShapeInertia();
277 } else if (shape == "sphere") {
278 pm->SetPointMassShapeType(PointMass::esSphere);
279 pm->SetRadius(radius);
280 pm->CalculateShapeInertia();
281 } else if (shape == "ball") {
282 pm->SetPointMassShapeType(PointMass::esBall);
283 pm->SetRadius(radius);
284 pm->CalculateShapeInertia();
289 pm->bind(PropertyManager, PointMasses.size());
290 PointMasses.push_back(pm);
293 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
295 double FGMassBalance::GetTotalPointMassWeight(void)
297 double PM_total_weight = 0.0;
299 for (unsigned int i=0; i<PointMasses.size(); i++) {
300 PM_total_weight += PointMasses[i]->Weight;
302 return PM_total_weight;
305 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
307 FGColumnVector3& FGMassBalance::GetPointMassMoment(void)
309 PointMassCG.InitMatrix();
311 for (unsigned int i=0; i<PointMasses.size(); i++) {
312 PointMassCG += PointMasses[i]->Weight*PointMasses[i]->Location;
317 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
319 FGMatrix33& FGMassBalance::CalculatePMInertias(void)
323 size = PointMasses.size();
324 if (size == 0) return pmJ;
328 for (unsigned int i=0; i<size; i++) {
329 pmJ += GetPointmassInertia( lbtoslug * PointMasses[i]->Weight, PointMasses[i]->Location );
330 pmJ += PointMasses[i]->GetPointMassInertia();
336 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
338 FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const
340 // Under the assumption that in the structural frame the:
342 // - X-axis is directed afterwards,
343 // - Y-axis is directed towards the right,
344 // - Z-axis is directed upwards,
346 // (as documented in http://jsbsim.sourceforge.net/JSBSimCoordinates.pdf)
347 // we have to subtract first the center of gravity of the plane which
348 // is also defined in the structural frame:
350 // FGColumnVector3 cgOff = r - vXYZcg;
352 // Next, we do a change of units:
354 // cgOff *= inchtoft;
356 // And then a 180 degree rotation is done about the Y axis so that the:
358 // - X-axis is directed forward,
359 // - Y-axis is directed towards the right,
360 // - Z-axis is directed downward.
362 // This is needed because the structural and body frames are 180 degrees apart.
364 return FGColumnVector3(inchtoft*(vXYZcg(1)-r(1)),
365 inchtoft*(r(2)-vXYZcg(2)),
366 inchtoft*(vXYZcg(3)-r(3)));
369 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
371 void FGMassBalance::bind(void)
373 typedef double (FGMassBalance::*PMF)(int) const;
374 PropertyManager->Tie("inertia/mass-slugs", this,
375 &FGMassBalance::GetMass);
376 PropertyManager->Tie("inertia/weight-lbs", this,
377 &FGMassBalance::GetWeight);
378 PropertyManager->Tie("inertia/empty-weight-lbs", this,
379 &FGMassBalance::GetEmptyWeight);
380 PropertyManager->Tie("inertia/cg-x-in", this,1,
381 (PMF)&FGMassBalance::GetXYZcg);
382 PropertyManager->Tie("inertia/cg-y-in", this,2,
383 (PMF)&FGMassBalance::GetXYZcg);
384 PropertyManager->Tie("inertia/cg-z-in", this,3,
385 (PMF)&FGMassBalance::GetXYZcg);
388 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
390 // This function binds properties for each pointmass object created.
392 void FGMassBalance::PointMass::bind(FGPropertyManager* PropertyManager, int num) {
393 string tmp = CreateIndexedPropertyName("inertia/pointmass-weight-lbs", num);
394 PropertyManager->Tie( tmp.c_str(), this, &PointMass::GetPointMassWeight,
395 &PointMass::SetPointMassWeight);
397 tmp = CreateIndexedPropertyName("inertia/pointmass-location-X-inches", num);
398 PropertyManager->Tie( tmp.c_str(), this, eX, &PointMass::GetPointMassLocation,
399 &PointMass::SetPointMassLocation);
400 tmp = CreateIndexedPropertyName("inertia/pointmass-location-Y-inches", num);
401 PropertyManager->Tie( tmp.c_str(), this, eY, &PointMass::GetPointMassLocation,
402 &PointMass::SetPointMassLocation);
403 tmp = CreateIndexedPropertyName("inertia/pointmass-location-Z-inches", num);
404 PropertyManager->Tie( tmp.c_str(), this, eZ, &PointMass::GetPointMassLocation,
405 &PointMass::SetPointMassLocation);
408 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
410 void FGMassBalance::GetMassPropertiesReport(void) const
412 cout << endl << fgblue << highint
413 << " Mass Properties Report (English units: lbf, in, slug-ft^2)"
415 cout << " " << underon << " Weight CG-X CG-Y"
416 << " CG-Z Ixx Iyy Izz" << underoff << endl;
418 cout << highint << setw(34) << left << " Base Vehicle " << normint
419 << right << setw(10) << EmptyWeight << setw(8) << vbaseXYZcg(eX) << setw(8)
420 << vbaseXYZcg(eY) << setw(8) << vbaseXYZcg(eZ) << setw(12) << baseJ(1,1)
421 << setw(12) << baseJ(2,2) << setw(12) << baseJ(3,3) << endl;
423 for (unsigned int i=0;i<PointMasses.size();i++) {
424 PointMass* pm = PointMasses[i];
425 double pmweight = pm->GetPointMassWeight();
426 cout << highint << left << setw(4) << i << setw(30) << pm->GetName() << normint
427 << right << setw(10) << pmweight << setw(8) << pm->GetLocation()(eX)
428 << setw(8) << pm->GetLocation()(eY) << setw(8) << pm->GetLocation()(eZ)
429 << setw(12) << pm->GetPointMassMoI(1,1) << setw(12) << pm->GetPointMassMoI(2,2)
430 << setw(12) << pm->GetPointMassMoI(3,3) << endl;
433 for (unsigned int i=0;i<FDMExec->GetPropulsion()->GetNumTanks() ;i++) {
434 FGTank* tank = FDMExec->GetPropulsion()->GetTank(i);
436 if (tank->GetType() == FGTank::ttFUEL && tank->GetGrainType() != FGTank::gtUNKNOWN) {
437 tankname = "Solid Fuel";
438 } else if (tank->GetType() == FGTank::ttFUEL) {
440 } else if (tank->GetType() == FGTank::ttOXIDIZER) {
441 tankname = "Oxidizer";
443 tankname = "(Unknown tank type)";
445 cout << highint << left << setw(4) << i << setw(30) << tankname << normint
446 << right << setw(10) << tank->GetContents() << setw(8) << tank->GetXYZ(eX)
447 << setw(8) << tank->GetXYZ(eY) << setw(8) << tank->GetXYZ(eZ)
448 << setw(12) << "*" << setw(12) << "*"
449 << setw(12) << "*" << endl;
452 cout << underon << setw(104) << " " << underoff << endl;
453 cout << highint << left << setw(30) << " Total: " << right << setw(14) << Weight
454 << setw(8) << vXYZcg(eX)
455 << setw(8) << vXYZcg(eY)
456 << setw(8) << vXYZcg(eZ)
457 << setw(12) << mJ(1,1)
458 << setw(12) << mJ(2,2)
459 << setw(12) << mJ(3,3)
462 cout.setf(ios_base::fixed);
465 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
466 // The bitmasked value choices are as follows:
467 // unset: In this case (the default) JSBSim would only print
468 // out the normally expected messages, essentially echoing
469 // the config files as they are read. If the environment
470 // variable is not set, debug_lvl is set to 1 internally
471 // 0: This requests JSBSim not to output any messages
473 // 1: This value explicity requests the normal JSBSim
475 // 2: This value asks for a message to be printed out when
476 // a class is instantiated
477 // 4: When this value is set, a message is displayed when a
478 // FGModel object executes its Run() method
479 // 8: When this value is set, various runtime state variables
480 // are printed out periodically
481 // 16: When set various parameters are sanity checked and
482 // a message is printed out when they go out of bounds
484 void FGMassBalance::Debug(int from)
486 if (debug_lvl <= 0) return;
488 if (debug_lvl & 1) { // Standard console startup message output
489 if (from == 2) { // Loading
490 cout << endl << " Mass and Balance:" << endl;
491 cout << " baseIxx: " << baseJ(1,1) << " slug-ft2" << endl;
492 cout << " baseIyy: " << baseJ(2,2) << " slug-ft2" << endl;
493 cout << " baseIzz: " << baseJ(3,3) << " slug-ft2" << endl;
494 cout << " baseIxy: " << baseJ(1,2) << " slug-ft2" << endl;
495 cout << " baseIxz: " << baseJ(1,3) << " slug-ft2" << endl;
496 cout << " baseIyz: " << baseJ(2,3) << " slug-ft2" << endl;
497 cout << " Empty Weight: " << EmptyWeight << " lbm" << endl;
498 cout << " CG (x, y, z): " << vbaseXYZcg << endl;
499 // ToDo: Need to add point mass outputs here
500 for (unsigned int i=0; i<PointMasses.size(); i++) {
501 cout << " Point Mass Object: " << PointMasses[i]->Weight << " lbs. at "
502 << "X, Y, Z (in.): " << PointMasses[i]->Location(eX) << " "
503 << PointMasses[i]->Location(eY) << " "
504 << PointMasses[i]->Location(eZ) << endl;
508 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
509 if (from == 0) cout << "Instantiated: FGMassBalance" << endl;
510 if (from == 1) cout << "Destroyed: FGMassBalance" << endl;
512 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
514 if (debug_lvl & 8 ) { // Runtime state variables
516 if (debug_lvl & 16) { // Sanity checking
518 if (EmptyWeight <= 0.0 || EmptyWeight > 1e9)
519 cout << "MassBalance::EmptyWeight out of bounds: " << EmptyWeight << endl;
520 if (Weight <= 0.0 || Weight > 1e9)
521 cout << "MassBalance::Weight out of bounds: " << Weight << endl;
522 if (Mass <= 0.0 || Mass > 1e9)
523 cout << "MassBalance::Mass out of bounds: " << Mass << endl;
526 if (debug_lvl & 64) {
527 if (from == 0) { // Constructor
528 cout << IdSrc << endl;
529 cout << IdHdr << endl;