X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGMassBalance.cpp;h=f925c8fccd0fd66500bb877c344197c536373f06;hb=fbee3d10f0aafd4178fc1313edb8593c156b2874;hp=3004964540142cbdcb9e9d973e807bf87ac035bc;hpb=407dcaff6236e7217eff16ac253218d3e51bce6d;p=flightgear.git

diff --git a/src/FDM/JSBSim/FGMassBalance.cpp b/src/FDM/JSBSim/FGMassBalance.cpp
index 300496454..f925c8fcc 100644
--- a/src/FDM/JSBSim/FGMassBalance.cpp
+++ b/src/FDM/JSBSim/FGMassBalance.cpp
@@ -39,8 +39,12 @@ INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGMassBalance.h"
+#include "FGPropulsion.h"
+#include "FGPropertyManager.h"
 
-static const char *IdSrc = "$Header$";
+namespace JSBSim {
+
+static const char *IdSrc = "$Id$";
 static const char *IdHdr = ID_MASSBALANCE;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -50,17 +54,250 @@ CLASS IMPLEMENTATION
 
 FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex)
 {
-  //
+  Name = "FGMassBalance";
+  Weight = EmptyWeight = Mass = 0.0;
+
+  vbaseXYZcg.InitMatrix(0.0);
+  baseJ.InitMatrix();
+  mJ.InitMatrix();
+  mJinv.InitMatrix();
+  pmJ.InitMatrix();
+
+  bind();
+
+  Debug(0);
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-bool FGMassBalance:: Run(void) {
+FGMassBalance::~FGMassBalance()
+{
+  unbind();
+  Debug(1);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+bool FGMassBalance::Run(void)
+{
+  double denom, k1, k2, k3, k4, k5, k6;
+  double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
 
   if (!FGModel::Run()) {
 
+    Weight = EmptyWeight + Propulsion->GetTanksWeight() + GetPointMassWeight();
+
+    Mass = lbtoslug*Weight;
+
+// Calculate new CG
+
+    vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg
+                                       + GetPointMassMoment() ) / Weight;
+
+// Calculate new total moments of inertia
+
+    // At first it is the base configuration inertia matrix ...
+    mJ = baseJ;
+    // ... with the additional term originating from the parallel axis theorem.
+    mJ += GetPointmassInertia( lbtoslug * EmptyWeight, vbaseXYZcg );
+    // Then add the contributions from the additional pointmasses.
+    mJ += CalculatePMInertias();
+    mJ += Propulsion->CalculateTankInertias();
+
+    Ixx = mJ(1,1);
+    Iyy = mJ(2,2);
+    Izz = mJ(3,3);
+    Ixy = -mJ(1,2);
+    Ixz = -mJ(1,3);
+    Iyz = -mJ(2,3);
+
+// Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
+
+    k1 = (Iyy*Izz - Iyz*Iyz);
+    k2 = (Iyz*Ixz + Ixy*Izz);
+    k3 = (Ixy*Iyz + Iyy*Ixz);
+
+    denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 );
+    k1 = k1*denom;
+    k2 = k2*denom;
+    k3 = k3*denom;
+    k4 = (Izz*Ixx - Ixz*Ixz)*denom;
+    k5 = (Ixy*Ixz + Iyz*Ixx)*denom;
+    k6 = (Ixx*Iyy - Ixy*Ixy)*denom;
+
+    mJinv.InitMatrix( k1, k2, k3,
+                      k2, k4, k5,
+                      k3, k5, k6 );
+
+    Debug(2);
+
     return false;
   } else {
     return true;
   }
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMassBalance::AddPointMass(double weight, double X, double Y, double Z)
+{
+  PointMassLoc.push_back(FGColumnVector3(X, Y, Z));
+  PointMassWeight.push_back(weight);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+double FGMassBalance::GetPointMassWeight(void)
+{
+  double PM_total_weight = 0.0;
+
+  for (unsigned int i=0; i<PointMassWeight.size(); i++) {
+    PM_total_weight += PointMassWeight[i];
+  }
+  return PM_total_weight;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3& FGMassBalance::GetPointMassMoment(void)
+{
+  PointMassCG.InitMatrix();
+
+  for (unsigned int i=0; i<PointMassLoc.size(); i++) {
+    PointMassCG += PointMassWeight[i]*PointMassLoc[i];
+  }
+  return PointMassCG;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33& FGMassBalance::CalculatePMInertias(void)
+{
+  unsigned int size;
+
+  size = PointMassLoc.size();
+  if (size == 0) return pmJ;
+
+  pmJ = FGMatrix33();
+
+  for (unsigned int i=0; i<size; i++)
+    pmJ += GetPointmassInertia( lbtoslug * PointMassWeight[i], PointMassLoc[i] );
+
+  return pmJ;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const
+{
+  // Under the assumption that in the structural frame the:
+  //
+  // - X-axis is directed afterwards,
+  // - Y-axis is directed towards the right,
+  // - Z-axis is directed upwards,
+  //
+  // (as documented in http://jsbsim.sourceforge.net/JSBSimCoordinates.pdf)
+  // we have to subtract first the center of gravity of the plane which
+  // is also defined in the structural frame:
+  //
+  //   FGColumnVector3 cgOff = r - vXYZcg;
+  //
+  // Next, we do a change of units:
+  //
+  //   cgOff *= inchtoft;
+  //
+  // And then a 180 degree rotation is done about the Y axis so that the:
+  //
+  // - X-axis is directed forward,
+  // - Y-axis is directed towards the right,
+  // - Z-axis is directed downward.
+  //
+  // This is needed because the structural and body frames are 180 degrees apart.
+
+  return FGColumnVector3(inchtoft*(vXYZcg(1)-r(1)),
+                         inchtoft*(r(2)-vXYZcg(2)),
+                         inchtoft*(vXYZcg(3)-r(3)));
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMassBalance::bind(void)
+{
+  typedef double (FGMassBalance::*PMF)(int) const;
+  PropertyManager->Tie("inertia/mass-slugs", this,
+                       &FGMassBalance::GetMass);
+  PropertyManager->Tie("inertia/weight-lbs", this,
+                       &FGMassBalance::GetWeight);
+  PropertyManager->Tie("inertia/cg-x-ft", this,1,
+                       (PMF)&FGMassBalance::GetXYZcg);
+  PropertyManager->Tie("inertia/cg-y-ft", this,2,
+                       (PMF)&FGMassBalance::GetXYZcg);
+  PropertyManager->Tie("inertia/cg-z-ft", this,3,
+                       (PMF)&FGMassBalance::GetXYZcg);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMassBalance::unbind(void)
+{
+  PropertyManager->Untie("inertia/mass-slugs");
+  PropertyManager->Untie("inertia/weight-lbs");
+  PropertyManager->Untie("inertia/cg-x-ft");
+  PropertyManager->Untie("inertia/cg-y-ft");
+  PropertyManager->Untie("inertia/cg-z-ft");
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+//    The bitmasked value choices are as follows:
+//    unset: In this case (the default) JSBSim would only print
+//       out the normally expected messages, essentially echoing
+//       the config files as they are read. If the environment
+//       variable is not set, debug_lvl is set to 1 internally
+//    0: This requests JSBSim not to output any messages
+//       whatsoever.
+//    1: This value explicity requests the normal JSBSim
+//       startup messages
+//    2: This value asks for a message to be printed out when
+//       a class is instantiated
+//    4: When this value is set, a message is displayed when a
+//       FGModel object executes its Run() method
+//    8: When this value is set, various runtime state variables
+//       are printed out periodically
+//    16: When set various parameters are sanity checked and
+//       a message is printed out when they go out of bounds
+
+void FGMassBalance::Debug(int from)
+{
+  if (debug_lvl <= 0) return;
+
+  if (debug_lvl & 1) { // Standard console startup message output
+    if (from == 0) { // Constructor
+
+    }
+  }
+  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
+    if (from == 0) cout << "Instantiated: FGMassBalance" << endl;
+    if (from == 1) cout << "Destroyed:    FGMassBalance" << endl;
+  }
+  if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
+  }
+  if (debug_lvl & 8 ) { // Runtime state variables
+  }
+  if (debug_lvl & 16) { // Sanity checking
+    if (from == 2) {
+      if (EmptyWeight <= 0.0 || EmptyWeight > 1e9)
+        cout << "MassBalance::EmptyWeight out of bounds: " << EmptyWeight << endl;
+      if (Weight <= 0.0 || Weight > 1e9)
+        cout << "MassBalance::Weight out of bounds: " << Weight << endl;
+      if (Mass <= 0.0 || Mass > 1e9)
+        cout << "MassBalance::Mass out of bounds: " << Mass << endl;
+    }
+  }
+  if (debug_lvl & 64) {
+    if (from == 0) { // Constructor
+      cout << IdSrc << endl;
+      cout << IdHdr << endl;
+    }
+  }
+}
+}