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 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 General Public License for more
  18  details.
  19
  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.
  23
  24  Further information about the GNU 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 <input_output/FGPropertyManager.h>
  44
  45 namespace JSBSim {
  46
  47 static const char *IdSrc = "$Id$";
  48 static const char *IdHdr = ID_MASSBALANCE;
  49
  50 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  51 CLASS IMPLEMENTATION
  52 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  53
  54
  55 FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex)
  56 {
  57   Name = "FGMassBalance";
  58   Weight = EmptyWeight = Mass = 0.0;
  59
  60   vbaseXYZcg.InitMatrix(0.0);
  61   baseJ.InitMatrix();
  62   mJ.InitMatrix();
  63   mJinv.InitMatrix();
  64   pmJ.InitMatrix();
  65
  66   bind();
  67
  68   Debug(0);
  69 }
  70
  71 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  72
  73 FGMassBalance::~FGMassBalance()
  74 {
  75   unbind();
  76   PointMasses.clear();
  77   Debug(1);
  78 }
  79
  80 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  81
  82 bool FGMassBalance::Load(Element* el)
  83 {
  84   Element *element;
  85   string element_name = "";
  86   double bixx, biyy, bizz, bixy, bixz, biyz;
  87
  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,
 102                                       -bixy,  biyy,  -biyz,
 103                                       -bixz,  -biyz,  bizz ));
 104   EmptyWeight = el->FindElementValueAsNumberConvertTo("emptywt", "LBS");
 105
 106   element = el->FindElement("location");
 107    while (element) {
 108      element_name = element->GetAttributeValue("name");
 109      if (element_name == "CG") vbaseXYZcg = element->FindElementTripletConvertTo("IN");
 110      element = el->FindNextElement("location");
 111    }
 112
 113 // Find all POINTMASS elements that descend from this METRICS branch of the
 114 // config file.
 115
 116   element = el->FindElement("pointmass");
 117   while (element) {
 118     AddPointMass(element);
 119     element = el->FindNextElement("pointmass");
 120   }
 121
 122   Debug(2);
 123   return true;
 124 }
 125
 126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 127
 128 bool FGMassBalance::Run(void)
 129 {
 130   double denom, k1, k2, k3, k4, k5, k6;
 131   double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
 132
 133   if (FGModel::Run()) return true;
 134   if (FDMExec->Holding()) return false;
 135
 136   Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetPointMassWeight();
 137
 138   Mass = lbtoslug*Weight;
 139
 140 // Calculate new CG
 141
 142 //  printf("%s:%i\n", __FILE__, __LINE__);
 143   vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg
 144                                      + GetPointMassMoment() ) / Weight;
 145
 146 // Calculate new total moments of inertia
 147
 148   // At first it is the base configuration inertia matrix ...
 149   mJ = baseJ;
 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();
 155
 156   Ixx = mJ(1,1);
 157   Iyy = mJ(2,2);
 158   Izz = mJ(3,3);
 159   Ixy = -mJ(1,2);
 160   Ixz = -mJ(1,3);
 161   Iyz = -mJ(2,3);
 162
 163 // Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
 164
 165   k1 = (Iyy*Izz - Iyz*Iyz);
 166   k2 = (Iyz*Ixz + Ixy*Izz);
 167   k3 = (Ixy*Iyz + Iyy*Ixz);
 168
 169   denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 );
 170   k1 = k1*denom;
 171   k2 = k2*denom;
 172   k3 = k3*denom;
 173   k4 = (Izz*Ixx - Ixz*Ixz)*denom;
 174   k5 = (Ixy*Ixz + Iyz*Ixx)*denom;
 175   k6 = (Ixx*Iyy - Ixy*Ixy)*denom;
 176
 177   mJinv.InitMatrix( k1, k2, k3,
 178                     k2, k4, k5,
 179                     k3, k5, k6 );
 180
 181   Debug(0);
 182
 183   return false;
 184 }
 185
 186 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 187
 188 void FGMassBalance::AddPointMass(Element* el)
 189 {
 190   Element* loc_element = el->FindElement("location");
 191   string pointmass_name = el->GetAttributeValue("name");
 192   if (!el) {
 193     cerr << "Pointmass " << pointmass_name << "has no location." << endl;
 194     exit(-1);
 195   }
 196   string loc_unit = loc_element->GetAttributeValue("unit");
 197   double w, x, y, z;
 198
 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);
 203
 204   PointMasses.push_back(PointMass(w, x, y, z));
 205 }
 206
 207 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 208
 209 double FGMassBalance::GetPointMassWeight(void)
 210 {
 211   double PM_total_weight = 0.0;
 212
 213   for (unsigned int i=0; i<PointMasses.size(); i++) {
 214     PM_total_weight += PointMasses[i].Weight;
 215   }
 216   return PM_total_weight;
 217 }
 218
 219 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 220
 221 FGColumnVector3& FGMassBalance::GetPointMassMoment(void)
 222 {
 223   PointMassCG.InitMatrix();
 224
 225   for (unsigned int i=0; i<PointMasses.size(); i++) {
 226     PointMassCG += PointMasses[i].Weight*PointMasses[i].Location;
 227   }
 228   return PointMassCG;
 229 }
 230
 231 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 232
 233 FGMatrix33& FGMassBalance::CalculatePMInertias(void)
 234 {
 235   unsigned int size;
 236
 237   size = PointMasses.size();
 238   if (size == 0) return pmJ;
 239
 240   pmJ = FGMatrix33();
 241
 242   for (unsigned int i=0; i<size; i++)
 243     pmJ += GetPointmassInertia( lbtoslug * PointMasses[i].Weight, PointMasses[i].Location );
 244
 245   return pmJ;
 246 }
 247
 248 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 249
 250 FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const
 251 {
 252   // Under the assumption that in the structural frame the:
 253   //
 254   // - X-axis is directed afterwards,
 255   // - Y-axis is directed towards the right,
 256   // - Z-axis is directed upwards,
 257   //
 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:
 261   //
 262   //   FGColumnVector3 cgOff = r - vXYZcg;
 263   //
 264   // Next, we do a change of units:
 265   //
 266   //   cgOff *= inchtoft;
 267   //
 268   // And then a 180 degree rotation is done about the Y axis so that the:
 269   //
 270   // - X-axis is directed forward,
 271   // - Y-axis is directed towards the right,
 272   // - Z-axis is directed downward.
 273   //
 274   // This is needed because the structural and body frames are 180 degrees apart.
 275
 276   return FGColumnVector3(inchtoft*(vXYZcg(1)-r(1)),
 277                          inchtoft*(r(2)-vXYZcg(2)),
 278                          inchtoft*(vXYZcg(3)-r(3)));
 279 }
 280
 281 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 282
 283 void FGMassBalance::bind(void)
 284 {
 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);
 296 }
 297
 298 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 299
 300 void FGMassBalance::unbind(void)
 301 {
 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");
 307 }
 308
 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
 316 //       whatsoever.
 317 //    1: This value explicity requests the normal JSBSim
 318 //       startup messages
 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
 327
 328 void FGMassBalance::Debug(int from)
 329 {
 330   if (debug_lvl <= 0) return;
 331
 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;
 349       }
 350     }
 351   }
 352   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 353     if (from == 0) cout << "Instantiated: FGMassBalance" << endl;
 354     if (from == 1) cout << "Destroyed:    FGMassBalance" << endl;
 355   }
 356   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 357   }
 358   if (debug_lvl & 8 ) { // Runtime state variables
 359   }
 360   if (debug_lvl & 16) { // Sanity checking
 361     if (from == 2) {
 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;
 368     }
 369   }
 370   if (debug_lvl & 64) {
 371     if (from == 0) { // Constructor
 372       cout << IdSrc << endl;
 373       cout << IdHdr << endl;
 374     }
 375   }
 376 }
 377 }