X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGMassBalance.cpp;h=f925c8fccd0fd66500bb877c344197c536373f06;hb=fbee3d10f0aafd4178fc1313edb8593c156b2874;hp=2770191ab82833368ac50f7343684b5218c84d55;hpb=5c3f4e999db5cc882f6f47630084ff00b53a4516;p=flightgear.git diff --git a/src/FDM/JSBSim/FGMassBalance.cpp b/src/FDM/JSBSim/FGMassBalance.cpp index 2770191ab..f925c8fcc 100644 --- a/src/FDM/JSBSim/FGMassBalance.cpp +++ b/src/FDM/JSBSim/FGMassBalance.cpp @@ -39,6 +39,10 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGMassBalance.h" +#include "FGPropulsion.h" +#include "FGPropertyManager.h" + +namespace JSBSim { static const char *IdSrc = "$Id$"; static const char *IdHdr = ID_MASSBALANCE; @@ -51,6 +55,15 @@ 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); } @@ -59,6 +72,7 @@ FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex) FGMassBalance::~FGMassBalance() { + unbind(); Debug(1); } @@ -66,24 +80,54 @@ FGMassBalance::~FGMassBalance() 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 = Weight / Inertial->gravity(); + 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(); -// Calculate new CG here. + Ixx = mJ(1,1); + Iyy = mJ(2,2); + Izz = mJ(3,3); + Ixy = -mJ(1,2); + Ixz = -mJ(1,3); + Iyz = -mJ(2,3); - vXYZcg = (Propulsion->GetTanksCG() + EmptyWeight*vbaseXYZcg - + GetPointMassCG() ) / Weight; +// Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation") -// Calculate new moments of inertia here + k1 = (Iyy*Izz - Iyz*Iyz); + k2 = (Iyz*Ixz + Ixy*Izz); + k3 = (Ixy*Iyz + Iyy*Ixz); - Ixx = baseIxx + Propulsion->GetTanksIxx(vXYZcg) + GetPMIxx(); - Iyy = baseIyy + Propulsion->GetTanksIyy(vXYZcg) + GetPMIyy(); - Izz = baseIzz + Propulsion->GetTanksIzz(vXYZcg) + GetPMIzz(); - Ixy = baseIxy + Propulsion->GetTanksIxy(vXYZcg) + GetPMIxy(); - Ixz = baseIxz + Propulsion->GetTanksIxz(vXYZcg) + GetPMIxz(); + 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); @@ -97,7 +141,7 @@ bool FGMassBalance::Run(void) void FGMassBalance::AddPointMass(double weight, double X, double Y, double Z) { - PointMassLoc.push_back(*(new FGColumnVector3(X, Y, Z))); + PointMassLoc.push_back(FGColumnVector3(X, Y, Z)); PointMassWeight.push_back(weight); } @@ -115,7 +159,7 @@ double FGMassBalance::GetPointMassWeight(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGColumnVector3& FGMassBalance::GetPointMassCG(void) +FGColumnVector3& FGMassBalance::GetPointMassMoment(void) { PointMassCG.InitMatrix(); @@ -127,62 +171,80 @@ FGColumnVector3& FGMassBalance::GetPointMassCG(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGMassBalance::GetPMIxx(void) +FGMatrix33& FGMassBalance::CalculatePMInertias(void) { - double I = 0.0; - for (unsigned int i=0; igravity()); - return I; -} + unsigned int size; -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + size = PointMassLoc.size(); + if (size == 0) return pmJ; -double FGMassBalance::GetPMIyy(void) -{ - double I = 0.0; - for (unsigned int i=0; igravity()); - return I; + pmJ = FGMatrix33(); + + for (unsigned int i=0; igravity()); - return I; + // 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))); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGMassBalance::GetPMIxy(void) +void FGMassBalance::bind(void) { - double I = 0.0; - for (unsigned int i=0; igravity()); - return I; + 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); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGMassBalance::GetPMIxz(void) +void FGMassBalance::unbind(void) { - double I = 0.0; - for (unsigned int i=0; igravity()); - return I; + 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"); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -214,8 +276,8 @@ void FGMassBalance::Debug(int from) } } if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGPiston" << endl; - if (from == 1) cout << "Destroyed: FGPiston" << endl; + 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 } @@ -231,5 +293,11 @@ void FGMassBalance::Debug(int from) cout << "MassBalance::Mass out of bounds: " << Mass << endl; } } + if (debug_lvl & 64) { + if (from == 0) { // Constructor + cout << IdSrc << endl; + cout << IdHdr << endl; + } + } +} } -