FGLocation::FGLocation(double lon, double lat, double radius)
: mCacheValid(false)
{
e2 = c = 0.0;
a = ec = ec2 = 1.0;
epa = 0.0;
mLon = mLat = mRadius = 0.0;
mGeodLat = GeodeticAltitude = 0.0;
mTl2ec.InitMatrix();
mTec2l.InitMatrix();
mTi2ec.InitMatrix();
mTec2i.InitMatrix();
mTi2l.InitMatrix();
mTl2i.InitMatrix();
double sinLat = sin(lat);
double cosLat = cos(lat);
double sinLon = sin(lon);
double cosLon = cos(lon);
mECLoc = FGColumnVector3( radius*cosLat*cosLon,
radius*cosLat*sinLon,
radius*sinLat );
}
FGColumnVector3 FGMassBalance::StructuralToBody(const FGColumnVector3& r) const
{
// 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)));
}
FGColumnVector3 FGPropeller::GetPFactor()
{
double px=0.0, py, pz;
py = Thrust * Sense * (GetActingLocationY() - GetLocationY()) / 12.0;
pz = Thrust * Sense * (GetActingLocationZ() - GetLocationZ()) / 12.0;
return FGColumnVector3(px, py, pz);
}
FGColumnVector3 FGPropeller::GetPFactor() const
{
// These are moments in lbf per ft : the lever arm along Z generates a moment
// along the pitch direction.
double p_pitch = Thrust * Sense * (GetActingLocationZ() - GetLocationZ()) / 12.0;
// The lever arm along Y generates a moment along the yaw direction.
double p_yaw = Thrust * Sense * (GetActingLocationY() - GetLocationY()) / 12.0;
return FGColumnVector3(0.0, p_pitch, p_yaw);
}
bool FGPropagate::InitModel(void)
{
if (!FGModel::InitModel()) return false;
// For initialization ONLY:
VState.vLocation.SetEllipse(in.SemiMajor, in.SemiMinor);
VState.vLocation.SetAltitudeAGL(4.0);
VState.dqPQRidot.resize(5, FGColumnVector3(0.0,0.0,0.0));
VState.dqUVWidot.resize(5, FGColumnVector3(0.0,0.0,0.0));
VState.dqInertialVelocity.resize(5, FGColumnVector3(0.0,0.0,0.0));
VState.dqQtrndot.resize(5, FGColumnVector3(0.0,0.0,0.0));
integrator_rotational_rate = eRectEuler;
integrator_translational_rate = eAdamsBashforth2;
integrator_rotational_position = eRectEuler;
integrator_translational_position = eAdamsBashforth3;
return true;
}
void FGLocation::SetPosition(double lon, double lat, double radius)
{
mCacheValid = false;
double sinLat = sin(lat);
double cosLat = cos(lat);
double sinLon = sin(lon);
double cosLon = cos(lon);
mECLoc = FGColumnVector3( radius*cosLat*cosLon,
radius*cosLat*sinLon,
radius*sinLat );
}
FGPropagate::FGPropagate(FGFDMExec* fdmex)
: FGModel(fdmex),
VehicleRadius(0)
{
Debug(0);
Name = "FGPropagate";
/// These define the indices use to select the various integrators.
// eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
integrator_rotational_rate = eRectEuler;
integrator_translational_rate = eAdamsBashforth2;
integrator_rotational_position = eRectEuler;
integrator_translational_position = eAdamsBashforth3;
VState.dqPQRidot.resize(5, FGColumnVector3(0.0,0.0,0.0));
VState.dqUVWidot.resize(5, FGColumnVector3(0.0,0.0,0.0));
VState.dqInertialVelocity.resize(5, FGColumnVector3(0.0,0.0,0.0));
VState.dqQtrndot.resize(5, FGQuaternion(0.0,0.0,0.0));
bind();
Debug(0);
}
void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
{
SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
// Set the position lat/lon/radius
VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
FGIC->GetLatitudeRadIC(),
FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
VehicleRadius = GetRadius();
radInv = 1.0/VehicleRadius;
// Set the Orientation from the euler angles
VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
FGIC->GetThetaRadIC(),
FGIC->GetPsiRadIC() );
// Set the velocities in the instantaneus body frame
VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
FGIC->GetVBodyFpsIC(),
FGIC->GetWBodyFpsIC() );
// Set the angular velocities in the instantaneus body frame.
VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
FGIC->GetQRadpsIC(),
FGIC->GetRRadpsIC() );
// Compute the local frame ECEF velocity
vVel = GetTb2l()*VState.vUVW;
// Finally, make sure that the quaternion stays normalized.
VState.vQtrn.Normalize();
// Recompute the LocalTerrainRadius.
RecomputeLocalTerrainRadius();
}
bool FGAccelerometer::Run(void )
{
// There is no input assumed. This is a dedicated acceleration sensor.
vRadius = MassBalance->StructuralToBody(vLocation);
//gravitational forces
vAccel = Propagate->GetTl2b() * FGColumnVector3(0, 0, Inertial->gravity());
//aircraft forces
vAccel += (Accelerations->GetUVWdot()
+ Accelerations->GetPQRdot() * vRadius
+ Propagate->GetPQR() * (Propagate->GetPQR() * vRadius));
// transform to the specified orientation
vAccel = mT * vAccel;
Input = vAccel(axis);
ProcessSensorSignal();
return true;
}
bool FGPropagate::InitModel(void)
{
if (!FGModel::InitModel()) return false;
// For initialization ONLY:
SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius();
VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
vOmega = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
last2_vPQRdot.InitMatrix();
last_vPQRdot.InitMatrix();
vPQRdot.InitMatrix();
last2_vQtrndot = FGQuaternion(0,0,0);
last_vQtrndot = FGQuaternion(0,0,0);
vQtrndot = FGQuaternion(0,0,0);
last2_vUVWdot.InitMatrix();
last_vUVWdot.InitMatrix();
vUVWdot.InitMatrix();
last2_vLocationDot.InitMatrix();
last_vLocationDot.InitMatrix();
vLocationDot.InitMatrix();
vOmegaLocal.InitMatrix();
integrator_rotational_rate = eAdamsBashforth2;
integrator_translational_rate = eTrapezoidal;
integrator_rotational_position = eAdamsBashforth2;
integrator_translational_position = eTrapezoidal;
return true;
}
FGGasCell::FGGasCell(FGFDMExec* exec, Element* el, unsigned int num,
const struct Inputs& input)
: FGForce(exec), in(input)
{
string token;
Element* element;
FGPropertyManager* PropertyManager = exec->GetPropertyManager();
MassBalance = exec->GetMassBalance();
gasCellJ = FGMatrix33();
gasCellM = FGColumnVector3();
Buoyancy = MaxVolume = MaxOverpressure = Temperature = Pressure =
Contents = Volume = dVolumeIdeal = 0.0;
Xradius = Yradius = Zradius = Xwidth = Ywidth = Zwidth = 0.0;
ValveCoefficient = ValveOpen = 0.0;
CellNum = num;
// NOTE: In the local system X points north, Y points east and Z points down.
SetTransformType(FGForce::tLocalBody);
type = el->GetAttributeValue("type");
if (type == "HYDROGEN") Type = ttHYDROGEN;
else if (type == "HELIUM") Type = ttHELIUM;
else if (type == "AIR") Type = ttAIR;
else Type = ttUNKNOWN;
element = el->FindElement("location");
if (element) {
vXYZ = element->FindElementTripletConvertTo("IN");
} else {
std::stringstream error;
error << "Fatal Error: No location found for this gas cell." << endl;
throw std::runtime_error(error.str());
}
if ((el->FindElement("x_radius") || el->FindElement("x_width")) &&
(el->FindElement("y_radius") || el->FindElement("y_width")) &&
(el->FindElement("z_radius") || el->FindElement("z_width"))) {
if (el->FindElement("x_radius")) {
Xradius = el->FindElementValueAsNumberConvertTo("x_radius", "FT");
}
if (el->FindElement("y_radius")) {
Yradius = el->FindElementValueAsNumberConvertTo("y_radius", "FT");
}
if (el->FindElement("z_radius")) {
Zradius = el->FindElementValueAsNumberConvertTo("z_radius", "FT");
}
if (el->FindElement("x_width")) {
Xwidth = el->FindElementValueAsNumberConvertTo("x_width", "FT");
}
if (el->FindElement("y_width")) {
Ywidth = el->FindElementValueAsNumberConvertTo("y_width", "FT");
}
if (el->FindElement("z_width")) {
Zwidth = el->FindElementValueAsNumberConvertTo("z_width", "FT");
}
// The volume is a (potentially) extruded ellipsoid.
// However, currently only a few combinations of radius and width are
// fully supported.
if ((Xradius != 0.0) && (Yradius != 0.0) && (Zradius != 0.0) &&
(Xwidth == 0.0) && (Ywidth == 0.0) && (Zwidth == 0.0)) {
// Ellipsoid volume.
MaxVolume = 4.0 * M_PI * Xradius * Yradius * Zradius / 3.0;
} else if ((Xradius == 0.0) && (Yradius != 0.0) && (Zradius != 0.0) &&
(Xwidth != 0.0) && (Ywidth == 0.0) && (Zwidth == 0.0)) {
// Cylindrical volume.
MaxVolume = M_PI * Yradius * Zradius * Xwidth;
} else {
cerr << "Warning: Unsupported gas cell shape." << endl;
MaxVolume =
(4.0 * M_PI * Xradius * Yradius * Zradius / 3.0 +
M_PI * Yradius * Zradius * Xwidth +
M_PI * Xradius * Zradius * Ywidth +
M_PI * Xradius * Yradius * Zwidth +
2.0 * Xradius * Ywidth * Zwidth +
2.0 * Yradius * Xwidth * Zwidth +
2.0 * Zradius * Xwidth * Ywidth +
Xwidth * Ywidth * Zwidth);
}
} else {
std::stringstream error;
error << "Fatal Error: Gas cell shape must be given." << endl;
throw std::runtime_error(error.str());
}
if (el->FindElement("max_overpressure")) {
MaxOverpressure = el->FindElementValueAsNumberConvertTo("max_overpressure",
"LBS/FT2");
}
if (el->FindElement("fullness")) {
const double Fullness = el->FindElementValueAsNumber("fullness");
if (0 <= Fullness) {
Volume = Fullness * MaxVolume;
} else {
cerr << "Warning: Invalid initial gas cell fullness value." << endl;
}
}
if (el->FindElement("valve_coefficient")) {
ValveCoefficient =
el->FindElementValueAsNumberConvertTo("valve_coefficient",
"FT4*SEC/SLUG");
ValveCoefficient = max(ValveCoefficient, 0.0);
}
// Initialize state
SetLocation(vXYZ);
if (Temperature == 0.0) {
Temperature = in.Temperature;
}
if (Pressure == 0.0) {
Pressure = in.Pressure;
}
if (Volume != 0.0) {
// Calculate initial gas content.
Contents = Pressure * Volume / (R * Temperature);
// Clip to max allowed value.
const double IdealPressure = Contents * R * Temperature / MaxVolume;
if (IdealPressure > Pressure + MaxOverpressure) {
Contents = (Pressure + MaxOverpressure) * MaxVolume / (R * Temperature);
Pressure = Pressure + MaxOverpressure;
} else {
Pressure = max(IdealPressure, Pressure);
}
} else {
// Calculate initial gas content.
Contents = Pressure * MaxVolume / (R * Temperature);
}
Volume = Contents * R * Temperature / Pressure;
Mass = Contents * M_gas();
// Bind relevant properties
string property_name, base_property_name;
base_property_name = CreateIndexedPropertyName("buoyant_forces/gas-cell", CellNum);
property_name = base_property_name + "/max_volume-ft3";
PropertyManager->Tie( property_name.c_str(), &MaxVolume, false );
PropertyManager->GetNode()->SetWritable( property_name, false );
property_name = base_property_name + "/temp-R";
PropertyManager->Tie( property_name.c_str(), &Temperature, false );
property_name = base_property_name + "/pressure-psf";
PropertyManager->Tie( property_name.c_str(), &Pressure, false );
property_name = base_property_name + "/volume-ft3";
PropertyManager->Tie( property_name.c_str(), &Volume, false );
property_name = base_property_name + "/buoyancy-lbs";
PropertyManager->Tie( property_name.c_str(), &Buoyancy, false );
property_name = base_property_name + "/contents-mol";
PropertyManager->Tie( property_name.c_str(), &Contents, false );
property_name = base_property_name + "/valve_open";
PropertyManager->Tie( property_name.c_str(), &ValveOpen, false );
Debug(0);
// Read heat transfer coefficients
if (Element* heat = el->FindElement("heat")) {
Element* function_element = heat->FindElement("function");
while (function_element) {
HeatTransferCoeff.push_back(new FGFunction(PropertyManager,
function_element));
function_element = heat->FindNextElement("function");
}
}
// Load ballonets if there are any
if (Element* ballonet_element = el->FindElement("ballonet")) {
while (ballonet_element) {
Ballonet.push_back(new FGBallonet(exec,
ballonet_element,
Ballonet.size(),
this, in));
ballonet_element = el->FindNextElement("ballonet");
}
}
}
bool FGAuxiliary::Run()
{
double A,B,D;
if (FGModel::Run()) return true; // return true if error returned from base class
if (FDMExec->Holding()) return false;
const FGColumnVector3& vPQR = Propagate->GetPQR();
const FGColumnVector3& vUVW = Propagate->GetUVW();
const FGColumnVector3& vUVWdot = Propagate->GetUVWdot();
const FGColumnVector3& vVel = Propagate->GetVel();
p = Atmosphere->GetPressure();
rhosl = Atmosphere->GetDensitySL();
psl = Atmosphere->GetPressureSL();
sat = Atmosphere->GetTemperature();
// Rotation
double cTht = Propagate->GetCosEuler(eTht);
double sTht = Propagate->GetSinEuler(eTht);
double cPhi = Propagate->GetCosEuler(ePhi);
double sPhi = Propagate->GetSinEuler(ePhi);
vEulerRates(eTht) = vPQR(eQ)*cPhi - vPQR(eR)*sPhi;
if (cTht != 0.0) {
vEulerRates(ePsi) = (vPQR(eQ)*sPhi + vPQR(eR)*cPhi)/cTht;
vEulerRates(ePhi) = vPQR(eP) + vEulerRates(ePsi)*sTht;
}
// 12/16/2005, JSB: For ground handling purposes, at this time, let's ramp
// in the effects of wind from 10 fps to 30 fps when there is weight on the
// landing gear wheels.
if (GroundReactions->GetWOW() && vUVW(eU) < 10) {
vAeroPQR = vPQR;
vAeroUVW = vUVW;
} else if (GroundReactions->GetWOW() && vUVW(eU) < 30) {
double factor = (vUVW(eU) - 10.0)/20.0;
vAeroPQR = vPQR - factor*Atmosphere->GetTurbPQR();
vAeroUVW = vUVW - factor*Propagate->GetTl2b()*Atmosphere->GetTotalWindNED();
} else {
FGColumnVector3 wind = Propagate->GetTl2b()*Atmosphere->GetTotalWindNED();
vAeroPQR = vPQR - Atmosphere->GetTurbPQR();
vAeroUVW = vUVW - wind;
}
Vt = vAeroUVW.Magnitude();
if ( Vt > 0.05) {
if (vAeroUVW(eW) != 0.0)
alpha = vAeroUVW(eU)*vAeroUVW(eU) > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0;
if (vAeroUVW(eV) != 0.0)
beta = vAeroUVW(eU)*vAeroUVW(eU)+vAeroUVW(eW)*vAeroUVW(eW) > 0.0 ? atan2(vAeroUVW(eV),
sqrt(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW))) : 0.0;
double mUW = (vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW));
double signU=1;
if (vAeroUVW(eU) != 0.0)
signU = vAeroUVW(eU)/fabs(vAeroUVW(eU));
if ( (mUW == 0.0) || (Vt == 0.0) ) {
adot = 0.0;
bdot = 0.0;
} else {
adot = (vAeroUVW(eU)*vUVWdot(eW) - vAeroUVW(eW)*vUVWdot(eU))/mUW;
bdot = (signU*mUW*vUVWdot(eV) - vAeroUVW(eV)*(vAeroUVW(eU)*vUVWdot(eU)
+ vAeroUVW(eW)*vUVWdot(eW)))/(Vt*Vt*sqrt(mUW));
}
} else {
alpha = beta = adot = bdot = 0;
}
Re = Vt * Aircraft->Getcbar() / Atmosphere->GetKinematicViscosity();
qbar = 0.5*Atmosphere->GetDensity()*Vt*Vt;
qbarUW = 0.5*Atmosphere->GetDensity()*(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW));
qbarUV = 0.5*Atmosphere->GetDensity()*(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eV)*vAeroUVW(eV));
Mach = Vt / Atmosphere->GetSoundSpeed();
MachU = vMachUVW(eU) = vAeroUVW(eU) / Atmosphere->GetSoundSpeed();
vMachUVW(eV) = vAeroUVW(eV) / Atmosphere->GetSoundSpeed();
vMachUVW(eW) = vAeroUVW(eW) / Atmosphere->GetSoundSpeed();
// Position
Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) );
psigt = atan2(vVel(eEast), vVel(eNorth));
if (psigt < 0.0) psigt += 2*M_PI;
gamma = atan2(-vVel(eDown), Vground);
tat = sat*(1 + 0.2*Mach*Mach); // Total Temperature, isentropic flow
tatc = RankineToCelsius(tat);
if (MachU < 1) { // Calculate total pressure assuming isentropic flow
pt = p*pow((1 + 0.2*MachU*MachU),3.5);
} else {
// Use Rayleigh pitot tube formula for normal shock in front of pitot tube
B = 5.76*MachU*MachU/(5.6*MachU*MachU - 0.8);
D = (2.8*MachU*MachU-0.4)*0.4167;
pt = p*pow(B,3.5)*D;
}
A = pow(((pt-p)/psl+1),0.28571);
if (MachU > 0.0) {
vcas = sqrt(7*psl/rhosl*(A-1));
veas = sqrt(2*qbar/rhosl);
} else {
vcas = veas = 0.0;
}
vPilotAccel.InitMatrix();
if ( Vt > 1.0 ) {
vAircraftAccel = Aerodynamics->GetForces()
+ Propulsion->GetForces()
+ GroundReactions->GetForces()
+ ExternalReactions->GetForces()
+ BuoyantForces->GetForces();
vAircraftAccel /= MassBalance->GetMass();
// Nz is Acceleration in "g's", along normal axis (-Z body axis)
Nz = -vAircraftAccel(eZ)/Inertial->gravity();
vToEyePt = MassBalance->StructuralToBody(Aircraft->GetXYZep());
vPilotAccel = vAircraftAccel + Propagate->GetPQRdot() * vToEyePt;
vPilotAccel += vPQR * (vPQR * vToEyePt);
} else {
// The line below handles low velocity (and on-ground) cases, basically
// representing the opposite of the force that the landing gear would
// exert on the ground (which is just the total weight). This eliminates
// any jitter that could be introduced by the landing gear. Theoretically,
// this branch could be eliminated, with a penalty of having a short
// transient at startup (lasting only a fraction of a second).
vPilotAccel = Propagate->GetTl2b() * FGColumnVector3( 0.0, 0.0, -Inertial->gravity() );
Nz = -vPilotAccel(eZ)/Inertial->gravity();
}
vPilotAccelN = vPilotAccel/Inertial->gravity();
// VRP computation
const FGLocation& vLocation = Propagate->GetLocation();
FGColumnVector3 vrpStructural = Aircraft->GetXYZvrp();
FGColumnVector3 vrpBody = MassBalance->StructuralToBody( vrpStructural );
FGColumnVector3 vrpLocal = Propagate->GetTb2l() * vrpBody;
vLocationVRP = vLocation.LocalToLocation( vrpLocal );
// Recompute some derived values now that we know the dependent parameters values ...
hoverbcg = Propagate->GetDistanceAGL() / Aircraft->GetWingSpan();
FGColumnVector3 vMac = Propagate->GetTb2l()*MassBalance->StructuralToBody(Aircraft->GetXYZrp());
hoverbmac = (Propagate->GetDistanceAGL() + vMac(3)) / Aircraft->GetWingSpan();
// when all model are executed,
// please calculate the distance from the initial point
CalculateRelativePosition();
return false;
}
