From b4bb24f10ec3ad705347a83ba2ea388a8ab34516 Mon Sep 17 00:00:00 2001
From: Bertrand Coconnier <bcoconni@users.sourceforge.net>
Date: Sun, 10 Nov 2019 16:10:47 +0100
Subject: [PATCH] [JSBSim] Fixed the displacements resulting from modifications
 of the mass distribution while the aircraft is sitting on ground (bug GH #230
 reported by Gijs de Rooy).

---
 src/FDM/JSBSim/models/FGMassBalance.cpp | 26 +++++++++++++++----------
 src/FDM/JSBSim/models/FGMassBalance.h   | 19 ++++++++++--------
 2 files changed, 27 insertions(+), 18 deletions(-)

diff --git a/src/FDM/JSBSim/models/FGMassBalance.cpp b/src/FDM/JSBSim/models/FGMassBalance.cpp
index 65c88c0d3..ef58391d9 100644
--- a/src/FDM/JSBSim/models/FGMassBalance.cpp
+++ b/src/FDM/JSBSim/models/FGMassBalance.cpp
@@ -42,6 +42,7 @@ INCLUDES
 
 #include "FGMassBalance.h"
 #include "FGFDMExec.h"
+#include "FGGroundReactions.h"
 #include "input_output/FGXMLElement.h"
 
 using namespace std;
@@ -53,15 +54,16 @@ CLASS IMPLEMENTATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 
-FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex)
+FGMassBalance::FGMassBalance(FGFDMExec* fdmex)
+  : FGModel(fdmex), GroundReactions(nullptr)
 {
   Name = "FGMassBalance";
   Weight = EmptyWeight = Mass = 0.0;
 
-  vbaseXYZcg.InitMatrix(0.0);
-  vXYZcg.InitMatrix(0.0);
-  vLastXYZcg.InitMatrix(0.0);
-  vDeltaXYZcg.InitMatrix(0.0);
+  vbaseXYZcg.InitMatrix();
+  vXYZcg.InitMatrix();
+  vLastXYZcg.InitMatrix();
+  vDeltaXYZcg.InitMatrix();
   baseJ.InitMatrix();
   mJ.InitMatrix();
   mJinv.InitMatrix();
@@ -88,8 +90,9 @@ bool FGMassBalance::InitModel(void)
 {
   if (!FGModel::InitModel()) return false;
 
-  vLastXYZcg.InitMatrix(0.0);
-  vDeltaXYZcg.InitMatrix(0.0);
+  GroundReactions = FDMExec->GetGroundReactions();
+  vLastXYZcg.InitMatrix();
+  vDeltaXYZcg.InitMatrix();
 
   return true;
 }
@@ -206,7 +209,9 @@ bool FGMassBalance::Run(bool Holding)
   vDeltaXYZcgBody = StructuralToBody(vLastXYZcg) - StructuralToBody(vXYZcg);
   vLastXYZcg = vXYZcg;
 
-  if (FDMExec->GetHoldDown())
+  // Compensate displacements of the structural frame when the mass distribution
+  // is modified while the aircraft is in contact with the ground.
+  if (FDMExec->GetHoldDown() || GroundReactions->GetWOW())
     Propagate->NudgeBodyLocation(vDeltaXYZcgBody);
 
 // Calculate new total moments of inertia
@@ -227,7 +232,8 @@ bool FGMassBalance::Run(bool Holding)
   Ixz = -mJ(1,3);
   Iyz = -mJ(2,3);
 
-// Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
+// Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control &
+// Simulation")
 
   k1 = (Iyy*Izz - Iyz*Iyz);
   k2 = (Iyz*Ixz + Ixy*Izz);
@@ -338,7 +344,7 @@ const FGMatrix33& FGMassBalance::CalculatePMInertias(void)
 {
   if (PointMasses.empty()) return pmJ;
 
-  pmJ = FGMatrix33();
+  pmJ.InitMatrix();
 
   for (auto pm: PointMasses) {
     pmJ += GetPointmassInertia( lbtoslug * pm->Weight, pm->Location );
diff --git a/src/FDM/JSBSim/models/FGMassBalance.h b/src/FDM/JSBSim/models/FGMassBalance.h
index b9e19c84d..2a0767071 100644
--- a/src/FDM/JSBSim/models/FGMassBalance.h
+++ b/src/FDM/JSBSim/models/FGMassBalance.h
@@ -48,6 +48,7 @@ FORWARD DECLARATIONSS
 namespace JSBSim {
 
 class FGPropagate;
+class FGGroundReactions;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 CLASS DOCUMENTATION
@@ -67,8 +68,8 @@ CLASS DOCUMENTATION
     sign of the inertia cross products are not modified by JSBSim so in most
     cases, negative values should be provided for <ixy>, <ixz> and <iyz>.
 
-    <h3>Configuration File Format:</h3>
-@code
+    <h3>Configuration File Format for \<mass_balance> Section:</h3>
+@code{.xml}
     <mass_balance>
         <ixx unit="{SLUG*FT2 | KG*M2}"> {number} </ixx>
         <iyy unit="{SLUG*FT2 | KG*M2}"> {number} </iyy>
@@ -113,12 +114,13 @@ public:
   bool Load(Element* el);
   bool InitModel(void) override;
   /** Runs the Mass Balance model; called by the Executive
-      Can pass in a value indicating if the executive is directing the simulation to Hold.
-      @param Holding if true, the executive has been directed to hold the sim from 
-                     advancing time. Some models may ignore this flag, such as the Input
-                     model, which may need to be active to listen on a socket for the
-                     "Resume" command to be given.
-      @return false if no error */
+      Can pass in a value indicating if the executive is directing the
+      simulation to Hold.
+      @param Holding if true, the executive has been directed to hold the sim
+                     from advancing time. Some models may ignore this flag, such
+                     as the Input model, which may need to be active to listen
+                     on a socket for the "Resume" command to be given.  @return
+                     false if no error */
   bool Run(bool Holding) override;
 
   double GetMass(void) const {return Mass;}
@@ -186,6 +188,7 @@ public:
 
 private:
   FGPropagate* Propagate;
+  FGGroundReactions* GroundReactions;
   double Weight;
   double EmptyWeight;
   double Mass;