bool FGMassBalance::Load(Element* document)
{
string element_name = "";
Name = "Mass Properties Model: " + document->GetAttributeValue("name");
// Perform base class Pre-Load
if (!FGModel::Load(document))
return false;
SetAircraftBaseInertias(ReadInertiaMatrix(document));
if (document->FindElement("emptywt")) {
EmptyWeight = document->FindElementValueAsNumberConvertTo("emptywt", "LBS");
}
Element *element = document->FindElement("location");
while (element) {
element_name = element->GetAttributeValue("name");
if (element_name == "CG") vbaseXYZcg = element->FindElementTripletConvertTo("IN");
element = document->FindNextElement("location");
}
// Find all POINTMASS elements that descend from this METRICS branch of the
// config file.
element = document->FindElement("pointmass");
while (element) {
AddPointMass(element);
element = document->FindNextElement("pointmass");
}
double ChildFDMWeight = 0.0;
for (int fdm=0; fdm<FDMExec->GetFDMCount(); fdm++) {
if (FDMExec->GetChildFDM(fdm)->mated) ChildFDMWeight += FDMExec->GetChildFDM(fdm)->exec->GetMassBalance()->GetWeight();
}
Weight = EmptyWeight + in.TanksWeight + GetTotalPointMassWeight()
+ in.GasMass*slugtolb + ChildFDMWeight;
Mass = lbtoslug*Weight;
PostLoad(document, PropertyManager);
Debug(2);
return true;
}
bool FGMassBalance::Run(bool Holding)
{
double denom, k1, k2, k3, k4, k5, k6;
double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
if (FGModel::Run(Holding)) return true;
if (Holding) return false;
RunPreFunctions();
double ChildFDMWeight = 0.0;
for (int fdm=0; fdm<FDMExec->GetFDMCount(); fdm++) {
if (FDMExec->GetChildFDM(fdm)->mated) ChildFDMWeight += FDMExec->GetChildFDM(fdm)->exec->GetMassBalance()->GetWeight();
}
Weight = EmptyWeight + in.TanksWeight + GetTotalPointMassWeight()
+ in.GasMass*slugtolb + ChildFDMWeight;
Mass = lbtoslug*Weight;
// Calculate new CG
vXYZcg = (EmptyWeight*vbaseXYZcg
+ GetPointMassMoment()
+ in.TanksMoment
+ in.GasMoment) / Weight;
// Track frame-by-frame delta CG, and move the EOM-tracked location
// by this amount.
if (vLastXYZcg.Magnitude() == 0.0) vLastXYZcg = vXYZcg;
vDeltaXYZcg = vXYZcg - vLastXYZcg;
vDeltaXYZcgBody = StructuralToBody(vLastXYZcg) - StructuralToBody(vXYZcg);
vLastXYZcg = vXYZcg;
FDMExec->GetPropagate()->NudgeBodyLocation(vDeltaXYZcgBody);
// 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 += in.TankInertia;
mJ += in.GasInertia;
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 );
RunPostFunctions();
Debug(0);
return false;
}
bool FGMassBalance::Load(Element* elem)
{
string element_name = "";
double bixx, biyy, bizz, bixy, bixz, biyz;
string fname="", file="";
string separator = "/";
fname = elem->GetAttributeValue("file");
if (!fname.empty()) {
file = FDMExec->GetFullAircraftPath() + separator + fname;
document = LoadXMLDocument(file);
if (document == 0L) return false;
} else {
document = elem;
}
FGModel::Load(document); // Perform base class Load.
bixx = biyy = bizz = bixy = bixz = biyz = 0.0;
if (document->FindElement("ixx"))
bixx = document->FindElementValueAsNumberConvertTo("ixx", "SLUG*FT2");
if (document->FindElement("iyy"))
biyy = document->FindElementValueAsNumberConvertTo("iyy", "SLUG*FT2");
if (document->FindElement("izz"))
bizz = document->FindElementValueAsNumberConvertTo("izz", "SLUG*FT2");
if (document->FindElement("ixy"))
bixy = document->FindElementValueAsNumberConvertTo("ixy", "SLUG*FT2");
if (document->FindElement("ixz"))
bixz = document->FindElementValueAsNumberConvertTo("ixz", "SLUG*FT2");
if (document->FindElement("iyz"))
biyz = document->FindElementValueAsNumberConvertTo("iyz", "SLUG*FT2");
SetAircraftBaseInertias(FGMatrix33( bixx, -bixy, bixz,
-bixy, biyy, -biyz,
bixz, -biyz, bizz ));
if (document->FindElement("emptywt")) {
EmptyWeight = document->FindElementValueAsNumberConvertTo("emptywt", "LBS");
}
Element *element = document->FindElement("location");
while (element) {
element_name = element->GetAttributeValue("name");
if (element_name == "CG") vbaseXYZcg = element->FindElementTripletConvertTo("IN");
element = document->FindNextElement("location");
}
// Find all POINTMASS elements that descend from this METRICS branch of the
// config file.
element = document->FindElement("pointmass");
while (element) {
AddPointMass(element);
element = document->FindNextElement("pointmass");
}
double ChildFDMWeight = 0.0;
for (int fdm=0; fdm<FDMExec->GetFDMCount(); fdm++) {
if (FDMExec->GetChildFDM(fdm)->mated) ChildFDMWeight += FDMExec->GetChildFDM(fdm)->exec->GetMassBalance()->GetWeight();
}
Weight = EmptyWeight + in.TanksWeight + GetTotalPointMassWeight()
+ in.GasMass*slugtolb + ChildFDMWeight;
Mass = lbtoslug*Weight;
PostLoad(document, PropertyManager);
Debug(2);
return true;
}
