src/FDM/JSBSim/models/FGMassBalance.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGMassBalance.cpp
   4  Author:       Jon S. Berndt
   5  Date started: 09/12/2000
   6  Purpose:      This module models weight and balance
   7
   8  ------------- Copyright (C) 2000  Jon S. Berndt (jsb@hal-pc.org) --------------
   9
  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
  13  version.
  14
  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
  18  details.
  19
  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.
  23
  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.
  26
  27 FUNCTIONAL DESCRIPTION
  28 --------------------------------------------------------------------------------
  29
  30 This class models the change in weight and balance of the aircraft due to fuel
  31 burnoff, etc.
  32
  33 HISTORY
  34 --------------------------------------------------------------------------------
  35 09/12/2000  JSB  Created
  36
  37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  38 INCLUDES
  39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  40
  41 #include "FGMassBalance.h"
  42 #include "FGPropulsion.h"
  43 #include "FGBuoyantForces.h"
  44 #include <input_output/FGPropertyManager.h>
  45
  46 namespace JSBSim {
  47
  48 static const char *IdSrc = "$Id$";
  49 static const char *IdHdr = ID_MASSBALANCE;
  50
  51 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  52 CLASS IMPLEMENTATION
  53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  54
  55
  56 FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex)
  57 {
  58   Name = "FGMassBalance";
  59   Weight = EmptyWeight = Mass = 0.0;
  60
  61   vbaseXYZcg.InitMatrix(0.0);
  62   baseJ.InitMatrix();
  63   mJ.InitMatrix();
  64   mJinv.InitMatrix();
  65   pmJ.InitMatrix();
  66
  67   bind();
  68
  69   Debug(0);
  70 }
  71
  72 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  73
  74 FGMassBalance::~FGMassBalance()
  75 {
  76   for (unsigned int i=0; i<PointMasses.size(); i++) delete PointMasses[i];
  77   PointMasses.clear();
  78
  79   Debug(1);
  80 }
  81
  82 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  83
  84 bool FGMassBalance::InitModel(void)
  85 {
  86   if (!FGModel::InitModel()) return false;
  87
  88   return true;
  89 }
  90
  91 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  92
  93 bool FGMassBalance::Load(Element* el)
  94 {
  95   Element *element;
  96   string element_name = "";
  97   double bixx, biyy, bizz, bixy, bixz, biyz;
  98
  99   bixx = biyy = bizz = bixy = bixz = biyz = 0.0;
 100   if (el->FindElement("ixx"))
 101     bixx = el->FindElementValueAsNumberConvertTo("ixx", "SLUG*FT2");
 102   if (el->FindElement("iyy"))
 103     biyy = el->FindElementValueAsNumberConvertTo("iyy", "SLUG*FT2");
 104   if (el->FindElement("izz"))
 105     bizz = el->FindElementValueAsNumberConvertTo("izz", "SLUG*FT2");
 106   if (el->FindElement("ixy"))
 107     bixy = el->FindElementValueAsNumberConvertTo("ixy", "SLUG*FT2");
 108   if (el->FindElement("ixz"))
 109     bixz = el->FindElementValueAsNumberConvertTo("ixz", "SLUG*FT2");
 110   if (el->FindElement("iyz"))
 111     biyz = el->FindElementValueAsNumberConvertTo("iyz", "SLUG*FT2");
 112   SetAircraftBaseInertias(FGMatrix33(  bixx,  -bixy,  bixz,
 113                                       -bixy,  biyy,  -biyz,
 114                                        bixz,  -biyz,  bizz ));
 115   EmptyWeight = el->FindElementValueAsNumberConvertTo("emptywt", "LBS");
 116
 117   element = el->FindElement("location");
 118   while (element) {
 119     element_name = element->GetAttributeValue("name");
 120     if (element_name == "CG") vbaseXYZcg = element->FindElementTripletConvertTo("IN");
 121     element = el->FindNextElement("location");
 122   }
 123
 124 // Find all POINTMASS elements that descend from this METRICS branch of the
 125 // config file.
 126
 127   element = el->FindElement("pointmass");
 128   while (element) {
 129     AddPointMass(element);
 130     element = el->FindNextElement("pointmass");
 131   }
 132
 133   double ChildFDMWeight = 0.0;
 134   for (int fdm=0; fdm<FDMExec->GetFDMCount(); fdm++) {
 135     if (FDMExec->GetChildFDM(fdm)->mated) ChildFDMWeight += FDMExec->GetChildFDM(fdm)->exec->GetMassBalance()->GetWeight();
 136   }
 137
 138   Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetTotalPointMassWeight()
 139     + BuoyantForces->GetGasMass()*slugtolb + ChildFDMWeight;
 140
 141   Mass = lbtoslug*Weight;
 142
 143   Debug(2);
 144   return true;
 145 }
 146
 147 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 148
 149 bool FGMassBalance::Run(void)
 150 {
 151   double denom, k1, k2, k3, k4, k5, k6;
 152   double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
 153
 154   if (FGModel::Run()) return true;
 155   if (FDMExec->Holding()) return false;
 156
 157   double ChildFDMWeight = 0.0;
 158   for (int fdm=0; fdm<FDMExec->GetFDMCount(); fdm++) {
 159     if (FDMExec->GetChildFDM(fdm)->mated) ChildFDMWeight += FDMExec->GetChildFDM(fdm)->exec->GetMassBalance()->GetWeight();
 160   }
 161
 162   Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetTotalPointMassWeight()
 163     + BuoyantForces->GetGasMass()*slugtolb + ChildFDMWeight;
 164
 165   Mass = lbtoslug*Weight;
 166
 167 // Calculate new CG
 168
 169   vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg
 170             + GetPointMassMoment()
 171             + BuoyantForces->GetGasMassMoment()) / Weight;
 172
 173 // Calculate new total moments of inertia
 174
 175   // At first it is the base configuration inertia matrix ...
 176   mJ = baseJ;
 177   // ... with the additional term originating from the parallel axis theorem.
 178   mJ += GetPointmassInertia( lbtoslug * EmptyWeight, vbaseXYZcg );
 179   // Then add the contributions from the additional pointmasses.
 180   mJ += CalculatePMInertias();
 181   mJ += Propulsion->CalculateTankInertias();
 182   mJ += BuoyantForces->GetGasMassInertia();
 183
 184   Ixx = mJ(1,1);
 185   Iyy = mJ(2,2);
 186   Izz = mJ(3,3);
 187   Ixy = -mJ(1,2);
 188   Ixz = -mJ(1,3);
 189   Iyz = -mJ(2,3);
 190
 191 // Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
 192
 193   k1 = (Iyy*Izz - Iyz*Iyz);
 194   k2 = (Iyz*Ixz + Ixy*Izz);
 195   k3 = (Ixy*Iyz + Iyy*Ixz);
 196
 197   denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 );
 198   k1 = k1*denom;
 199   k2 = k2*denom;
 200   k3 = k3*denom;
 201   k4 = (Izz*Ixx - Ixz*Ixz)*denom;
 202   k5 = (Ixy*Ixz + Iyz*Ixx)*denom;
 203   k6 = (Ixx*Iyy - Ixy*Ixy)*denom;
 204
 205   mJinv.InitMatrix( k1, k2, k3,
 206                     k2, k4, k5,
 207                     k3, k5, k6 );
 208
 209   Debug(0);
 210
 211   return false;
 212 }
 213
 214 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 215
 216 void FGMassBalance::AddPointMass(Element* el)
 217 {
 218   Element* loc_element = el->FindElement("location");
 219   string pointmass_name = el->GetAttributeValue("name");
 220   if (!loc_element) {
 221     cerr << "Pointmass " << pointmass_name << " has no location." << endl;
 222     exit(-1);
 223   }
 224
 225   double w = el->FindElementValueAsNumberConvertTo("weight", "LBS");
 226   FGColumnVector3 vXYZ = loc_element->FindElementTripletConvertTo("IN");
 227
 228   PointMasses.push_back(new PointMass(w, vXYZ));
 229   PointMasses.back()->bind(PropertyManager, PointMasses.size()-1);
 230 }
 231
 232 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 233
 234 double FGMassBalance::GetTotalPointMassWeight(void)
 235 {
 236   double PM_total_weight = 0.0;
 237
 238   for (unsigned int i=0; i<PointMasses.size(); i++) {
 239     PM_total_weight += PointMasses[i]->Weight;
 240   }
 241   return PM_total_weight;
 242 }
 243
 244 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 245
 246 FGColumnVector3& FGMassBalance::GetPointMassMoment(void)
 247 {
 248   PointMassCG.InitMatrix();
 249
 250   for (unsigned int i=0; i<PointMasses.size(); i++) {
 251     PointMassCG += PointMasses[i]->Weight*PointMasses[i]->Location;
 252   }
 253   return PointMassCG;
 254 }
 255
 256 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 257
 258 FGMatrix33& FGMassBalance::CalculatePMInertias(void)
 259 {
 260   unsigned int size;
 261
 262   size = PointMasses.size();
 263   if (size == 0) return pmJ;
 264
 265   pmJ = FGMatrix33();
 266
 267   for (unsigned int i=0; i<size; i++)
 268     pmJ += GetPointmassInertia( lbtoslug * PointMasses[i]->Weight, PointMasses[i]->Location );
 269
 270   return pmJ;
 271 }
 272
 273 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 274
 275 FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const
 276 {
 277   // Under the assumption that in the structural frame the:
 278   //
 279   // - X-axis is directed afterwards,
 280   // - Y-axis is directed towards the right,
 281   // - Z-axis is directed upwards,
 282   //
 283   // (as documented in http://jsbsim.sourceforge.net/JSBSimCoordinates.pdf)
 284   // we have to subtract first the center of gravity of the plane which
 285   // is also defined in the structural frame:
 286   //
 287   //   FGColumnVector3 cgOff = r - vXYZcg;
 288   //
 289   // Next, we do a change of units:
 290   //
 291   //   cgOff *= inchtoft;
 292   //
 293   // And then a 180 degree rotation is done about the Y axis so that the:
 294   //
 295   // - X-axis is directed forward,
 296   // - Y-axis is directed towards the right,
 297   // - Z-axis is directed downward.
 298   //
 299   // This is needed because the structural and body frames are 180 degrees apart.
 300
 301   return FGColumnVector3(inchtoft*(vXYZcg(1)-r(1)),
 302                          inchtoft*(r(2)-vXYZcg(2)),
 303                          inchtoft*(vXYZcg(3)-r(3)));
 304 }
 305
 306 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 307
 308 void FGMassBalance::bind(void)
 309 {
 310   typedef double (FGMassBalance::*PMF)(int) const;
 311   PropertyManager->Tie("inertia/mass-slugs", this,
 312                        &FGMassBalance::GetMass);
 313   PropertyManager->Tie("inertia/weight-lbs", this,
 314                        &FGMassBalance::GetWeight);
 315   PropertyManager->Tie("inertia/empty-weight-lbs", this,
 316     &FGMassBalance::GetWeight, &FGMassBalance::SetEmptyWeight);
 317   PropertyManager->Tie("inertia/cg-x-in", this,1,
 318                        (PMF)&FGMassBalance::GetXYZcg);
 319   PropertyManager->Tie("inertia/cg-y-in", this,2,
 320                        (PMF)&FGMassBalance::GetXYZcg);
 321   PropertyManager->Tie("inertia/cg-z-in", this,3,
 322                        (PMF)&FGMassBalance::GetXYZcg);
 323 }
 324
 325 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 326 //    The bitmasked value choices are as follows:
 327 //    unset: In this case (the default) JSBSim would only print
 328 //       out the normally expected messages, essentially echoing
 329 //       the config files as they are read. If the environment
 330 //       variable is not set, debug_lvl is set to 1 internally
 331 //    0: This requests JSBSim not to output any messages
 332 //       whatsoever.
 333 //    1: This value explicity requests the normal JSBSim
 334 //       startup messages
 335 //    2: This value asks for a message to be printed out when
 336 //       a class is instantiated
 337 //    4: When this value is set, a message is displayed when a
 338 //       FGModel object executes its Run() method
 339 //    8: When this value is set, various runtime state variables
 340 //       are printed out periodically
 341 //    16: When set various parameters are sanity checked and
 342 //       a message is printed out when they go out of bounds
 343
 344 void FGMassBalance::Debug(int from)
 345 {
 346   if (debug_lvl <= 0) return;
 347
 348   if (debug_lvl & 1) { // Standard console startup message output
 349     if (from == 2) { // Loading
 350       cout << endl << "  Mass and Balance:" << endl;
 351       cout << "    baseIxx: " << baseJ(1,1) << " slug-ft2" << endl;
 352       cout << "    baseIyy: " << baseJ(2,2) << " slug-ft2" << endl;
 353       cout << "    baseIzz: " << baseJ(3,3) << " slug-ft2" << endl;
 354       cout << "    baseIxy: " << baseJ(1,2) << " slug-ft2" << endl;
 355       cout << "    baseIxz: " << baseJ(1,3) << " slug-ft2" << endl;
 356       cout << "    baseIyz: " << baseJ(2,3) << " slug-ft2" << endl;
 357       cout << "    EmptyWeight: " << EmptyWeight << " lbm" << endl;
 358       cout << "    CG (x, y, z): " << vbaseXYZcg << endl;
 359       // ToDo: Need to add point mass outputs here
 360       for (unsigned int i=0; i<PointMasses.size(); i++) {
 361         cout << "    Point Mass Object: " << PointMasses[i]->Weight << " lbs. at "
 362                    << "X, Y, Z (in.): " << PointMasses[i]->Location(eX) << "  "
 363                    << PointMasses[i]->Location(eY) << "  "
 364                    << PointMasses[i]->Location(eZ) << endl;
 365       }
 366     }
 367   }
 368   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 369     if (from == 0) cout << "Instantiated: FGMassBalance" << endl;
 370     if (from == 1) cout << "Destroyed:    FGMassBalance" << endl;
 371   }
 372   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 373   }
 374   if (debug_lvl & 8 ) { // Runtime state variables
 375   }
 376   if (debug_lvl & 16) { // Sanity checking
 377     if (from == 2) {
 378       if (EmptyWeight <= 0.0 || EmptyWeight > 1e9)
 379         cout << "MassBalance::EmptyWeight out of bounds: " << EmptyWeight << endl;
 380       if (Weight <= 0.0 || Weight > 1e9)
 381         cout << "MassBalance::Weight out of bounds: " << Weight << endl;
 382       if (Mass <= 0.0 || Mass > 1e9)
 383         cout << "MassBalance::Mass out of bounds: " << Mass << endl;
 384     }
 385   }
 386   if (debug_lvl & 64) {
 387     if (from == 0) { // Constructor
 388       cout << IdSrc << endl;
 389       cout << IdHdr << endl;
 390     }
 391   }
 392 }
 393 }