Dynamic calculation of CG and inertias.

diff --git a/src/FDM/JSBSim/FGAircraft.cpp b/src/FDM/JSBSim/FGAircraft.cpp
index 4fcfc64b432a12d8aa4d77094ead3deb364fef7f..e165cbe0e000b48faec16dcf708a830d8a939038 100644 (file)
--- a/src/FDM/JSBSim/FGAircraft.cpp
+++ b/src/FDM/JSBSim/FGAircraft.cpp
@@ -190,24 +190,24 @@ bool FGAircraft::LoadAircraft(string aircraft_path, string engine_path, string f
                                aircraftfile >> cbar;
                                cout << "Aircraft Chord: " << cbar << endl;
                        } else if (holding_string == "AC_IXX") {
-                               aircraftfile >> Ixx;
-                               cout << "Aircraft Ixx: " << Ixx << endl;
+                               aircraftfile >> baseIxx;
+                               cout << "Aircraft Base Ixx: " << baseIxx << endl;
                        } else if (holding_string == "AC_IYY") {
-                               aircraftfile >> Iyy;
-                               cout << "Aircraft Iyy: " << Iyy << endl;
+                               aircraftfile >> baseIyy;
+                               cout << "Aircraft Base Iyy: " << baseIyy << endl;
                        } else if (holding_string == "AC_IZZ") {
-                               aircraftfile >> Izz;
-                               cout << "Aircraft Izz: " << Izz << endl;
+                               aircraftfile >> baseIzz;
+                               cout << "Aircraft Base Izz: " << baseIzz << endl;
                        } else if (holding_string == "AC_IXZ") {
-                               aircraftfile >> Ixz;
-                               cout << "Aircraft Ixz: " << Ixz << endl;
+                               aircraftfile >> baseIxz;
+                               cout << "Aircraft Base Ixz: " << baseIxz << endl;
                        } else if (holding_string == "AC_EMPTYWT") {
                                aircraftfile >> EmptyWeight;
                                EmptyMass = EmptyWeight / GRAVITY;
                                cout << "Aircraft Empty Weight: " << EmptyWeight << endl;
                        } else if (holding_string == "AC_CGLOC") {
-                               aircraftfile >> Xcg >> Ycg >> Zcg;
-                               cout << "Aircraft C.G.: " << Xcg << " " << Ycg << " " << Zcg << endl;
+                               aircraftfile >> baseXcg >> baseYcg >> baseZcg;
+                               cout << "Aircraft Base C.G.: " << baseXcg << " " << baseYcg << " " << baseZcg << endl;
                        } else if (holding_string == "AC_EYEPTLOC") {
                                aircraftfile >> Xep >> Yep >> Zep;
                                cout << "Pilot Eyepoint: " << Xep << " " << Yep << " " << Zep << endl;
@@ -319,6 +319,9 @@ bool FGAircraft::Run(void)
 
 void FGAircraft::MassChange()
 {
+  float Xt, Xw, Yt, Yw, Zt, Zw, Tw;
+  float IXXt, IYYt, IZZt, IXZt;
+
   // UPDATE TANK CONTENTS
   //
   // For each engine, cycle through the tanks and draw an equal amount of
@@ -372,6 +375,38 @@ void FGAircraft::MassChange()
     Weight += Tank[t]->GetContents();
 
   Mass = Weight / GRAVITY;
+
+  // Calculate new CG here.
+
+  Xt = Yt = Zt = 0;
+  Xw = Yw = Zw = 0;
+  for (int t=0; t<numTanks; t++) {
+    Xt += Tank[t]->GetX()*Tank[t]->GetContents();
+    Yt += Tank[t]->GetY()*Tank[t]->GetContents();
+    Zt += Tank[t]->GetZ()*Tank[t]->GetContents();
+
+    Tw += Tank[t]->GetContents();
+  }
+
+  Xcg = (Xt + EmptyWeight*baseXcg) / (Tw + EmptyWeight);
+  Ycg = (Yt + EmptyWeight*baseYcg) / (Tw + EmptyWeight);
+  Zcg = (Zt + EmptyWeight*baseZcg) / (Tw + EmptyWeight);
+
+  // Calculate new moments of inertia here
+
+  IXXt = IYYt = IZZt = IXZt = 0.0;
+  for (int t=0; t<numTanks; t++) {
+    IXXt += ((Tank[t]->GetX()-Xcg)/12.0)*((Tank[t]->GetX() - Xcg)/12.0)*Tank[t]->GetContents()/GRAVITY;
+    IYYt += ((Tank[t]->GetY()-Ycg)/12.0)*((Tank[t]->GetY() - Ycg)/12.0)*Tank[t]->GetContents()/GRAVITY;
+    IZZt += ((Tank[t]->GetZ()-Zcg)/12.0)*((Tank[t]->GetZ() - Zcg)/12.0)*Tank[t]->GetContents()/GRAVITY;
+    IXZt += ((Tank[t]->GetX()-Xcg)/12.0)*((Tank[t]->GetZ() - Zcg)/12.0)*Tank[t]->GetContents()/GRAVITY;
+  }
+
+  Ixx = baseIxx + IXXt;
+  Iyy = baseIyy + IYYt;
+  Izz = baseIzz + IZZt;
+  Ixz = baseIxz + IXZt;
+
 }
 
 

diff --git a/src/FDM/JSBSim/FGAircraft.h b/src/FDM/JSBSim/FGAircraft.h
index aa7229079ed27c1d386829dc84f555793cd61ce6..6e1b3df2ab261dc70a9305b886f748b2b3d59dbe 100644 (file)
--- a/src/FDM/JSBSim/FGAircraft.h
+++ b/src/FDM/JSBSim/FGAircraft.h
@@ -167,7 +167,9 @@ private:
   float Moments[3];
   float Forces[3];
   string AircraftName;
-  float Ixx, Iyy, Izz, Ixz, EmptyMass, Mass;
+  float baseIxx, baseIyy, baseIzz, baseIxz, EmptyMass, Mass;
+  float Ixx, Iyy, Izz, Ixz;
+  float baseXcg, baseYcg, baseZcg;
   float Xcg, Ycg, Zcg;
   float Xep, Yep, Zep;
   float rho, qbar, Vt;

diff --git a/src/FDM/JSBSim/FGOutput.cpp b/src/FDM/JSBSim/FGOutput.cpp
index cf626ac76cf0f3d5c862b02a582bc2f45648524e..3d501834a09e19bbbce3b5020b0c1cd9f00cb397 100644 (file)
--- a/src/FDM/JSBSim/FGOutput.cpp
+++ b/src/FDM/JSBSim/FGOutput.cpp
@@ -95,6 +95,12 @@ void FGOutput::DelimitedOutput(void)
     cout << "Udot,";
     cout << "Vdot,";
     cout << "Wdot,";
+    cout << "P,";
+    cout << "Q,";
+    cout << "R,";
+    cout << "PDot,";
+    cout << "QDot,";
+    cout << "RDot,";
     cout << "Fx,";
     cout << "Fy,";
     cout << "Fz,";
@@ -114,7 +120,7 @@ void FGOutput::DelimitedOutput(void)
   cout << Rotation->Getphi() << ",";
   cout << Rotation->Gettht() << ",";
   cout << Rotation->Getpsi() << ",";
-  cout << Atmosphere->Getrho() << ",";
+  cout << Atmosphere->GetDensity() << ",";
   cout << State->GetVt() << ",";
   cout << Translation->GetU() << ",";
   cout << Translation->GetV() << ",";
@@ -125,6 +131,12 @@ void FGOutput::DelimitedOutput(void)
   cout << Translation->GetUdot() << ",";
   cout << Translation->GetVdot() << ",";
   cout << Translation->GetWdot() << ",";
+  cout << Rotation->GetP() << ",";
+  cout << Rotation->GetQ() << ",";
+  cout << Rotation->GetR() << ",";
+  cout << Rotation->GetPdot() << ",";
+  cout << Rotation->GetQdot() << ",";
+  cout << Rotation->GetRdot() << ",";
   cout << Aircraft->GetFx() << ",";
   cout << Aircraft->GetFy() << ",";
   cout << Aircraft->GetFz() << ",";
@@ -160,6 +172,12 @@ void FGOutput::DelimitedOutput(string fname)
     datafile << "Udot,";
     datafile << "Vdot,";
     datafile << "Wdot,";
+    datafile << "P,";
+    datafile << "Q,";
+    datafile << "R,";
+    datafile << "PDot,";
+    datafile << "QDot,";
+    datafile << "RDot,";
     datafile << "Fx,";
     datafile << "Fy,";
     datafile << "Fz,";
@@ -179,7 +197,7 @@ void FGOutput::DelimitedOutput(string fname)
   datafile << Rotation->Getphi() << ",";
   datafile << Rotation->Gettht() << ",";
   datafile << Rotation->Getpsi() << ",";
-  datafile << Atmosphere->Getrho() << ",";
+  datafile << Atmosphere->GetDensity() << ",";
   datafile << State->GetVt() << ",";
   datafile << Translation->GetU() << ",";
   datafile << Translation->GetV() << ",";
@@ -190,6 +208,12 @@ void FGOutput::DelimitedOutput(string fname)
   datafile << Translation->GetUdot() << ",";
   datafile << Translation->GetVdot() << ",";
   datafile << Translation->GetWdot() << ",";
+  datafile << Rotation->GetP() << ",";
+  datafile << Rotation->GetQ() << ",";
+  datafile << Rotation->GetR() << ",";
+  datafile << Rotation->GetPdot() << ",";
+  datafile << Rotation->GetQdot() << ",";
+  datafile << Rotation->GetRdot() << ",";
   datafile << Aircraft->GetFx() << ",";
   datafile << Aircraft->GetFy() << ",";
   datafile << Aircraft->GetFz() << ",";

diff --git a/src/FDM/JSBSim/FGTank.h b/src/FDM/JSBSim/FGTank.h
index 54baddacb6e379e5559fa736f5ad2ee97e381d78..a329ffb50f62e8e14b37750c782c8cab1f98842a 100644 (file)
--- a/src/FDM/JSBSim/FGTank.h
+++ b/src/FDM/JSBSim/FGTank.h
@@ -78,6 +78,9 @@ public:
   bool GetSelected(void) {return Selected;}
   float GetPctFull(void) {return PctFull;}
   float GetContents(void) {return Contents;}
+  float inline GetX(void) {return X;}
+  float inline GetY(void) {return Y;}
+  float inline GetZ(void) {return Z;}
 
   enum TankType {ttUNKNOWN, ttFUEL, ttOXIDIZER};