src/FDM/JSBSim/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 "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   Debug(1);
  77 }
  78
  79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  80
  81 bool FGMassBalance::Run(void)
  82 {
  83   double denom, k1, k2, k3, k4, k5, k6;
  84   double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
  85
  86   if (!FGModel::Run()) {
  87
  88     Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetPointMassWeight();
  89
  90     Mass = lbtoslug*Weight;
  91
  92 // Calculate new CG
  93
  94     vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg
  95                                        + GetPointMassMoment() ) / Weight;
  96
  97 // Calculate new total moments of inertia
  98
  99     // At first it is the base configuration inertia matrix ...
 100     mJ = baseJ;
 101     // ... with the additional term originating from the parallel axis theorem.
 102     mJ += GetPointmassInertia( lbtoslug * EmptyWeight, vbaseXYZcg );
 103     // Then add the contributions from the additional pointmasses.
 104     mJ += CalculatePMInertias();
 105     mJ += Propulsion->CalculateTankInertias();
 106
 107     Ixx = mJ(1,1);
 108     Iyy = mJ(2,2);
 109     Izz = mJ(3,3);
 110     Ixy = -mJ(1,2);
 111     Ixz = -mJ(1,3);
 112     Iyz = -mJ(2,3);
 113
 114 // Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
 115
 116     k1 = (Iyy*Izz - Iyz*Iyz);
 117     k2 = (Iyz*Ixz + Ixy*Izz);
 118     k3 = (Ixy*Iyz + Iyy*Ixz);
 119
 120     denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 );
 121     k1 = k1*denom;
 122     k2 = k2*denom;
 123     k3 = k3*denom;
 124     k4 = (Izz*Ixx - Ixz*Ixz)*denom;
 125     k5 = (Ixy*Ixz + Iyz*Ixx)*denom;
 126     k6 = (Ixx*Iyy - Ixy*Ixy)*denom;
 127
 128     mJinv.InitMatrix( k1, k2, k3,
 129                       k2, k4, k5,
 130                       k3, k5, k6 );
 131
 132     Debug(2);
 133
 134     return false;
 135   } else {
 136     return true;
 137   }
 138 }
 139
 140 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 141
 142 void FGMassBalance::AddPointMass(double weight, double X, double Y, double Z)
 143 {
 144   PointMassLoc.push_back(FGColumnVector3(X, Y, Z));
 145   PointMassWeight.push_back(weight);
 146 }
 147
 148 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 149
 150 double FGMassBalance::GetPointMassWeight(void)
 151 {
 152   double PM_total_weight = 0.0;
 153
 154   for (unsigned int i=0; i<PointMassWeight.size(); i++) {
 155     PM_total_weight += PointMassWeight[i];
 156   }
 157   return PM_total_weight;
 158 }
 159
 160 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 161
 162 FGColumnVector3& FGMassBalance::GetPointMassMoment(void)
 163 {
 164   PointMassCG.InitMatrix();
 165
 166   for (unsigned int i=0; i<PointMassLoc.size(); i++) {
 167     PointMassCG += PointMassWeight[i]*PointMassLoc[i];
 168   }
 169   return PointMassCG;
 170 }
 171
 172 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 173
 174 FGMatrix33& FGMassBalance::CalculatePMInertias(void)
 175 {
 176   unsigned int size;
 177
 178   size = PointMassLoc.size();
 179   if (size == 0) return pmJ;
 180
 181   pmJ = FGMatrix33();
 182
 183   for (unsigned int i=0; i<size; i++)
 184     pmJ += GetPointmassInertia( lbtoslug * PointMassWeight[i], PointMassLoc[i] );
 185
 186   return pmJ;
 187 }
 188
 189 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 190
 191 FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const
 192 {
 193   // Under the assumption that in the structural frame the:
 194   //
 195   // - X-axis is directed afterwards,
 196   // - Y-axis is directed towards the right,
 197   // - Z-axis is directed upwards,
 198   //
 199   // (as documented in http://jsbsim.sourceforge.net/JSBSimCoordinates.pdf)
 200   // we have to subtract first the center of gravity of the plane which
 201   // is also defined in the structural frame:
 202   //
 203   //   FGColumnVector3 cgOff = r - vXYZcg;
 204   //
 205   // Next, we do a change of units:
 206   //
 207   //   cgOff *= inchtoft;
 208   //
 209   // And then a 180 degree rotation is done about the Y axis so that the:
 210   //
 211   // - X-axis is directed forward,
 212   // - Y-axis is directed towards the right,
 213   // - Z-axis is directed downward.
 214   //
 215   // This is needed because the structural and body frames are 180 degrees apart.
 216
 217   return FGColumnVector3(inchtoft*(vXYZcg(1)-r(1)),
 218                          inchtoft*(r(2)-vXYZcg(2)),
 219                          inchtoft*(vXYZcg(3)-r(3)));
 220 }
 221
 222 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 223
 224 void FGMassBalance::bind(void)
 225 {
 226   typedef double (FGMassBalance::*PMF)(int) const;
 227   PropertyManager->Tie("inertia/mass-slugs", this,
 228                        &FGMassBalance::GetMass);
 229   PropertyManager->Tie("inertia/weight-lbs", this,
 230                        &FGMassBalance::GetWeight);
 231   PropertyManager->Tie("inertia/cg-x-ft", this,1,
 232                        (PMF)&FGMassBalance::GetXYZcg);
 233   PropertyManager->Tie("inertia/cg-y-ft", this,2,
 234                        (PMF)&FGMassBalance::GetXYZcg);
 235   PropertyManager->Tie("inertia/cg-z-ft", this,3,
 236                        (PMF)&FGMassBalance::GetXYZcg);
 237 }
 238
 239 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 240
 241 void FGMassBalance::unbind(void)
 242 {
 243   PropertyManager->Untie("inertia/mass-slugs");
 244   PropertyManager->Untie("inertia/weight-lbs");
 245   PropertyManager->Untie("inertia/cg-x-ft");
 246   PropertyManager->Untie("inertia/cg-y-ft");
 247   PropertyManager->Untie("inertia/cg-z-ft");
 248 }
 249
 250 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 251 //    The bitmasked value choices are as follows:
 252 //    unset: In this case (the default) JSBSim would only print
 253 //       out the normally expected messages, essentially echoing
 254 //       the config files as they are read. If the environment
 255 //       variable is not set, debug_lvl is set to 1 internally
 256 //    0: This requests JSBSim not to output any messages
 257 //       whatsoever.
 258 //    1: This value explicity requests the normal JSBSim
 259 //       startup messages
 260 //    2: This value asks for a message to be printed out when
 261 //       a class is instantiated
 262 //    4: When this value is set, a message is displayed when a
 263 //       FGModel object executes its Run() method
 264 //    8: When this value is set, various runtime state variables
 265 //       are printed out periodically
 266 //    16: When set various parameters are sanity checked and
 267 //       a message is printed out when they go out of bounds
 268
 269 void FGMassBalance::Debug(int from)
 270 {
 271   if (debug_lvl <= 0) return;
 272
 273   if (debug_lvl & 1) { // Standard console startup message output
 274     if (from == 0) { // Constructor
 275
 276     }
 277   }
 278   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 279     if (from == 0) cout << "Instantiated: FGMassBalance" << endl;
 280     if (from == 1) cout << "Destroyed:    FGMassBalance" << endl;
 281   }
 282   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 283   }
 284   if (debug_lvl & 8 ) { // Runtime state variables
 285   }
 286   if (debug_lvl & 16) { // Sanity checking
 287     if (from == 2) {
 288       if (EmptyWeight <= 0.0 || EmptyWeight > 1e9)
 289         cout << "MassBalance::EmptyWeight out of bounds: " << EmptyWeight << endl;
 290       if (Weight <= 0.0 || Weight > 1e9)
 291         cout << "MassBalance::Weight out of bounds: " << Weight << endl;
 292       if (Mass <= 0.0 || Mass > 1e9)
 293         cout << "MassBalance::Mass out of bounds: " << Mass << endl;
 294     }
 295   }
 296   if (debug_lvl & 64) {
 297     if (from == 0) { // Constructor
 298       cout << IdSrc << endl;
 299       cout << IdHdr << endl;
 300     }
 301   }
 302 }
 303 }