X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGMassBalance.cpp;h=f925c8fccd0fd66500bb877c344197c536373f06;hb=fbee3d10f0aafd4178fc1313edb8593c156b2874;hp=5df0f3c187c89087eae20a78ea3d66024d372bf3;hpb=f04d342f6a5e42e1a80c62b5e6dbf71e68b3ba06;p=flightgear.git diff --git a/src/FDM/JSBSim/FGMassBalance.cpp b/src/FDM/JSBSim/FGMassBalance.cpp index 5df0f3c18..f925c8fcc 100644 --- a/src/FDM/JSBSim/FGMassBalance.cpp +++ b/src/FDM/JSBSim/FGMassBalance.cpp @@ -39,8 +39,11 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGMassBalance.h" +#include "FGPropulsion.h" #include "FGPropertyManager.h" +namespace JSBSim { + static const char *IdSrc = "$Id$"; static const char *IdHdr = ID_MASSBALANCE; @@ -53,11 +56,13 @@ FGMassBalance::FGMassBalance(FGFDMExec* fdmex) : FGModel(fdmex) { Name = "FGMassBalance"; Weight = EmptyWeight = Mass = 0.0; - Ixx = Iyy = Izz = Ixy = Ixz = 0.0; - baseIxx = baseIyy = baseIzz = baseIxy = baseIxz = 0.0; - vbaseXYZcg(eX) = 0.0; - vbaseXYZcg(eY) = 0.0; - vbaseXYZcg(eZ) = 0.0; + + vbaseXYZcg.InitMatrix(0.0); + baseJ.InitMatrix(); + mJ.InitMatrix(); + mJinv.InitMatrix(); + pmJ.InitMatrix(); + bind(); Debug(0); @@ -75,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 here. +// Calculate new CG vXYZcg = (Propulsion->GetTanksMoment() + EmptyWeight*vbaseXYZcg + GetPointMassMoment() ) / Weight; -// Calculate new moments of inertia here - - 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(); +// 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); @@ -106,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); } @@ -136,83 +171,63 @@ FGColumnVector3& FGMassBalance::GetPointMassMoment(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; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -double FGMassBalance::GetPMIxy(void) -{ - double I = 0.0; - for (unsigned int i=0; igravity()); - return I; + return pmJ; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGMassBalance::GetPMIxz(void) +FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const { - double I = 0.0; - 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))); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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/ixx-lbsft2", this, - &FGMassBalance::GetIxx); - PropertyManager->Tie("inertia/iyy-lbsft2", this, - &FGMassBalance::GetIyy); - PropertyManager->Tie("inertia/izz-lbsft2", this, - &FGMassBalance::GetIzz); - PropertyManager->Tie("inertia/ixy-lbsft2", this, - &FGMassBalance::GetIxy); - PropertyManager->Tie("inertia/ixz-lbsft2", this, - &FGMassBalance::GetIxz); PropertyManager->Tie("inertia/cg-x-ft", this,1, (PMF)&FGMassBalance::GetXYZcg); PropertyManager->Tie("inertia/cg-y-ft", this,2, @@ -227,11 +242,6 @@ void FGMassBalance::unbind(void) { PropertyManager->Untie("inertia/mass-slugs"); PropertyManager->Untie("inertia/weight-lbs"); - PropertyManager->Untie("inertia/ixx-lbsft2"); - PropertyManager->Untie("inertia/iyy-lbsft2"); - PropertyManager->Untie("inertia/izz-lbsft2"); - PropertyManager->Untie("inertia/ixy-lbsft2"); - PropertyManager->Untie("inertia/ixz-lbsft2"); PropertyManager->Untie("inertia/cg-x-ft"); PropertyManager->Untie("inertia/cg-y-ft"); PropertyManager->Untie("inertia/cg-z-ft"); @@ -266,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 } @@ -290,4 +300,4 @@ void FGMassBalance::Debug(int from) } } } - +}