namespace JSBSim {
-static const char *IdSrc = "$Id: FGFDMExec.cpp,v 1.115 2011/09/25 11:56:00 bcoconni Exp $";
+static const char *IdSrc = "$Id: FGFDMExec.cpp,v 1.118 2011/10/22 15:11:23 bcoconni Exp $";
static const char *IdHdr = ID_FDMEXEC;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Frame = 0;
Error = 0;
- GroundCallback = 0;
+ GroundCallback = new FGDefaultGroundCallback();
IC = 0;
Trim = 0;
Script = 0;
if (FDMctr > 0) (*FDMctr)--;
+ if(GroundCallback)
+ delete GroundCallback;
+
Debug(1);
}
Models[ePropulsion] = new FGPropulsion(this);
Models[eAerodynamics] = new FGAerodynamics (this);
- GroundCallback = new FGGroundCallback(((FGInertial*)Models[eInertial])->GetRefRadius());
+ GroundCallback->SetSeaLevelRadius(((FGInertial*)Models[eInertial])->GetRefRadius());
Models[eGroundReactions] = new FGGroundReactions(this);
Models[eExternalReactions] = new FGExternalReactions(this);
delete IC;
delete Trim;
- delete GroundCallback;
-
Error = 0;
modelLoaded = false;
firstPass = false;
}
+ IncrTime();
+
// returns true if success, false if complete
if (Script != 0 && !IntegrationSuspended()) success = Script->RunScript();
- IncrTime();
-
for (unsigned int i = 0; i < Models.size(); i++) {
LoadInputs(i);
Models[i]->Run(holding);
Propagate->InitializeDerivatives();
LoadInputs(eAtmosphere);
Atmosphere->Run(false);
- Winds->SetWindNED( FGIC->GetWindNFpsIC(),
- FGIC->GetWindEFpsIC(),
- FGIC->GetWindDFpsIC() );
+ Winds->SetWindNED(FGIC->GetWindNEDFpsIC());
Auxiliary->Run(false);
}
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_FDMEXEC "$Id: FGFDMExec.h,v 1.71 2011/09/07 02:37:04 jberndt Exp $"
+#define ID_FDMEXEC "$Id: FGFDMExec.h,v 1.72 2011/10/14 22:46:49 bcoconni Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
property actually maps toa function call of DoTrim().
@author Jon S. Berndt
- @version $Revision: 1.71 $
+ @version $Revision: 1.72 $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool RunIC(void);
/** Sets the ground callback pointer.
- @param gc A pointer to a ground callback object. */
+ @param gc A pointer to a ground callback object.
+ */
void SetGroundCallback(FGGroundCallback* gc);
/** Loads an aircraft model.
namespace JSBSim {
-static const char *IdSrc = "$Id: FGJSBBase.cpp,v 1.31 2011/07/27 03:55:48 jberndt Exp $";
+static const char *IdSrc = "$Id: FGJSBBase.cpp,v 1.32 2011/10/22 14:38:30 bcoconni Exp $";
static const char *IdHdr = ID_JSBBASE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+double FGJSBBase::VcalibratedFromMach(double mach, double p, double psl, double rhosl)
+{
+ double pt,A;
+
+ if (mach < 0) mach=0;
+ if (mach < 1) //calculate total pressure assuming isentropic flow
+ pt=p*pow((1 + 0.2*mach*mach),3.5);
+ else {
+ // shock in front of pitot tube, we'll assume its normal and use
+ // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
+ // pressure behind the shock to the static pressure in front of
+ // the normal shock assumption should not be a bad one -- most supersonic
+ // aircraft place the pitot probe out front so that it is the forward
+ // most point on the aircraft. The real shock would, of course, take
+ // on something like the shape of a rounded-off cone but, here again,
+ // the assumption should be good since the opening of the pitot probe
+ // is very small and, therefore, the effects of the shock curvature
+ // should be small as well. AFAIK, this approach is fairly well accepted
+ // within the aerospace community
+
+ // The denominator below is zero for Mach ~ 0.38, for which
+ // we'll never be here, so we're safe
+
+ pt = p*166.92158*pow(mach,7.0)/pow(7*mach*mach-1,2.5);
+ }
+
+ A = pow(((pt-p)/psl+1),0.28571);
+ return sqrt(7*psl/rhosl*(A-1));
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+double FGJSBBase::MachFromVcalibrated(double vcas, double p, double psl, double rhosl)
+{
+ double pt = p + psl*(pow(1+vcas*vcas*rhosl/(7.0*psl),3.5)-1);
+
+ if (pt/p < 1.89293)
+ return sqrt(5.0*(pow(pt/p, 0.2857143) -1)); // Mach < 1
+ else {
+ // Mach >= 1
+ double mach = sqrt(0.77666*pt/p); // Initial guess is based on a quadratic approximation of the Rayleigh formula
+ double delta = 1.;
+ double target = pt/(166.92158*p);
+ int iter = 0;
+
+ // Find the root with Newton-Raphson. Since the differential is never zero,
+ // the function is monotonic and has only one root with a multiplicity of one.
+ // Convergence is certain.
+ while (delta > 1E-5 && iter < 10) {
+ double m2 = mach*mach; // Mach^2
+ double m6 = m2*m2*m2; // Mach^6
+ delta = mach*m6/pow(7.0*m2-1.0,2.5) - target;
+ double diff = 7.0*m6*(2.0*m2-1)/pow(7.0*m2-1.0,3.5); // Never zero when Mach >= 1
+ mach -= delta/diff;
+ iter++;
+ }
+
+ return mach;
+ }
+}
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
} // namespace JSBSim
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_JSBBASE "$Id: FGJSBBase.h,v 1.33 2011/06/27 03:14:25 jberndt Exp $"
+#define ID_JSBBASE "$Id: FGJSBBase.h,v 1.34 2011/10/22 14:38:30 bcoconni Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
* This class provides universal constants, utility functions, messaging
* functions, and enumerated constants to JSBSim.
@author Jon S. Berndt
- @version $Id: FGJSBBase.h,v 1.33 2011/06/27 03:14:25 jberndt Exp $
+ @version $Id: FGJSBBase.h,v 1.34 2011/10/22 14:38:30 bcoconni Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
return kelvin - 273.15;
}
+ /** Calculate the calibrated airspeed from the Mach number. It uses the
+ * Rayleigh formula for supersonic speeds (See "Introduction to Aerodynamics
+ * of a Compressible Fluid - H.W. Liepmann, A.E. Puckett - Wiley & sons
+ * (1947)" ยง5.4 pp 75-80)
+ * @param mach The Mach number
+ * @param p Pressure in psf
+ * @param psl Pressure at sea level in psf
+ * @param rhosl Density at sea level in slugs/ft^3
+ * @return The calibrated airspeed (CAS) in ft/s
+ * */
+ static double VcalibratedFromMach(double mach, double p, double psl, double rhosl);
+
+ /** Calculate the Mach number from the calibrated airspeed. For subsonic
+ * speeds, the reversed formula has a closed form. For supersonic speeds, the
+ * Rayleigh formula is reversed by the Newton-Raphson algorithm.
+ * @param vcas The calibrated airspeed (CAS) in ft/s
+ * @param p Pressure in psf
+ * @param psl Pressure at sea level in psf
+ * @param rhosl Density at sea level in slugs/ft^3
+ * @return The Mach number
+ * */
+ static double MachFromVcalibrated(double vcas, double p, double psl, double rhosl);
+
/** Finite precision comparison.
@param a first value to compare
@param b second value to compare
cont = FGColumnVector3( contact[0], contact[1], contact[2] );
return agl;
}
+
+ virtual double GetTerrainGeoCentRadius(double t, const FGLocation& l) const {
+ double loc_cart[3] = { l(eX), l(eY), l(eZ) };
+ double contact[3], normal[3], vel[3], angularVel[3], agl = 0;
+ mInterface->get_agl_ft(t, loc_cart, SG_METER_TO_FEET*2, contact, normal,
+ vel, angularVel, &agl);
+ return sqrt(contact[0]*contact[0]+contact[1]*contact[1]+contact[2]*contact[2]);
+ }
+
+ virtual double GetSeaLevelRadius(const FGLocation& l) const {
+ double seaLevelRadius, latGeoc;
+
+ sgGeodToGeoc(l.GetGeodLatitudeRad(), l.GetGeodAltitude(),
+ &seaLevelRadius, &latGeoc);
+
+ return seaLevelRadius * SG_METER_TO_FEET;
+ }
+
+ virtual void SetTerrainGeoCentRadius(double radius) {}
+ virtual void SetSeaLevelRadius(double radius) {}
private:
FGJSBsim* mInterface;
};
-wind_from_east->getDoubleValue(),
-wind_from_down->getDoubleValue() );
- //Atmosphere->SetExTemperature(get_Static_temperature());
- //Atmosphere->SetExPressure(get_Static_pressure());
- //Atmosphere->SetExDensity(get_Density());
SG_LOG(SG_FLIGHT,SG_INFO,"T,p,rho: " << Atmosphere->GetTemperature()
<< ", " << Atmosphere->GetPressure()
<< ", " << Atmosphere->GetDensity() );
needTrim = startup_trim->getBoolValue();
common_init();
- fgic->SetSeaLevelRadiusFtIC( get_Sea_level_radius() );
copy_to_JSBsim();
fdmex->RunIC(); //loop JSBSim once w/o integrating
} // end FGEngine code block
}
-
- Propagate->SetSeaLevelRadius( get_Sea_level_radius() );
-
Atmosphere->SetTemperature(temperature->getDoubleValue(), get_Altitude(), FGAtmosphere::eCelsius);
Atmosphere->SetPressureSL(pressureSL->getDoubleValue(), FGAtmosphere::eInchesHg);
fgic->SetSeaLevelRadiusFtIC( sea_level_radius_ft );
fgic->SetLatitudeRadIC( lat_geoc );
}
- else {
- Propagate->SetSeaLevelRadius( sea_level_radius_ft );
+ else
Propagate->SetLatitude(lat_geoc);
- FGInterface::set_Latitude(lat);
- }
+
+ FGInterface::set_Latitude(lat);
}
if (needTrim)
fgic->SetLongitudeRadIC(lon);
- else {
+ else
Propagate->SetLongitude(lon);
- FGInterface::set_Longitude(lon);
- }
+
+ FGInterface::set_Longitude(lon);
}
// Sets the altitude above sea level.
if (needTrim)
fgic->SetAltitudeASLFtIC(alt);
- else {
+ else
Propagate->SetAltitudeASL(alt);
- FGInterface::set_Altitude(alt);
- }
+
+ FGInterface::set_Altitude(alt);
}
void FGJSBsim::set_V_calibrated_kts(double vc)
if (needTrim)
fgic->SetVcalibratedKtsIC(vc);
else {
- double mach = getMachFromVcas(vc);
+ double p=pressure->getDoubleValue();
+ double psl=fdmex->GetAtmosphere()->GetPressureSL();
+ double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
+ double mach = FGJSBBase::MachFromVcalibrated(vc, p, psl, rhosl);
double temp = 1.8*(temperature->getDoubleValue()+273.15);
double soundSpeed = sqrt(1.4*1716.0*temp);
FGColumnVector3 vUVW = Propagate->GetUVW();
Propagate->SetUVW(1, vUVW(1));
Propagate->SetUVW(2, vUVW(2));
Propagate->SetUVW(3, vUVW(3));
-
- FGInterface::set_V_calibrated_kts(vc);
}
+
+ FGInterface::set_V_calibrated_kts(vc);
}
void FGJSBsim::set_Mach_number(double mach)
Propagate->SetUVW(1, vUVW(1));
Propagate->SetUVW(2, vUVW(2));
Propagate->SetUVW(3, vUVW(3));
-
- FGInterface::set_Mach_number(mach);
}
+
+ FGInterface::set_Mach_number(mach);
}
void FGJSBsim::set_Velocities_Local( double north, double east, double down )
Propagate->SetUVW(1, vUVW(1));
Propagate->SetUVW(2, vUVW(2));
Propagate->SetUVW(3, vUVW(3));
-
- FGInterface::set_Velocities_Local(north, east, down);
}
+
+ FGInterface::set_Velocities_Local(north, east, down);
}
void FGJSBsim::set_Velocities_Wind_Body( double u, double v, double w)
Propagate->SetUVW(1, u);
Propagate->SetUVW(2, v);
Propagate->SetUVW(3, w);
-
- FGInterface::set_Velocities_Wind_Body(u, v, w);
}
+
+ FGInterface::set_Velocities_Wind_Body(u, v, w);
}
//Euler angles
FGMatrix33 Ti2b = Tl2b*Propagate->GetTi2l();
FGQuaternion Qi = Ti2b.GetQuaternion();
Propagate->SetInertialOrientation(Qi);
-
- FGInterface::set_Euler_Angles(phi, theta, psi);
}
+
+ FGInterface::set_Euler_Angles(phi, theta, psi);
}
//Flight Path
Propagate->SetUVW(1, vUVW(1));
Propagate->SetUVW(2, vUVW(2));
Propagate->SetUVW(3, vUVW(3));
-
- FGInterface::set_Climb_Rate(roc);
}
+
+ FGInterface::set_Climb_Rate(roc);
}
}
Propagate->SetUVW(1, vUVW(1));
Propagate->SetUVW(2, vUVW(2));
Propagate->SetUVW(3, vUVW(3));
-
- FGInterface::set_Gamma_vert_rad(gamma);
- }
- }
-}
-// Reverse the VCAS formula to obtain the corresponding Mach number. For subsonic
-// speeds, the reversed formula has a closed form. For supersonic speeds, the
-// formula is reversed by the Newton-Raphson algorithm.
-
-double FGJSBsim::getMachFromVcas(double vcas)
-{
- double p=pressure->getDoubleValue();
- double psl=fdmex->GetAtmosphere()->GetPressureSL();
- double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
-
- double pt = p + psl*(pow(1+vcas*vcas*rhosl/(7.0*psl),3.5)-1);
-
- if (pt/p < 1.89293)
- return sqrt(5.0*(pow(pt/p, 0.2857143) -1)); // Mach < 1
- else {
- // Mach >= 1
- double mach = sqrt(0.77666*pt/p); // Initial guess is based on a quadratic approximation of the Rayleigh formula
- double delta = 1.;
- double target = pt/(166.92158*p);
- int iter = 0;
-
- // Find the root with Newton-Raphson. Since the differential is never zero,
- // the function is monotonic and has only one root with a multiplicity of one.
- // Convergence is certain.
- while (delta > 1E-5 && iter < 10) {
- double m2 = mach*mach; // Mach^2
- double m6 = m2*m2*m2; // Mach^6
- delta = mach*m6/pow(7.0*m2-1.0,2.5) - target;
- double diff = 7.0*m6*(2.0*m2-1)/pow(7.0*m2-1.0,3.5); // Never zero when Mach >= 1
- mach -= delta/diff;
- iter++;
}
- return mach;
+ FGInterface::set_Gamma_vert_rad(gamma);
}
}
void do_trim(void);
- double getMachFromVcas(double vcas);
bool update_ground_cache(JSBSim::FGLocation cart, double* cart_pos, double dt);
void init_gear(void);
void update_gear(void);
namespace JSBSim {
-static const char *IdSrc = "$Id: FGInitialCondition.cpp,v 1.69 2011/08/04 12:46:32 jberndt Exp $";
+static const char *IdSrc = "$Id: FGInitialCondition.cpp,v 1.75 2011/10/23 15:05:32 bcoconni Exp $";
static const char *IdHdr = ID_INITIALCONDITION;
//******************************************************************************
if(FDMExec != NULL ) {
PropertyManager=fdmex->GetPropertyManager();
+ Atmosphere=fdmex->GetAtmosphere();
Constructing = true;
bind();
Constructing = false;
InitializeIC();
- p = p0; q = q0; r = r0;
+ vPQR_body = FGColumnVector3(p0, q0, r0);
alpha = alpha0; beta = beta0;
- phi = phi0; theta = theta0; psi = psi0;
position.SetPosition(lonRad0, latRad0, altAGLFt0 + terrain_elevation + sea_level_radius);
- FGQuaternion Quat(phi, theta, psi);
- Quat.Normalize();
- Tl2b = Quat.GetT();
- Tb2l = Tl2b.Transposed();
+ orientation = FGQuaternion(phi0, theta0, psi0);
+ const FGMatrix33& Tb2l = orientation.GetTInv();
vUVW_NED = Tb2l * FGColumnVector3(u0, v0, w0);
vt = vUVW_NED.Magnitude();
void FGInitialCondition::InitializeIC(void)
{
alpha=beta=0;
- theta=phi=psi=0;
terrain_elevation = 0;
sea_level_radius = fdmex->GetInertial()->GetRefRadius();
+ position.SetEllipse(fdmex->GetInertial()->GetSemimajor(), fdmex->GetInertial()->GetSemiminor());
position.SetPosition(0., 0., sea_level_radius);
position.SetEarthPositionAngle(fdmex->GetPropagate()->GetEarthPositionAngle());
+ orientation = FGQuaternion(0.0, 0.0, 0.0);
vUVW_NED.InitMatrix();
- p=q=r=0;
+ vPQR_body.InitMatrix();
vt=0;
targetNlfIC = 1.0;
Tw2b.InitMatrix(1., 0., 0., 0., 1., 0., 0., 0., 1.);
Tb2w.InitMatrix(1., 0., 0., 0., 1., 0., 0., 0., 1.);
- Tl2b.InitMatrix(1., 0., 0., 0., 1., 0., 0., 0., 1.);
- Tb2l.InitMatrix(1., 0., 0., 0., 1., 0., 0., 0., 1.);
}
//******************************************************************************
if (outfile.is_open()) {
outfile << "<?xml version=\"1.0\"?>" << endl;
outfile << "<initialize name=\"reset00\">" << endl;
- outfile << " <ubody unit=\"FT/SEC\"> " << Propagate->GetUVW(eX) << " </ubody> " << endl;
- outfile << " <vbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eY) << " </vbody> " << endl;
- outfile << " <wbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eZ) << " </wbody> " << endl;
- outfile << " <phi unit=\"DEG\"> " << Propagate->GetEuler(ePhi) << " </phi>" << endl;
- outfile << " <theta unit=\"DEG\"> " << Propagate->GetEuler(eTht) << " </theta>" << endl;
- outfile << " <psi unit=\"DEG\"> " << Propagate->GetEuler(ePsi) << " </psi>" << endl;
+ outfile << " <ubody unit=\"FT/SEC\"> " << Propagate->GetUVW(eU) << " </ubody> " << endl;
+ outfile << " <vbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eV) << " </vbody> " << endl;
+ outfile << " <wbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eW) << " </wbody> " << endl;
+ outfile << " <phi unit=\"DEG\"> " << Propagate->GetEuler(ePhi)*radtodeg << " </phi>" << endl;
+ outfile << " <theta unit=\"DEG\"> " << Propagate->GetEuler(eTht)*radtodeg << " </theta>" << endl;
+ outfile << " <psi unit=\"DEG\"> " << Propagate->GetEuler(ePsi)*radtodeg << " </psi>" << endl;
outfile << " <longitude unit=\"DEG\"> " << Propagate->GetLongitudeDeg() << " </longitude>" << endl;
outfile << " <latitude unit=\"DEG\"> " << Propagate->GetLatitudeDeg() << " </latitude>" << endl;
outfile << " <altitude unit=\"FT\"> " << Propagate->GetDistanceAGL() << " </altitude>" << endl;
void FGInitialCondition::SetVequivalentKtsIC(double ve)
{
double altitudeASL = position.GetRadius() - sea_level_radius;
- double rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL);
- double rhoSL = fdmex->GetAtmosphere()->GetDensitySL();
+ double rho = Atmosphere->GetDensity(altitudeASL);
+ double rhoSL = Atmosphere->GetDensitySL();
SetVtrueFpsIC(ve*ktstofps*sqrt(rhoSL/rho));
lastSpeedSet = setve;
}
void FGInitialCondition::SetMachIC(double mach)
{
double altitudeASL = position.GetRadius() - sea_level_radius;
- double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL);
+ double temperature = Atmosphere->GetTemperature(altitudeASL);
double soundSpeed = sqrt(SHRatio*Reng*temperature);
SetVtrueFpsIC(mach*soundSpeed);
lastSpeedSet = setmach;
void FGInitialCondition::SetVcalibratedKtsIC(double vcas)
{
double altitudeASL = position.GetRadius() - sea_level_radius;
- double mach = getMachFromVcas(fabs(vcas)*ktstofps);
- double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL);
+ double pressure = Atmosphere->GetPressure(altitudeASL);
+ double pressureSL = Atmosphere->GetPressureSL();
+ double rhoSL = Atmosphere->GetDensitySL();
+ double mach = MachFromVcalibrated(fabs(vcas)*ktstofps, pressure, pressureSL, rhoSL);
+ double temperature = Atmosphere->GetTemperature(altitudeASL);
double soundSpeed = sqrt(SHRatio*Reng*temperature);
SetVtrueFpsIC(mach*soundSpeed);
void FGInitialCondition::calcAeroAngles(const FGColumnVector3& _vt_NED)
{
+ const FGMatrix33& Tl2b = orientation.GetT();
FGColumnVector3 _vt_BODY = Tl2b * _vt_NED;
double ua = _vt_BODY(eX);
double va = _vt_BODY(eY);
void FGInitialCondition::SetVgroundFpsIC(double vg)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
- vUVW_NED(eU) = vg*cos(psi);
- vUVW_NED(eV) = vg*sin(psi);
+ vUVW_NED(eU) = vg * orientation.GetCosEuler(ePsi);
+ vUVW_NED(eV) = vg * orientation.GetSinEuler(ePsi);
vUVW_NED(eW) = 0.;
_vt_NED = vUVW_NED + _vWIND_NED;
vt = _vt_NED.Magnitude();
void FGInitialCondition::SetVtrueFpsIC(double vtrue)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
return;
}
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _WIND_NED = _vt_NED - vUVW_NED;
double hdot0 = -_vt_NED(eW);
void FGInitialCondition::SetAlphaRadIC(double alfa)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
calcThetaBeta(alfa, _vt_NED);
}
void FGInitialCondition::calcThetaBeta(double alfa, const FGColumnVector3& _vt_NED)
{
+ FGColumnVector3 vOrient = orientation.GetEuler();
double calpha = cos(alfa), salpha = sin(alfa);
- double cpsi = cos(psi), spsi = sin(psi);
- double cphi = cos(phi), sphi = sin(phi);
+ double cpsi = orientation.GetCosEuler(ePsi), spsi = orientation.GetSinEuler(ePsi);
+ double cphi = orientation.GetCosEuler(ePhi), sphi = orientation.GetSinEuler(ePhi);
FGMatrix33 Tpsi( cpsi, spsi, 0.,
-spsi, cpsi, 0.,
0., 0., 1.);
v0xz.Normalize();
v1xz.Normalize();
double sinTheta = (v1xz * v0xz)(eY);
- theta = asin(sinTheta);
+ vOrient(eTht) = asin(sinTheta);
- FGQuaternion Quat(phi, theta, psi);
- Quat.Normalize();
- Tl2b = Quat.GetT();
- Tb2l = Quat.GetTInv();
+ orientation = FGQuaternion(vOrient);
+ const FGMatrix33& Tl2b = orientation.GetT();
FGColumnVector3 v2 = Talpha * Tl2b * _vt_NED;
alpha = alfa;
void FGInitialCondition::SetBetaRadIC(double bta)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
+ FGColumnVector3 vOrient = orientation.GetEuler();
beta = bta;
double calpha = cos(alpha), salpha = sin(alpha);
double cbeta = cos(beta), sbeta = sin(beta);
+ double cphi = orientation.GetCosEuler(ePhi), sphi = orientation.GetSinEuler(ePhi);
+ FGMatrix33 TphiInv(1., 0., 0.,
+ 0., cphi,-sphi,
+ 0., sphi, cphi);
Tw2b = FGMatrix33(calpha*cbeta, -calpha*sbeta, -salpha,
sbeta, cbeta, 0.0,
salpha*cbeta, -salpha*sbeta, calpha);
Tb2w = Tw2b.Transposed();
- FGColumnVector3 vf = FGQuaternion(eX, phi).GetTInv() * Tw2b * FGColumnVector3(vt, 0., 0.);
+ FGColumnVector3 vf = TphiInv * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 v0xy(_vt_NED(eX), _vt_NED(eY), 0.);
FGColumnVector3 v1xy(sqrt(v0xy(eX)*v0xy(eX)+v0xy(eY)*v0xy(eY)-vf(eY)*vf(eY)),vf(eY),0.);
v0xy.Normalize();
double sinPsi = (v1xy * v0xy)(eZ);
double cosPsi = DotProduct(v0xy, v1xy);
- psi = atan2(sinPsi, cosPsi);
+ vOrient(ePsi) = atan2(sinPsi, cosPsi);
FGMatrix33 Tpsi( cosPsi, sinPsi, 0.,
-sinPsi, cosPsi, 0.,
0., 0., 1.);
v2xz.Normalize();
vfxz.Normalize();
double sinTheta = (v2xz * vfxz)(eY);
- theta = -asin(sinTheta);
+ vOrient(eTht) = -asin(sinTheta);
- FGQuaternion Quat(phi, theta, psi);
- Quat.Normalize();
- Tl2b = Quat.GetT();
- Tb2l = Quat.GetTInv();
+ orientation = FGQuaternion(vOrient);
}
//******************************************************************************
-// Modifies the body frame orientation (roll angle phi). The true airspeed in
-// the local NED frame is kept unchanged. Hence the true airspeed in the body
-// frame is modified.
+// Modifies the body frame orientation.
-void FGInitialCondition::SetPhiRadIC(double fi)
+void FGInitialCondition::SetEulerAngleRadIC(int idx, double angle)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
+ const FGMatrix33& Tl2b = orientation.GetT();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
+ FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
+ FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
+ FGColumnVector3 vOrient = orientation.GetEuler();
- phi = fi;
- FGQuaternion Quat = FGQuaternion(phi, theta, psi);
- Quat.Normalize();
- Tl2b = Quat.GetT();
- Tb2l = Quat.GetTInv();
-
- calcAeroAngles(_vt_NED);
-}
-
-//******************************************************************************
-// Modifies the body frame orientation (pitch angle theta). The true airspeed in
-// the local NED frame is kept unchanged. Hence the true airspeed in the body
-// frame is modified.
-
-void FGInitialCondition::SetThetaRadIC(double teta)
-{
- FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
-
- theta = teta;
- FGQuaternion Quat = FGQuaternion(phi, theta, psi);
- Quat.Normalize();
- Tl2b = Quat.GetT();
- Tb2l = Quat.GetTInv();
-
- calcAeroAngles(_vt_NED);
-}
-
-//******************************************************************************
-// Modifies the body frame orientation (yaw angle psi). The true airspeed in
-// the local NED frame is kept unchanged. Hence the true airspeed in the body
-// frame is modified.
-
-void FGInitialCondition::SetPsiRadIC(double psy)
-{
- FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
+ vOrient(idx) = angle;
+ orientation = FGQuaternion(vOrient);
- psi = psy;
- FGQuaternion Quat = FGQuaternion(phi, theta, psi);
- Quat.Normalize();
- Tl2b = Quat.GetT();
- Tb2l = Quat.GetTInv();
+ if ((lastSpeedSet != setned) && (lastSpeedSet != setvg)) {
+ const FGMatrix33& newTb2l = orientation.GetTInv();
+ vUVW_NED = newTb2l * _vUVW_BODY;
+ _vt_NED = vUVW_NED + _vWIND_NED;
+ vt = _vt_NED.Magnitude();
+ }
calcAeroAngles(_vt_NED);
}
void FGInitialCondition::SetBodyVelFpsIC(int idx, double vel)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
+ const FGMatrix33& Tl2b = orientation.GetT();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
void FGInitialCondition::SetNEDVelFpsIC(int idx, double vel)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
void FGInitialCondition::SetCrossWindKtsIC(double cross)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
- FGColumnVector3 _vCROSS(-sin(psi), cos(psi), 0.);
+ FGColumnVector3 _vCROSS(-orientation.GetSinEuler(ePsi), orientation.GetCosEuler(ePsi), 0.);
// Gram-Schmidt process is used to remove the existing cross wind component
_vWIND_NED -= DotProduct(_vWIND_NED, _vCROSS) * _vCROSS;
void FGInitialCondition::SetHeadWindKtsIC(double head)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
// This is a head wind, so the direction vector for the wind
// needs to be set opposite to the heading the aircraft
// is taking. So, the cos and sin of the heading (psi)
// are negated in the line below.
- FGColumnVector3 _vHEAD(-cos(psi), -sin(psi), 0.);
+ FGColumnVector3 _vHEAD(-orientation.GetCosEuler(ePsi), -orientation.GetSinEuler(ePsi), 0.);
// Gram-Schmidt process is used to remove the existing head wind component
_vWIND_NED -= DotProduct(_vWIND_NED, _vHEAD) * _vHEAD;
void FGInitialCondition::SetWindDownKtsIC(double wD)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
_vt_NED(eW) = vUVW_NED(eW) + wD;
void FGInitialCondition::SetWindMagKtsIC(double mag)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
FGColumnVector3 _vHEAD(_vWIND_NED(eU), _vWIND_NED(eV), 0.);
void FGInitialCondition::SetWindDirDegIC(double dir)
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
double mag = _vWIND_NED.Magnitude(eU, eV);
void FGInitialCondition::SetAltitudeASLFtIC(double alt)
{
double altitudeASL = position.GetRadius() - sea_level_radius;
- double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL);
+ double temperature = Atmosphere->GetTemperature(altitudeASL);
+ double pressure = Atmosphere->GetPressure(altitudeASL);
+ double pressureSL = Atmosphere->GetPressureSL();
double soundSpeed = sqrt(SHRatio*Reng*temperature);
- double rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL);
- double rhoSL = fdmex->GetAtmosphere()->GetDensitySL();
+ double rho = Atmosphere->GetDensity(altitudeASL);
+ double rhoSL = Atmosphere->GetDensitySL();
double mach0 = vt / soundSpeed;
- double vc0 = calcVcas(mach0);
+ double vc0 = VcalibratedFromMach(mach0, pressure, pressureSL, rhoSL);
double ve0 = vt * sqrt(rho/rhoSL);
altitudeASL=alt;
position.SetRadius(alt + sea_level_radius);
- temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL);
+ temperature = Atmosphere->GetTemperature(altitudeASL);
soundSpeed = sqrt(SHRatio*Reng*temperature);
- rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL);
+ rho = Atmosphere->GetDensity(altitudeASL);
+ pressure = Atmosphere->GetPressure(altitudeASL);
switch(lastSpeedSet) {
case setvc:
- mach0 = getMachFromVcas(vc0);
+ mach0 = MachFromVcalibrated(vc0, pressure, pressureSL, rhoSL);
+ SetVtrueFpsIC(mach0 * soundSpeed);
+ break;
case setmach:
SetVtrueFpsIC(mach0 * soundSpeed);
break;
}
}
-//******************************************************************************
-// Calculate the VCAS. Uses the Rayleigh formula for supersonic speeds
-// (See "Introduction to Aerodynamics of a Compressible Fluid - H.W. Liepmann,
-// A.E. Puckett - Wiley & sons (1947)" ยง5.4 pp 75-80)
-
-double FGInitialCondition::calcVcas(double Mach) const
-{
- double altitudeASL = position.GetRadius() - sea_level_radius;
- double p=fdmex->GetAtmosphere()->GetPressure(altitudeASL);
- double psl=fdmex->GetAtmosphere()->GetPressureSL();
- double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
- double pt,A,vcas;
-
- if (Mach < 0) Mach=0;
- if (Mach < 1) //calculate total pressure assuming isentropic flow
- pt=p*pow((1 + 0.2*Mach*Mach),3.5);
- else {
- // shock in front of pitot tube, we'll assume its normal and use
- // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
- // pressure behind the shock to the static pressure in front of
- // the normal shock assumption should not be a bad one -- most supersonic
- // aircraft place the pitot probe out front so that it is the forward
- // most point on the aircraft. The real shock would, of course, take
- // on something like the shape of a rounded-off cone but, here again,
- // the assumption should be good since the opening of the pitot probe
- // is very small and, therefore, the effects of the shock curvature
- // should be small as well. AFAIK, this approach is fairly well accepted
- // within the aerospace community
-
- // The denominator below is zero for Mach ~ 0.38, for which
- // we'll never be here, so we're safe
-
- pt = p*166.92158*pow(Mach,7.0)/pow(7*Mach*Mach-1,2.5);
- }
-
- A = pow(((pt-p)/psl+1),0.28571);
- vcas = sqrt(7*psl/rhosl*(A-1));
- //cout << "calcVcas: vcas= " << vcas*fpstokts << " mach= " << Mach << " pressure: " << pt << endl;
- return vcas;
-}
-
-//******************************************************************************
-// Reverse the VCAS formula to obtain the corresponding Mach number. For subsonic
-// speeds, the reversed formula has a closed form. For supersonic speeds, the
-// formula is reversed by the Newton-Raphson algorithm.
-
-double FGInitialCondition::getMachFromVcas(double vcas)
-{
- double altitudeASL = position.GetRadius() - sea_level_radius;
- double p=fdmex->GetAtmosphere()->GetPressure(altitudeASL);
- double psl=fdmex->GetAtmosphere()->GetPressureSL();
- double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
-
- double pt = p + psl*(pow(1+vcas*vcas*rhosl/(7.0*psl),3.5)-1);
-
- if (pt/p < 1.89293)
- return sqrt(5.0*(pow(pt/p, 0.2857143) -1)); // Mach < 1
- else {
- // Mach >= 1
- double mach = sqrt(0.77666*pt/p); // Initial guess is based on a quadratic approximation of the Rayleigh formula
- double delta = 1.;
- double target = pt/(166.92158*p);
- int iter = 0;
-
- // Find the root with Newton-Raphson. Since the differential is never zero,
- // the function is monotonic and has only one root with a multiplicity of one.
- // Convergence is certain.
- while (delta > 1E-5 && iter < 10) {
- double m2 = mach*mach; // Mach^2
- double m6 = m2*m2*m2; // Mach^6
- delta = mach*m6/pow(7.0*m2-1.0,2.5) - target;
- double diff = 7.0*m6*(2.0*m2-1)/pow(7.0*m2-1.0,3.5); // Never zero when Mach >= 1
- mach -= delta/diff;
- iter++;
- }
-
- return mach;
- }
-}
-
//******************************************************************************
double FGInitialCondition::GetWindDirDegIC(void) const
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
double FGInitialCondition::GetNEDWindFpsIC(int idx) const
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
double FGInitialCondition::GetWindFpsIC(void) const
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
double FGInitialCondition::GetBodyWindFpsIC(int idx) const
{
+ const FGMatrix33& Tl2b = orientation.GetT();
FGColumnVector3 _vt_BODY = Tw2b * FGColumnVector3(vt, 0., 0.);
FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
FGColumnVector3 _vWIND_BODY = _vt_BODY - _vUVW_BODY;
double FGInitialCondition::GetVcalibratedKtsIC(void) const
{
double altitudeASL = position.GetRadius() - sea_level_radius;
- double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL);
+ double temperature = Atmosphere->GetTemperature(altitudeASL);
+ double pressure = Atmosphere->GetPressure(altitudeASL);
+ double pressureSL = Atmosphere->GetPressureSL();
+ double rhoSL = Atmosphere->GetDensitySL();
double soundSpeed = sqrt(SHRatio*Reng*temperature);
double mach = vt / soundSpeed;
- return fpstokts * calcVcas(mach);
+ return fpstokts * VcalibratedFromMach(mach, pressure, pressureSL, rhoSL);
}
//******************************************************************************
double FGInitialCondition::GetVequivalentKtsIC(void) const
{
double altitudeASL = position.GetRadius() - sea_level_radius;
- double rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL);
- double rhoSL = fdmex->GetAtmosphere()->GetDensitySL();
+ double rho = Atmosphere->GetDensity(altitudeASL);
+ double rhoSL = Atmosphere->GetDensitySL();
return fpstokts * vt * sqrt(rho/rhoSL);
}
double FGInitialCondition::GetMachIC(void) const
{
double altitudeASL = position.GetRadius() - sea_level_radius;
- double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL);
+ double temperature = Atmosphere->GetTemperature(altitudeASL);
double soundSpeed = sqrt(SHRatio*Reng*temperature);
return vt / soundSpeed;
}
double FGInitialCondition::GetBodyVelFpsIC(int idx) const
{
+ const FGMatrix33& Tl2b = orientation.GetT();
FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
return _vUVW_BODY(idx);
result = Load_v1();
}
+ fdmex->RunIC();
+
// Check to see if any engines are specified to be initialized in a running state
FGPropulsion* propulsion = fdmex->GetPropulsion();
Element* running_elements = document->FindElement("running");
running_elements = document->FindNextElement("running");
}
- fdmex->RunIC();
- fdmex->GetPropagate()->DumpState();
-
return result;
}
else if (document->FindElement("altitudeMSL")) // This is feet above sea level
position.SetRadius(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT") + sea_level_radius);
+ FGColumnVector3 vOrient = orientation.GetEuler();
+
if (document->FindElement("phi"))
- phi = document->FindElementValueAsNumberConvertTo("phi", "RAD");
+ vOrient(ePhi) = document->FindElementValueAsNumberConvertTo("phi", "RAD");
if (document->FindElement("theta"))
- theta = document->FindElementValueAsNumberConvertTo("theta", "RAD");
+ vOrient(eTht) = document->FindElementValueAsNumberConvertTo("theta", "RAD");
if (document->FindElement("psi"))
- psi = document->FindElementValueAsNumberConvertTo("psi", "RAD");
+ vOrient(ePsi) = document->FindElementValueAsNumberConvertTo("psi", "RAD");
- FGQuaternion Quat(phi, theta, psi);
- Quat.Normalize();
- Tl2b = Quat.GetT();
- Tb2l = Quat.GetTInv();
+ orientation = FGQuaternion(vOrient);
if (document->FindElement("ubody"))
SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo("ubody", "FT/SEC"));
-radInv*vUVW_NED(eNorth),
-radInv*vUVW_NED(eEast)*position.GetTanLatitude() );
- p = vOmegaLocal(eP);
- q = vOmegaLocal(eR);
- r = vOmegaLocal(eQ);
+ vPQR_body = vOmegaLocal;
return result;
}
// ToDo: Do we need to deal with normalization of the quaternions here?
Element* orientation_el = document->FindElement("orientation");
- FGQuaternion QuatLocal2Body;
if (orientation_el) {
string frame = orientation_el->GetAttributeValue("frame");
frame = to_lower(frame);
QuatI2Body.Normalize();
FGQuaternion QuatLocal2I = position.GetTl2i();
QuatLocal2I.Normalize();
- QuatLocal2Body = QuatLocal2I * QuatI2Body;
+ orientation = QuatLocal2I * QuatI2Body;
} else if (frame == "ecef") {
QuatEC2Body.Normalize();
FGQuaternion QuatLocal2EC = position.GetTl2ec(); // Get Q_e/l from matrix
QuatLocal2EC.Normalize();
- QuatLocal2Body = QuatLocal2EC * QuatEC2Body; // Q_b/l = Q_e/l * Q_b/e
+ orientation = QuatLocal2EC * QuatEC2Body; // Q_b/l = Q_e/l * Q_b/e
} else if (frame == "local") {
- QuatLocal2Body = FGQuaternion(vOrient);
+ orientation = FGQuaternion(vOrient);
} else {
}
}
- QuatLocal2Body.Normalize();
- phi = QuatLocal2Body.GetEuler(ePhi);
- theta = QuatLocal2Body.GetEuler(eTht);
- psi = QuatLocal2Body.GetEuler(ePsi);
- Tl2b = QuatLocal2Body.GetT();
- Tb2l = QuatLocal2Body.GetTInv();
-
// Initialize vehicle velocity
// Allowable frames
// - ECI (Earth Centered Inertial)
Element* velocity_el = document->FindElement("velocity");
FGColumnVector3 vInitVelocity = FGColumnVector3(0.0, 0.0, 0.0);
FGMatrix33 mTec2l = position.GetTec2l();
+ const FGMatrix33& Tb2l = orientation.GetTInv();
+
if (velocity_el) {
string frame = velocity_el->GetAttributeValue("frame");
// - ECEF (Earth Centered, Earth Fixed)
// - Body
- FGColumnVector3 vLocalRate;
Element* attrate_el = document->FindElement("attitude_rate");
+ const FGMatrix33& Tl2b = orientation.GetT();
// Refer to Stevens and Lewis, 1.5-14a, pg. 49.
// This is the rotation rate of the "Local" frame, expressed in the local frame.
FGColumnVector3 vAttRate = attrate_el->FindElementTripletConvertTo("RAD/SEC");
if (frame == "eci") {
- vLocalRate = Tl2b * position.GetTi2l() * (vAttRate - vOmegaEarth);
+ vPQR_body = Tl2b * position.GetTi2l() * (vAttRate - vOmegaEarth);
} else if (frame == "ecef") {
- vLocalRate = Tl2b * position.GetTec2l() * vAttRate;
+ vPQR_body = Tl2b * position.GetTec2l() * vAttRate;
} else if (frame == "local") {
- vLocalRate = vAttRate + vOmegaLocal;
+ vPQR_body = vAttRate + vOmegaLocal;
} else if (!frame.empty()) { // misspelling of frame
cerr << endl << fgred << " Attitude rate frame type: \"" << frame
result = false;
} else if (frame.empty()) {
- vLocalRate = vOmegaLocal;
+ vPQR_body = vOmegaLocal;
}
} else { // Body frame attitude rate assumed 0 relative to local.
- vLocalRate = vOmegaLocal;
+ vPQR_body = vOmegaLocal;
}
- p = vLocalRate(eP);
- q = vLocalRate(eQ);
- r = vLocalRate(eR);
-
return result;
}
&FGInitialCondition::SetPhiDegIC,
true);
PropertyManager->Tie("ic/psi-true-deg", this,
- &FGInitialCondition::GetPsiDegIC );
+ &FGInitialCondition::GetPsiDegIC,
+ &FGInitialCondition::SetPsiDegIC,
+ true);
PropertyManager->Tie("ic/lat-gc-deg", this,
&FGInitialCondition::GetLatitudeDegIC,
&FGInitialCondition::SetLatitudeDegIC,
&FGInitialCondition::SetPhiRadIC,
true);
PropertyManager->Tie("ic/psi-true-rad", this,
- &FGInitialCondition::GetPsiRadIC);
+ &FGInitialCondition::GetPsiRadIC,
+ &FGInitialCondition::SetPsiRadIC,
+ true);
PropertyManager->Tie("ic/lat-gc-rad", this,
&FGInitialCondition::GetLatitudeRadIC,
&FGInitialCondition::SetLatitudeRadIC,
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_INITIALCONDITION "$Id: FGInitialCondition.h,v 1.28 2011/07/10 19:03:49 jberndt Exp $"
+#define ID_INITIALCONDITION "$Id: FGInitialCondition.h,v 1.31 2011/10/23 15:05:32 bcoconni Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
class FGFDMExec;
class FGMatrix33;
class FGColumnVector3;
+class FGAtmosphere;
typedef enum { setvt, setvc, setve, setmach, setuvw, setned, setvg } speedset;
@property ic/r-rad_sec (read/write) Yaw rate initial condition in radians/second
@author Tony Peden
- @version "$Id: FGInitialCondition.h,v 1.28 2011/07/10 19:03:49 jberndt Exp $"
+ @version "$Id: FGInitialCondition.h,v 1.31 2011/10/23 15:05:32 bcoconni Exp $"
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/** Gets the initial pitch angle.
@return Initial pitch angle in degrees */
- double GetThetaDegIC(void) const { return theta*radtodeg; }
+ double GetThetaDegIC(void) const { return orientation.GetEulerDeg(eTht); }
/** Gets the initial roll angle.
@return Initial phi in degrees */
- double GetPhiDegIC(void) const { return phi*radtodeg; }
+ double GetPhiDegIC(void) const { return orientation.GetEulerDeg(ePhi); }
/** Gets the initial heading angle.
@return Initial psi in degrees */
- double GetPsiDegIC(void) const { return psi*radtodeg; }
+ double GetPsiDegIC(void) const { return orientation.GetEulerDeg(ePsi); }
/** Gets the initial latitude.
@return Initial geocentric latitude in degrees */
@param vd Initial down velocity in feet/second */
void SetVDownFpsIC(double vd) { SetNEDVelFpsIC(eW, vd); }
- /** Sets the initial roll rate.
+ /** Sets the initial body axis roll rate.
@param P Initial roll rate in radians/second */
- void SetPRadpsIC(double P) { p = P; }
+ void SetPRadpsIC(double P) { vPQR_body(eP) = P; }
- /** Sets the initial pitch rate.
+ /** Sets the initial body axis pitch rate.
@param Q Initial pitch rate in radians/second */
- void SetQRadpsIC(double Q) { q = Q; }
+ void SetQRadpsIC(double Q) { vPQR_body(eQ) = Q; }
- /** Sets the initial yaw rate.
+ /** Sets the initial body axis yaw rate.
@param R initial yaw rate in radians/second */
- void SetRRadpsIC(double R) { r = R; }
+ void SetRRadpsIC(double R) { vPQR_body(eR) = R; }
/** Sets the initial wind velocity.
@param wN Initial wind velocity in local north direction, feet/second
@return Initial body axis Z wind velocity in feet/second */
double GetWindWFpsIC(void) const { return GetBodyWindFpsIC(eW); }
+ /** Gets the initial wind velocity in the NED local frame
+ @return Initial wind velocity in NED frame in feet/second */
+ const FGColumnVector3 GetWindNEDFpsIC(void) const {
+ const FGMatrix33& Tb2l = orientation.GetTInv();
+ FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
+ return _vt_NED - vUVW_NED;
+ }
+
/** Gets the initial wind velocity in local frame.
@return Initial wind velocity toward north in feet/second */
double GetWindNFpsIC(void) const { return GetNEDWindFpsIC(eX); }
@return Initial rate of climb in feet/second */
double GetClimbRateFpsIC(void) const
{
+ const FGMatrix33& Tb2l = orientation.GetTInv();
FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
return _vt_NED(eW);
}
+ /** Gets the initial body velocity
+ @return Initial body velocity in feet/second. */
+ const FGColumnVector3 GetUVWFpsIC(void) const {
+ const FGMatrix33& Tl2b = orientation.GetT();
+ return Tl2b * vUVW_NED;
+ }
+
/** Gets the initial body axis X velocity.
@return Initial body axis X velocity in feet/second. */
double GetUBodyFpsIC(void) const { return GetBodyVelFpsIC(eU); }
@return Initial local frame Z (Down) axis velocity in feet/second. */
double GetVDownFpsIC(void) const { return vUVW_NED(eW); }
+ /** Gets the initial body rotation rate
+ @return Initial body rotation rate in radians/second */
+ const FGColumnVector3 GetPQRRadpsIC(void) const { return vPQR_body; }
+
/** Gets the initial body axis roll rate.
@return Initial body axis roll rate in radians/second */
- double GetPRadpsIC() const { return p; }
+ double GetPRadpsIC() const { return vPQR_body(eP); }
/** Gets the initial body axis pitch rate.
@return Initial body axis pitch rate in radians/second */
- double GetQRadpsIC() const { return q; }
+ double GetQRadpsIC() const { return vPQR_body(eQ); }
/** Gets the initial body axis yaw rate.
@return Initial body axis yaw rate in radians/second */
- double GetRRadpsIC() const { return r; }
+ double GetRRadpsIC() const { return vPQR_body(eR); }
/** Sets the initial flight path angle.
@param gamma Initial flight path angle in radians */
@param alpha Initial angle of attack in radians */
void SetAlphaRadIC(double alpha);
- /** Sets the initial pitch angle.
- @param theta Initial pitch angle in radians */
- void SetThetaRadIC(double theta);
-
/** Sets the initial sideslip angle.
@param beta Initial angle of sideslip in radians. */
void SetBetaRadIC(double beta);
/** Sets the initial roll angle.
@param phi Initial roll angle in radians */
- void SetPhiRadIC(double phi);
+ void SetPhiRadIC(double phi) { SetEulerAngleRadIC(ePhi, phi); }
+
+ /** Sets the initial pitch angle.
+ @param theta Initial pitch angle in radians */
+ void SetThetaRadIC(double theta) { SetEulerAngleRadIC(eTht, theta); }
/** Sets the initial heading angle.
@param psi Initial heading angle in radians */
- void SetPsiRadIC(double psi);
+ void SetPsiRadIC(double psi) { SetEulerAngleRadIC(ePsi, psi); }
/** Sets the initial latitude.
@param lat Initial latitude in radians */
@return Initial sideslip angle in radians */
double GetBetaRadIC(void) const { return beta; }
- /** Gets the initial roll angle.
- @return Initial roll angle in radians */
- double GetPhiRadIC(void) const { return phi; }
+ /** Gets the initial position
+ @return Initial location */
+ const FGLocation& GetPosition(void) const { return position; }
/** Gets the initial latitude.
@return Initial latitude in radians */
@return Initial longitude in radians */
double GetLongitudeRadIC(void) const { return position.GetLongitude(); }
+ /** Gets the initial orientation
+ @return Initial orientation */
+ const FGQuaternion& GetOrientation(void) const { return orientation; }
+
+ /** Gets the initial roll angle.
+ @return Initial roll angle in radians */
+ double GetPhiRadIC(void) const { return orientation.GetEuler(ePhi); }
+
/** Gets the initial pitch angle.
@return Initial pitch angle in radians */
- double GetThetaRadIC(void) const { return theta; }
+ double GetThetaRadIC(void) const { return orientation.GetEuler(eTht); }
/** Gets the initial heading angle.
@return Initial heading angle in radians */
- double GetPsiRadIC(void) const { return psi; }
+ double GetPsiRadIC(void) const { return orientation.GetEuler(ePsi); }
/** Gets the initial speedset.
@return Initial speedset */
private:
FGColumnVector3 vUVW_NED;
+ FGColumnVector3 vPQR_body;
FGLocation position;
+ FGQuaternion orientation;
double vt;
- double p,q,r;
double sea_level_radius;
double terrain_elevation;
double targetNlfIC;
FGMatrix33 Tw2b, Tb2w;
- FGMatrix33 Tl2b, Tb2l;
- double alpha, beta, theta, phi, psi;
+ double alpha, beta;
speedset lastSpeedSet;
FGFDMExec *fdmex;
FGPropertyManager *PropertyManager;
+ FGAtmosphere* Atmosphere;
bool Load_v1(void);
bool Load_v2(void);
bool Constructing;
void InitializeIC(void);
+ void SetEulerAngleRadIC(int idx, double angle);
void SetBodyVelFpsIC(int idx, double vel);
void SetNEDVelFpsIC(int idx, double vel);
double GetBodyWindFpsIC(int idx) const;
double GetNEDWindFpsIC(int idx) const;
double GetBodyVelFpsIC(int idx) const;
- double getMachFromVcas(double vcas);
- double calcVcas(double Mach) const;
void calcAeroAngles(const FGColumnVector3& _vt_BODY);
void calcThetaBeta(double alfa, const FGColumnVector3& _vt_NED);
void bind(void);
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-FGGroundCallback::FGGroundCallback()
+FGDefaultGroundCallback::FGDefaultGroundCallback(double referenceRadius)
{
- mReferenceRadius = 20925650.0; // Sea level radius
+ mSeaLevelRadius = referenceRadius; // Sea level radius
+ mTerrainLevelRadius = mSeaLevelRadius;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-FGGroundCallback::FGGroundCallback(double ReferenceRadius)
+double FGDefaultGroundCallback::GetAltitude(const FGLocation& loc) const
{
- mReferenceRadius = ReferenceRadius;
+ return loc.GetRadius() - mSeaLevelRadius;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-FGGroundCallback::~FGGroundCallback()
-{
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-double FGGroundCallback::GetAltitude(const FGLocation& loc) const
-{
- return loc.GetRadius() - mReferenceRadius;
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-double FGGroundCallback::GetAGLevel(double t, const FGLocation& loc,
+double FGDefaultGroundCallback::GetAGLevel(double t, const FGLocation& loc,
FGLocation& contact, FGColumnVector3& normal,
FGColumnVector3& vel, FGColumnVector3& angularVel) const
{
normal = FGColumnVector3(loc).Normalize();
double loc_radius = loc.GetRadius(); // Get the radius of the given location
// (e.g. the CG)
- double agl = loc_radius - mReferenceRadius;
- contact = (mReferenceRadius/loc_radius)*FGColumnVector3(loc);
+ double agl = loc_radius - mTerrainLevelRadius;
+ contact = (mTerrainLevelRadius/loc_radius)*FGColumnVector3(loc);
return agl;
}
-}
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+} // namespace JSBSim
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_GROUNDCALLBACK "$Id: FGGroundCallback.h,v 1.9 2010/10/07 03:45:40 jberndt Exp $"
+#define ID_GROUNDCALLBACK "$Id: FGGroundCallback.h,v 1.12 2011/10/14 22:46:49 bcoconni Exp $"
namespace JSBSim {
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-/** This class provides callback slots to get ground specific data like
- ground elevation and such.
- There is a default implementation, which returns values for a
- ball formed earth.
+/** This class provides callback slots to get ground specific data.
+
+ The default implementation returns values for a
+ ball formed earth with an adjustable terrain elevation.
@author Mathias Froehlich
- @version $Id: FGGroundCallback.h,v 1.9 2010/10/07 03:45:40 jberndt Exp $
+ @version $Id: FGGroundCallback.h,v 1.12 2011/10/14 22:46:49 bcoconni Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
class FGGroundCallback : public FGJSBBase
{
public:
- /** Default constructor.
- Within this constructor, the reference radius is set to the WGS84 equatorial
- radius. This constructor should really not be called, instead relying on the
- constructor that takes reference radius as an argument. */
- FGGroundCallback();
-
- /** Constructor
- This constructor accepts the reference radius in feet. This is the preferred
- constructor. */
- FGGroundCallback(double ReferenceRadius);
- virtual ~FGGroundCallback();
-
- /** Compute the altitude above sealevel. */
- virtual double GetAltitude(const FGLocation& l) const;
- /** Compute the altitude above ground. Defaults to sealevel altitude. */
- virtual double GetAGLevel(double t, const FGLocation& l, FGLocation& cont,
- FGColumnVector3& n, FGColumnVector3& v,
- FGColumnVector3& w) const;
- virtual void SetTerrainGeoCentRadius(double radius) {mReferenceRadius = radius;}
- virtual double GetTerrainGeoCentRadius(void) const {return mReferenceRadius;}
+
+ FGGroundCallback() {}
+ virtual ~FGGroundCallback() {}
+
+ /** Compute the altitude above sealevel
+ @param l location
+ */
+ virtual double GetAltitude(const FGLocation& l) const = 0;
+
+ /** Compute the altitude above ground.
+ The altitude depends on time t and location l.
+ @param t simulation time
+ @param l location
+ @param contact Contact point location below the location l
+ @param normal Normal vector at the contact point
+ @param v Linear velocity at the contact point
+ @param w Angular velocity at the contact point
+ @return altitude above ground
+ */
+ virtual double GetAGLevel(double t, const FGLocation& location,
+ FGLocation& contact,
+ FGColumnVector3& normal, FGColumnVector3& v,
+ FGColumnVector3& w) const = 0;
+
+ /** Compute the local terrain radius
+ @param t simulation time
+ @param location location
+ */
+ virtual double GetTerrainGeoCentRadius(double t, const FGLocation& location) const = 0;
+
+ /** Return the sea level radius
+ @param t simulation time
+ @param location location
+ */
+ virtual double GetSeaLevelRadius(const FGLocation& location) const = 0;
+
+ /** Set the local terrain radius.
+ Only needs to be implemented if JSBSim should be allowed
+ to modify the local terrain radius (see the default implementation)
+ */
+ virtual void SetTerrainGeoCentRadius(double radius) { }
+
+ /** Set the sea level radius.
+ Only needs to be implemented if JSBSim should be allowed
+ to modify the sea level radius (see the default implementation)
+ */
+ virtual void SetSeaLevelRadius(double radius) { }
+
+};
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// The default sphere earth implementation:
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+class FGDefaultGroundCallback : public FGGroundCallback
+{
+public:
+
+ FGDefaultGroundCallback(double referenceRadius = 20925650.0);
+
+ double GetAltitude(const FGLocation& l) const;
+
+ double GetAGLevel(double t, const FGLocation& location,
+ FGLocation& contact,
+ FGColumnVector3& normal, FGColumnVector3& v,
+ FGColumnVector3& w) const;
+
+ void SetTerrainGeoCentRadius(double radius) { mTerrainLevelRadius = radius;}
+ double GetTerrainGeoCentRadius(double t, const FGLocation& location) const
+ { return mTerrainLevelRadius; }
+
+ void SetSeaLevelRadius(double radius) { mSeaLevelRadius = radius; }
+ double GetSeaLevelRadius(const FGLocation& location) const
+ {return mSeaLevelRadius; }
+
private:
- /// Reference radius.
- double mReferenceRadius;
+
+ double mSeaLevelRadius;
+ double mTerrainLevelRadius;
};
+
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endif
namespace JSBSim {
-static const char *IdSrc = "$Id: FGLocation.cpp,v 1.23 2010/09/22 11:34:09 jberndt Exp $";
+static const char *IdSrc = "$Id: FGLocation.cpp,v 1.25 2011/10/16 00:19:56 bcoconni Exp $";
static const char *IdHdr = ID_LOCATION;
using std::cerr;
using std::endl;
e = l.e;
eps2 = l.eps2;
f = l.f;
+ epa = l.epa;
/*ag
* if the cache is not valid, all of the following values are unset.
mTl2ec = l.mTl2ec;
mTec2l = l.mTec2l;
+ mTi2ec = l.mTi2ec;
+ mTec2i = l.mTec2i;
+ mTi2l = l.mTi2l;
+ mTl2i = l.mTl2i;
initial_longitude = l.initial_longitude;
mGeodLat = l.mGeodLat;
e = l.e;
eps2 = l.eps2;
f = l.f;
+ epa = l.epa;
//ag See comment in constructor above
if (!mCacheValid) return *this;
mTl2ec = l.mTl2ec;
mTec2l = l.mTec2l;
+ mTi2ec = l.mTi2ec;
+ mTec2i = l.mTec2i;
+ mTi2l = l.mTi2l;
+ mTl2i = l.mTl2i;
initial_longitude = l.initial_longitude;
mGeodLat = l.mGeodLat;
namespace JSBSim {
-static const char *IdSrc = "$Id: FGAerodynamics.cpp,v 1.41 2011/08/04 12:46:32 jberndt Exp $";
+static const char *IdSrc = "$Id: FGAerodynamics.cpp,v 1.44 2011/10/23 14:23:13 jentron Exp $";
static const char *IdHdr = ID_AERODYNAMICS;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ( fabs(vFw(eDrag)) > 0.0) lod = fabs( vFw(eLift) / vFw(eDrag) );
// Calculate aerodynamic reference point shift, if any. The shift
- // takes place in the body axis. That is, if the shift is negative,
+ // takes place in the structual axis. That is, if the shift is positive,
// it is towards the back (tail) of the vehicle. The AeroRPShift
// function should be non-dimensionalized by the wing chord. The
// calculated vDeltaRP will be in feet.
if (AeroRPShift) vDeltaRP(eX) = AeroRPShift->GetValue()*in.Wingchord;
- vDXYZcg = in.RPBody + vDeltaRP;
+ vDXYZcg(eX) = in.RPBody(eX) - vDeltaRP(eX); // vDeltaRP is given in the structural frame
+ vDXYZcg(eY) = in.RPBody(eY) + vDeltaRP(eY);
+ vDXYZcg(eZ) = in.RPBody(eZ) - vDeltaRP(eZ);
vMoments = vDXYZcg*vForces; // M = r X F
for (unsigned int axis = 0; axis < 6; axis++) {
for (unsigned int sd = 0; sd < AeroFunctions[axis].size(); sd++) {
if (buf.tellp() > 0) buf << delimeter;
- buf << setw(9) << AeroFunctions[axis][sd]->GetValue();
+ buf << AeroFunctions[axis][sd]->GetValue();
}
}
namespace JSBSim {
-static const char *IdSrc = "$Id: FGOutput.cpp,v 1.62 2011/09/25 15:38:30 bcoconni Exp $";
+static const char *IdSrc = "$Id: FGOutput.cpp,v 1.63 2011/10/10 02:33:34 jentron Exp $";
static const char *IdHdr = ID_OUTPUT;
// (stolen from FGFS native_fdm.cxx)
{
if (type == "CSV") {
Type = otCSV;
- delimeter = ", ";
+ delimeter = ",";
} else if (type == "TABULAR") {
Type = otTab;
delimeter = "\t";
namespace JSBSim {
-static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.96 2011/09/17 15:36:35 bcoconni Exp $";
+static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.98 2011/10/22 15:11:24 bcoconni Exp $";
static const char *IdHdr = ID_PROPAGATE;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGPropagate::FGPropagate(FGFDMExec* fdmex)
: FGModel(fdmex),
- LocalTerrainRadius(0),
- SeaLevelRadius(0),
VehicleRadius(0)
{
Debug(0);
bool FGPropagate::InitModel(void)
{
// For initialization ONLY:
- SeaLevelRadius = LocalTerrainRadius = in.RefRadius;
- FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
+ VState.vLocation.SetRadius( FDMExec->GetGroundCallback()->
+ GetTerrainGeoCentRadius(0.0,VState.vLocation) + 4.0 );
- VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
VState.vLocation.SetEllipse(in.SemiMajor, in.SemiMinor);
vInertialVelocity.InitMatrix();
// Initialize the State Vector elements and the transformation matrices
// Set the position lat/lon/radius
- VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
- FGIC->GetLatitudeRadIC(),
- FGIC->GetAltitudeASLFtIC() + SeaLevelRadius);
-
- VState.vLocation.SetEarthPositionAngle(0.0);
+ VState.vLocation = FGIC->GetPosition();
Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
// Set the orientation from the euler angles (is normalized within the
// constructor). The Euler angles represent the orientation of the body
// frame relative to the local frame.
- VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
- FGIC->GetThetaRadIC(),
- FGIC->GetPsiRadIC() );
+ VState.qAttitudeLocal = FGIC->GetOrientation();
VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
UpdateBodyMatrices();
// Set the velocities in the instantaneus body frame
- VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
- FGIC->GetVBodyFpsIC(),
- FGIC->GetWBodyFpsIC() );
+ VState.vUVW = FGIC->GetUVWFpsIC();
// Compute the local frame ECEF velocity
vVel = Tb2l * VState.vUVW;
- // Recompute the LocalTerrainRadius.
- RecomputeLocalTerrainRadius();
-
+ // Compute local terrain velocity
+ RecomputeLocalTerrainVelocity();
VehicleRadius = GetRadius();
// Set the angular velocities of the body frame relative to the ECEF frame,
// expressed in the body frame.
- VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
- FGIC->GetQRadpsIC(),
- FGIC->GetRRadpsIC() );
+ VState.vPQR = FGIC->GetPQRRadpsIC();
VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
CalculateUVW();
// Set auxilliary state variables
- RecomputeLocalTerrainRadius();
-
+ RecomputeLocalTerrainVelocity();
VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-void FGPropagate::RecomputeLocalTerrainRadius(void)
+void FGPropagate::RecomputeLocalTerrainVelocity()
{
- FGLocation contactloc;
- FGColumnVector3 dummy;
- double t = FDMExec->GetSimTime();
-
- // Get the LocalTerrain radius.
- FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc,
- dummy, LocalTerrainVelocity, LocalTerrainAngularVelocity);
- LocalTerrainRadius = contactloc.GetRadius();
- FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
+ FGLocation contact;
+ FGColumnVector3 normal;
+ FDMExec->GetGroundCallback()->GetAGLevel(FDMExec->GetSimTime(),
+ VState.vLocation,
+ contact, normal,
+ LocalTerrainVelocity,
+ LocalTerrainAngularVelocity);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropagate::SetTerrainElevation(double terrainElev)
{
- LocalTerrainRadius = terrainElev + SeaLevelRadius;
- FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
+ double radius = terrainElev + FDMExec->GetGroundCallback()->GetSeaLevelRadius(VState.vLocation);
+ FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(radius);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::SetSeaLevelRadius(double tt)
+{
+ FDMExec->GetGroundCallback()->SetSeaLevelRadius(tt);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+double FGPropagate::GetLocalTerrainRadius(void) const
+{
+ return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius(FDMExec->GetSimTime(),
+ VState.vLocation);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGPropagate::GetTerrainElevation(void) const
{
- return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
+ return GetLocalTerrainRadius()
+ - FDMExec->GetGroundCallback()->GetSeaLevelRadius(VState.vLocation);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGPropagate::GetDistanceAGL(void) const
{
- return VState.vLocation.GetRadius() - LocalTerrainRadius;
+ FGColumnVector3 dummy;
+ FGLocation dummyloc;
+ double t = FDMExec->GetSimTime();
+ return FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, dummyloc,
+ dummy, dummy, dummy);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::SetAltitudeASL(double altASL)
+{
+ SetRadius(altASL + FDMExec->GetGroundCallback()->GetSeaLevelRadius(VState.vLocation));
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+double FGPropagate::GetAltitudeASL(void) const
+{
+ return VState.vLocation.GetRadius()
+ - FDMExec->GetGroundCallback()->GetSeaLevelRadius(VState.vLocation);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGPropagate::SetDistanceAGL(double tt)
+{
+ SetAltitudeASL(tt + GetTerrainElevation());
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Tec2i = Ti2ec.Transposed();
UpdateLocationMatrices();
SetInertialOrientation(vstate.qAttitudeECI);
- RecomputeLocalTerrainRadius();
+ RecomputeLocalTerrainVelocity();
VehicleRadius = GetRadius();
VState.vUVW = vstate.vUVW;
vVel = Tb2l * VState.vUVW;
void FGPropagate::UpdateVehicleState(void)
{
- RecomputeLocalTerrainRadius();
+ RecomputeLocalTerrainVelocity();
VehicleRadius = GetRadius();
VState.vInertialPosition = Tec2i * VState.vLocation;
UpdateLocationMatrices();
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.63 2011/08/21 15:35:39 bcoconni Exp $"
+#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.64 2011/10/14 22:46:49 bcoconni Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@endcode
@author Jon S. Berndt, Mathias Froehlich, Bertrand Coconnier
- @version $Id: FGPropagate.h,v 1.63 2011/08/21 15:35:39 bcoconni Exp $
+ @version $Id: FGPropagate.h,v 1.64 2011/10/14 22:46:49 bcoconni Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
units ft
@return The current altitude above sea level in feet.
*/
- double GetAltitudeASL(void) const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
+ double GetAltitudeASL(void) const;
/** Returns the current altitude above sea level.
This function returns the altitude above sea level.
units feet
@return distance of the local terrain from the center of the earth.
*/
- double GetLocalTerrainRadius(void) const { return LocalTerrainRadius; }
+ double GetLocalTerrainRadius(void) const;
double GetEarthPositionAngle(void) const { return VState.vLocation.GetEPA(); }
const FGColumnVector3& GetTerrainVelocity(void) const { return LocalTerrainVelocity; }
const FGColumnVector3& GetTerrainAngularVelocity(void) const { return LocalTerrainAngularVelocity; }
+ void RecomputeLocalTerrainVelocity();
+
double GetTerrainElevation(void) const;
double GetDistanceAGL(void) const;
double GetRadius(void) const {
VehicleRadius = r;
VState.vInertialPosition = Tec2i * VState.vLocation;
}
- void SetAltitudeASL(double altASL) { SetRadius(altASL + SeaLevelRadius); }
- void SetAltitudeASLmeters(double altASL) { SetRadius(altASL/fttom + SeaLevelRadius); }
- void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
+
+ void SetAltitudeASL(double altASL);
+ void SetAltitudeASLmeters(double altASL) { SetAltitudeASL(altASL/fttom); }
+
+ void SetSeaLevelRadius(double tt);
void SetTerrainElevation(double tt);
- void SetDistanceAGL(double tt) { SetRadius(tt + LocalTerrainRadius); }
+ void SetDistanceAGL(double tt);
+
void SetInitialState(const FGInitialCondition *);
void SetLocation(const FGLocation& l);
void SetLocation(const FGColumnVector3& lv)
SetLocation(l);
}
- void RecomputeLocalTerrainRadius(void);
-
void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
VState.vInertialPosition -= Tb2i*deltaLoc;
VState.vLocation -= Tb2ec*deltaLoc;
FGMatrix33 Ti2l;
FGMatrix33 Tl2i;
- double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
+ double VehicleRadius;
FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
+
eIntegrateType integrator_rotational_rate;
eIntegrateType integrator_translational_rate;
eIntegrateType integrator_rotational_position;
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_WINDS "$Id: FGWinds.h,v 1.5 2011/09/07 12:21:45 jberndt Exp $"
+#define ID_WINDS "$Id: FGWinds.h,v 1.6 2011/10/22 15:11:24 bcoconni Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
/// Sets a wind component in NED frame.
virtual void SetWindNED(int idx, double wind) { vWindNED(idx)=wind;}
+ /// Sets the wind components in NED frame.
+ virtual void SetWindNED(const FGColumnVector3& wind) { vWindNED=wind; }
+
/// Retrieves the wind components in NED frame.
virtual FGColumnVector3& GetWindNED(void) { return vWindNED; }
namespace JSBSim {
-static const char *IdSrc = "$Id: FGPiston.cpp,v 1.67 2011/09/25 23:56:11 jentron Exp $";
+static const char *IdSrc = "$Id: FGPiston.cpp,v 1.68 2011/10/11 15:13:34 jentron Exp $";
static const char *IdHdr = ID_PISTON;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
RunPreFunctions();
+ TotalDeltaT = ( in.TotalDeltaT < 1e-9 ) ? 1.0 : in.TotalDeltaT;
+
/* The thruster controls the engine RPM because it encapsulates the gear ratio and other transmission variables */
RPM = Thruster->GetEngineRPM();
// Add a one second lag to manifold pressure changes
double dMAP=0;
- if (in.TotalDeltaT > 0.0)
- dMAP = (TMAP - p_ram * map_coefficient) * in.TotalDeltaT;
- else
- dMAP = (TMAP - p_ram * map_coefficient) / 120;
+ dMAP = (TMAP - p_ram * map_coefficient) * TotalDeltaT;
TMAP -=dMAP;
} else { // Drop towards ambient - guess an appropriate time constant for now
combustion_efficiency = 0;
dEGTdt = (RankineToKelvin(in.Temperature) - ExhaustGasTemp_degK) / 100.0;
- if (in.TotalDeltaT > 0.0)
- delta_T_exhaust = dEGTdt * in.TotalDeltaT;
- else
- delta_T_exhaust = dEGTdt / 120;
+ delta_T_exhaust = dEGTdt * TotalDeltaT;
ExhaustGasTemp_degK += delta_T_exhaust;
}
double HeatCapacityCylinderHead = CpCylinderHead * MassCylinderHead;
- if (in.TotalDeltaT > 0.0)
- CylinderHeadTemp_degK +=
- (dqdt_cylinder_head / HeatCapacityCylinderHead) * in.TotalDeltaT;
- else
- CylinderHeadTemp_degK +=
- (dqdt_cylinder_head / HeatCapacityCylinderHead) / 120.0;
+ CylinderHeadTemp_degK +=
+ (dqdt_cylinder_head / HeatCapacityCylinderHead) * TotalDeltaT;
+
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double dOilTempdt = (target_oil_temp - OilTemp_degK) / time_constant;
- if (in.TotalDeltaT > 0.0)
- OilTemp_degK += (dOilTempdt * in.TotalDeltaT);
- else
- OilTemp_degK += (dOilTempdt / 120.0);
+ OilTemp_degK += (dOilTempdt * TotalDeltaT);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_PISTON "$Id: FGPiston.h,v 1.31 2011/08/04 13:45:42 jberndt Exp $";
+#define ID_PISTON "$Id: FGPiston.h,v 1.32 2011/10/11 16:16:16 jentron Exp $";
#define FG_MAX_BOOST_SPEEDS 3
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@author David Megginson (initial porting and additional code)
@author Ron Jensen (additional engine code)
@see Taylor, Charles Fayette, "The Internal Combustion Engine in Theory and Practice"
- @version $Id: FGPiston.h,v 1.31 2011/08/04 13:45:42 jberndt Exp $
+ @version $Id: FGPiston.h,v 1.32 2011/10/11 16:16:16 jentron Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
// Inputs (in addition to those in FGEngine).
//
+ double TotalDeltaT; // Time in seconds between calls.
double p_amb; // Pascals
double p_ram; // Pascals
double T_amb; // degrees Kelvin
namespace JSBSim {
-static const char *IdSrc = "$Id: FGPropeller.cpp,v 1.38 2011/09/24 14:26:46 jentron Exp $";
+static const char *IdSrc = "$Id: FGPropeller.cpp,v 1.39 2011/10/15 13:00:57 bcoconni Exp $";
static const char *IdHdr = ID_PROPELLER;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
local_element = prop_element->GetParent()->FindElement("sense");
if (local_element) {
double Sense = local_element->GetDataAsNumber();
- SetSense(fabs(Sense)/Sense);
+ SetSense(Sense >= 0.0 ? 1.0 : -1.0);
}
local_element = prop_element->GetParent()->FindElement("p_factor");
if (local_element) {
#include "FGRotor.h"
#include "input_output/FGXMLElement.h"
#include "models/FGMassBalance.h"
+#include "models/FGPropulsion.h" // to get the GearRatio from a linked rotor
using std::cerr;
using std::endl;
namespace JSBSim {
-static const char *IdSrc = "$Id: FGRotor.cpp,v 1.17 2011/09/24 14:26:46 jentron Exp $";
+static const char *IdSrc = "$Id: FGRotor.cpp,v 1.18 2011/10/15 21:30:28 jentron Exp $";
static const char *IdHdr = ID_ROTOR;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+// Note: The FGTransmission class is currently carried 'pick-a-pack' by
+// FGRotor.
+
+FGTransmission::FGTransmission(FGFDMExec *exec, int num) :
+ FreeWheelTransmission(1.0),
+ ThrusterMoment(1.0), EngineMoment(1.0), EngineFriction(0.0),
+ ClutchCtrlNorm(1.0), BrakeCtrlNorm(0.0), MaxBrakePower(0.0),
+ EngineRPM(0.0), ThrusterRPM(0.0)
+{
+ double dt;
+ PropertyManager = exec->GetPropertyManager();
+ dt = exec->GetDeltaT();
+
+ // avoid too abrupt changes in transmission
+ FreeWheelLag = Filter(200.0,dt);
+ BindModel(num);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGTransmission::~FGTransmission(){
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+// basically P = Q*w and Q_Engine + (-Q_Rotor) = J * dw/dt, J = Moment
+//
+void FGTransmission::Calculate(double EnginePower, double ThrusterTorque, double dt) {
+
+ double coupling = 1.0, coupling_sq = 1.0;
+ double fw_mult = 1.0;
+
+ double d_omega = 0.0, engine_d_omega = 0.0, thruster_d_omega = 0.0; // relative changes
+
+ double engine_omega = rpm_to_omega(EngineRPM);
+ double safe_engine_omega = engine_omega < 1e-1 ? 1e-1 : engine_omega;
+ double engine_torque = EnginePower / safe_engine_omega;
+
+ double thruster_omega = rpm_to_omega(ThrusterRPM);
+ double safe_thruster_omega = thruster_omega < 1e-1 ? 1e-1 : thruster_omega;
+
+ engine_torque -= EngineFriction / safe_engine_omega;
+ ThrusterTorque += Constrain(0.0, BrakeCtrlNorm, 1.0) * MaxBrakePower / safe_thruster_omega;
+
+ // would the FWU release ?
+ engine_d_omega = engine_torque/EngineMoment * dt;
+ thruster_d_omega = - ThrusterTorque/ThrusterMoment * dt;
+
+ if ( thruster_omega+thruster_d_omega > engine_omega+engine_d_omega ) {
+ // don't drive the engine
+ FreeWheelTransmission = 0.0;
+ } else {
+ FreeWheelTransmission = 1.0;
+ }
+
+ fw_mult = FreeWheelLag.execute(FreeWheelTransmission);
+ coupling = fw_mult * Constrain(0.0, ClutchCtrlNorm, 1.0);
+
+ if (coupling < 0.999999) { // are the separate calculations needed ?
+ // assume linear transfer
+ engine_d_omega =
+ (engine_torque - ThrusterTorque*coupling)/(ThrusterMoment*coupling + EngineMoment) * dt;
+ thruster_d_omega =
+ (engine_torque*coupling - ThrusterTorque)/(ThrusterMoment + EngineMoment*coupling) * dt;
+
+ EngineRPM += omega_to_rpm(engine_d_omega);
+ ThrusterRPM += omega_to_rpm(thruster_d_omega);
+
+ // simulate friction in clutch
+ coupling_sq = coupling*coupling;
+ EngineRPM = (1.0-coupling_sq) * EngineRPM + coupling_sq * 0.02 * (49.0*EngineRPM + ThrusterRPM);
+ ThrusterRPM = (1.0-coupling_sq) * ThrusterRPM + coupling_sq * 0.02 * (EngineRPM + 49.0*ThrusterRPM);
+
+ // enforce equal rpm
+ if ( fabs(EngineRPM-ThrusterRPM) < 1e-3 ) {
+ EngineRPM = ThrusterRPM = 0.5 * (EngineRPM+ThrusterRPM);
+ }
+ } else {
+ d_omega = (engine_torque - ThrusterTorque)/(ThrusterMoment + EngineMoment) * dt;
+ EngineRPM = ThrusterRPM += omega_to_rpm(d_omega);
+ }
+
+ // nothing will turn backward
+ if (EngineRPM < 0.0 ) EngineRPM = 0.0;
+ if (ThrusterRPM < 0.0 ) ThrusterRPM = 0.0;
+
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+bool FGTransmission::BindModel(int num)
+{
+ string property_name, base_property_name;
+ base_property_name = CreateIndexedPropertyName("propulsion/engine", num);
+
+ property_name = base_property_name + "/brake-ctrl-norm";
+ PropertyManager->Tie( property_name.c_str(), this, &FGTransmission::GetBrakeCtrl, &FGTransmission::SetBrakeCtrl);
+ property_name = base_property_name + "/free-wheel-transmission";
+ PropertyManager->Tie( property_name.c_str(), this, &FGTransmission::GetFreeWheelTransmission);
+
+ return true;
+}
+
+
+
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Constructor
rho(0.002356), // environment
Radius(0.0), BladeNum(0), // configuration parameters
Sense(1.0), NominalRPM(0.0), MinimalRPM(0.0), MaximalRPM(0.0),
- ExternalRPM(0), RPMdefinition(0), ExtRPMsource(NULL),
+ ExternalRPM(0), RPMdefinition(0), ExtRPMsource(NULL), SourceGearRatio(1.0),
BladeChord(0.0), LiftCurveSlope(0.0), BladeTwist(0.0), HingeOffset(0.0),
BladeFlappingMoment(0.0), BladeMassMoment(0.0), PolarMoment(0.0),
InflowLag(0.0), TipLossB(0.0),
theta_downwash(0.0), phi_downwash(0.0),
ControlMap(eMainCtrl), // control
CollectiveCtrl(0.0), LateralCtrl(0.0), LongitudinalCtrl(0.0),
- BrakeCtrlNorm(0.0), MaxBrakePower(0.0),
- FreeWheelPresent(0), FreeWheelThresh(0.0), // free-wheeling-unit (FWU)
- FreeWheelTransmission(0.0)
+ Transmission(NULL), // interaction with engine
+ EngineRPM(0.0), MaxBrakePower(0.0), GearLoss(0.0), GearMoment(0.0)
{
FGColumnVector3 location(0.0, 0.0, 0.0), orientation(0.0, 0.0, 0.0);
Element *thruster_element;
// ExternalRPM -- is the RPM dictated ?
if (rotor_element->FindElement("ExternalRPM")) {
ExternalRPM = 1;
+ SourceGearRatio = 1.0;
RPMdefinition = (int) rotor_element->FindElementValueAsNumber("ExternalRPM");
+ int rdef = RPMdefinition;
+ if (RPMdefinition>=0) {
+ // avoid ourself and (still) unknown engines.
+ if (!exec->GetPropulsion()->GetEngine(RPMdefinition) || RPMdefinition==num) {
+ RPMdefinition = -1;
+ } else {
+ FGThruster *tr = exec->GetPropulsion()->GetEngine(RPMdefinition)->GetThruster();
+ SourceGearRatio = tr->GetGearRatio();
+ // cout << "# got sources' GearRatio: " << SourceGearRatio << endl;
+ }
+ }
+ if (RPMdefinition != rdef) {
+ cerr << "# discarded given RPM source (" << rdef << ") and switched to external control (-1)." << endl;
+ }
}
// configure the rotor parameters
Configure(rotor_element);
+ // configure the transmission parameters
+ if (!ExternalRPM) {
+ Transmission = new FGTransmission(exec, num);
+
+ Transmission->SetMaxBrakePower(MaxBrakePower);
+
+ if (rotor_element->FindElement("gearloss")) {
+ GearLoss = rotor_element->FindElementValueAsNumberConvertTo("gearloss","HP");
+ GearLoss *= hptoftlbssec;
+ }
+
+ if (GearLoss<1e-6) { // TODO, allow 0 ?
+ double ehp = 0.5 * BladeNum*BladeChord*Radius*Radius; // guess engine power
+ //cout << "# guessed engine power: " << ehp << endl;
+ GearLoss = 0.0025 * ehp * hptoftlbssec;
+ }
+ Transmission->SetEngineFriction(GearLoss);
+
+ // The MOI sensed behind the gear ( MOI_engine*sqr(GearRatio) ).
+ if (rotor_element->FindElement("gearmoment")) {
+ GearMoment = rotor_element->FindElementValueAsNumberConvertTo("gearmoment","SLUG*FT2");
+ }
+
+ if (GearMoment<1e-2) { // TODO, need better check for lower limit
+ GearMoment = 0.1*PolarMoment;
+ }
+ Transmission->SetEngineMoment(GearMoment);
+
+ Transmission->SetThrusterMoment(PolarMoment);
+ }
+
// shaft representation - a rather simple transform,
// but using a matrix is safer.
TboToHsr.InitMatrix( 0.0, 0.0, 1.0,
// calculation would cause jumps too. 1Hz seems sufficient.
damp_hagl = Filter(1.0, dt);
- // avoid too abrupt changes in power transmission
- FreeWheelLag = Filter(200.0,dt);
-
// enable import-export
BindModel();
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGRotor::~FGRotor(){
+ if (Transmission) delete Transmission;
Debug(1);
}
GroundEffectExp = ConfigValue(rotor_element, "groundeffectexp", 0.0);
GroundEffectShift = ConfigValueConv(rotor_element, "groundeffectshift", 0.0, "FT");
- // handle optional free-wheeling-unit (FWU)
- FreeWheelPresent = 0;
- FreeWheelTransmission = 1.0;
- if (rotor_element->FindElement("freewheelthresh")) {
- FreeWheelThresh = rotor_element->FindElementValueAsNumber("freewheelthresh");
- if (FreeWheelThresh > 1.0) {
- FreeWheelPresent = 1;
- FreeWheelTransmission = 0.0;
- }
- }
-
// precalc often used powers
R[0]=1.0; R[1]=Radius; R[2]=R[1]*R[1]; R[3]=R[2]*R[1]; R[4]=R[3]*R[1];
B[0]=1.0; B[1]=TipLossB; B[2]=B[1]*B[1]; B[3]=B[2]*B[1]; B[4]=B[3]*B[1];
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-void FGRotor::CalcStatePart1(void)
+void FGRotor::CalcRotorState(void)
{
double A_IC; // lateral (roll) control in radians
double B_IC; // longitudinal (pitch) control in radians
// handle RPM requirements, calc omega.
if (ExternalRPM && ExtRPMsource) {
- RPM = ExtRPMsource->getDoubleValue() / GearRatio;
+ RPM = ExtRPMsource->getDoubleValue() * ( SourceGearRatio / GearRatio );
}
// MinimalRPM is always >= 1. MaximalRPM is always >= NominalRPM
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-void FGRotor::CalcStatePart2(double PowerAvailable)
-{
- if (! ExternalRPM) {
- // calculate new RPM
- double ExcessTorque = PowerAvailable / Omega;
- double deltaOmega = ExcessTorque / PolarMoment * in.TotalDeltaT;
- RPM += deltaOmega/(2.0*M_PI) * 60.0;
- }
- RPM = Constrain(MinimalRPM, RPM, MaximalRPM); // trim again
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-// Simulation of a free-wheeling-unit (FWU). Might need improvements.
-
-void FGRotor::calc_freewheel_state(double p_source, double p_load) {
-
- // engine is off/detached, release.
- if (p_source<1e-3) {
- FreeWheelTransmission = 0.0;
- return;
- }
-
- // engine is driving the rotor, engage.
- if (p_source >= p_load) {
- FreeWheelTransmission = 1.0;
- return;
- }
-
- // releases if engine is detached, but stays calm if
- // the load changes due to rotor dynamics.
- if (p_source > 0.0 && p_load/(p_source+0.1) > FreeWheelThresh ) {
- FreeWheelTransmission = 0.0;
- return;
- }
-
- return;
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
double FGRotor::Calculate(double EnginePower)
{
- double FWmult = 1.0;
- double DeltaPower;
- CalcStatePart1();
+ CalcRotorState();
- PowerRequired = Torque * Omega + BrakeCtrlNorm * MaxBrakePower;
+ if (! ExternalRPM) {
+ Transmission->SetClutchCtrlNorm(ClutchCtrlNorm);
- if (FreeWheelPresent) {
- calc_freewheel_state(EnginePower * ClutchCtrlNorm, PowerRequired);
- FWmult = FreeWheelLag.execute(FreeWheelTransmission);
- }
+ // the RPM values are handled inside Transmission
+ Transmission->Calculate(EnginePower, Torque, in.TotalDeltaT);
- DeltaPower = EnginePower * ClutchCtrlNorm * FWmult - PowerRequired;
+ EngineRPM = Transmission->GetEngineRPM() * GearRatio;
+ RPM = Transmission->GetThrusterRPM();
+ } else {
+ EngineRPM = RPM * GearRatio;
+ }
- CalcStatePart2(DeltaPower);
+ RPM = Constrain(MinimalRPM, RPM, MaximalRPM); // trim again
return Thrust;
}
PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetLongitudinalCtrl, &FGRotor::SetLongitudinalCtrl);
}
- property_name = base_property_name + "/brake-ctrl-norm";
- PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetBrakeCtrl, &FGRotor::SetBrakeCtrl);
- property_name = base_property_name + "/free-wheel-transmission";
- PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetFreeWheelTransmission);
-
if (ExternalRPM) {
if (RPMdefinition == -1) {
property_name = base_property_name + "/x-rpm-dict";
ExtRPMsource = PropertyManager->GetNode(property_name, true);
} else if (RPMdefinition >= 0 && RPMdefinition != EngineNum) {
string ipn = CreateIndexedPropertyName("propulsion/engine", RPMdefinition);
- property_name = ipn + "/engine-rpm";
+ property_name = ipn + "/rotor-rpm";
ExtRPMsource = PropertyManager->GetNode(property_name, false);
if (! ExtRPMsource) {
cerr << "# Warning: Engine number " << EngineNum << "." << endl;
- cerr << "# No 'engine-rpm' property found for engine " << RPMdefinition << "." << endl;
+ cerr << "# No 'rotor-rpm' property found for engine " << RPMdefinition << "." << endl;
cerr << "# Please check order of engine definitons." << endl;
}
} else {
if (RPMdefinition == -1) {
cout << " RPM is controlled externally" << endl;
} else {
- cout << " RPM source set to engine " << RPMdefinition << endl;
+ cout << " RPM source set to thruster " << RPMdefinition << endl;
}
}
cout << " Lock Number = " << LockNumberByRho * 0.002356 << " (SL)" << endl;
cout << " Solidity = " << Solidity << endl;
cout << " Max Brake Power = " << MaxBrakePower/hptoftlbssec << " HP" << endl;
+ cout << " Gear Loss = " << GearLoss/hptoftlbssec << " HP" << endl;
+ cout << " Gear Moment = " << GearMoment << endl;
switch (ControlMap) {
case eTailCtrl: ControlMapName = "Tail Rotor"; break;
}
cout << " Control Mapping = " << ControlMapName << endl;
- if (FreeWheelPresent) {
- cout << " Free Wheel Threshold = " << FreeWheelThresh << endl;
- } else {
- cout << " No FWU present" << endl;
- }
-
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_ROTOR "$Id: FGRotor.h,v 1.11 2011/09/24 14:26:46 jentron Exp $"
+#define ID_ROTOR "$Id: FGRotor.h,v 1.12 2011/10/15 21:30:28 jentron Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
<inflowlag> {number} </inflowlag>
<tiplossfactor> {number} </tiplossfactor>
<maxbrakepower unit="{POWER}"> {number} </maxbrakepower>
+ <gearloss unit="{POWER}"> {number} </gearloss>
+ <gearmoment unit="{MOMENT}"> {number} </gearmoment>
<controlmap> {MAIN|TAIL|TANDEM} </controlmap>
<ExternalRPM> {number} </ExternalRPM>
responses (typical values for main rotor: 0.1 - 0.2 s).
\<tiplossfactor> - Tip-loss factor. The Blade fraction that produces lift.
Value usually ranges between 0.95 - 1.0, optional (B).
+
\<maxbrakepower> - Rotor brake, 20-30 hp should work for a mid size helicopter.
+ \<gearloss> - Friction in gear, 0.2% to 3% of the engine power, optional (see notes).
+ \<gearmoment> - Approximation for the moment of inertia of the gear (and engine),
+ defaults to 0.1 * polarmoment, optional.
\<controlmap> - Defines the control inputs used (see notes).
+
\<ExternalRPM> - Links the rotor to another rotor, or an user controllable property.
Experimental properties
-
+
\<groundeffectexp> - Exponent for ground effect approximation. Values usually range from 0.04
for large rotors to 0.1 for smaller ones. As a rule of thumb the effect
vanishes at a height 2-3 times the rotor diameter.
Omitting or setting to 0.0 disables the effect calculation.
\<groundeffectshift> - Further adjustment of ground effect, approx. hub height or slightly above.
- \<freewheelthresh> - Ratio of thruster power to engine power. The FWU will release when above
- the threshold. The value shouldn't be too close to 1.0, 1.5 seems ok.
- 0 disables this feature, which is also the default.
-
</pre>
<h3>Notes:</h3>
<tt>propulsion/engine[x]/longitudinal-ctrl-rad</tt>.</li>
<li> The tail collective (aka antitorque, aka pedal) control input. Read from
<tt>propulsion/engine[x]/antitorque-ctrl-rad</tt> or
- <tt>propulsion/engine[x]/tail-collective-ctrl-rad</tt>.</li>
+ <tt>propulsion/engine[x]/tail-collective-ctrl-rad</tt>.</li>
</ul>
<h4>- Engine issues -</h4>
- In order to keep the rotor speed constant, use of a RPM-Governor system is
+ In order to keep the rotor/engine speed constant, use of a RPM-Governor system is
encouraged (see examples).
+ In case the model requires the manual use of a clutch the <tt>\<gearloss\></tt>
+ property might need attention.<ul>
+
+ <li> Electrical: here the gear-loss should be rather large to keep the engine
+ controllable when the clutch is open (although full throttle might still make it
+ spin away).</li>
+ <li> Piston: this engine model already has some internal friction loss and also
+ looses power if it spins too high. Here the gear-loss could be set to 0.25%
+ of the engine power (which is also the approximated default).</li>
+ <li> Turboprop: Here the default value might be a bit too small. Also it's advisable
+ to adjust the power table for rpm values that are far beyond the nominal value.</li>
+
+ </ul>
+
<h4>- Development hints -</h4>
Setting <tt>\<ExternalRPM> -1 \</ExternalRPM></tt> the rotor's RPM is controlled by
</dl>
@author Thomas Kreitler
- @version $Id: FGRotor.h,v 1.11 2011/09/24 14:26:46 jentron Exp $
+ @version $Id: FGRotor.h,v 1.12 2011/10/15 21:30:28 jentron Exp $
*/
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+class FGTransmission : public FGJSBBase {
+
+public:
+ FGTransmission(FGFDMExec *exec, int num);
+ ~FGTransmission();
+
+ void Calculate(double EnginePower, double ThrusterTorque, double dt);
+
+ void SetMaxBrakePower(double x) {MaxBrakePower=x;}
+ double GetMaxBrakePower() const {return MaxBrakePower;}
+ void SetEngineFriction(double x) {EngineFriction=x;}
+ double GetEngineFriction() const {return EngineFriction;}
+ void SetEngineMoment(double x) {EngineMoment=x;}
+ double GetEngineMoment() const {return EngineMoment;}
+ void SetThrusterMoment(double x) {ThrusterMoment=x;}
+ double GetThrusterMoment() const {return ThrusterMoment;}
+
+ double GetFreeWheelTransmission() const {return FreeWheelTransmission;}
+ double GetEngineRPM() {return EngineRPM;}
+ double GetThrusterRPM() {return ThrusterRPM;}
+
+ double GetBrakeCtrl() const {return BrakeCtrlNorm;}
+ void SetBrakeCtrl(double x) {BrakeCtrlNorm=x;}
+ void SetClutchCtrlNorm(double x) {ClutchCtrlNorm=x;}
+
+private:
+ bool BindModel(int num);
+ // void Debug(int from);
+
+ inline double omega_to_rpm(double w) {return w * 9.54929658551372014613302580235;} // omega/(2.0*PI) * 60.0
+ inline double rpm_to_omega(double r) {return r * .104719755119659774615421446109;} // (rpm/60.0)*2.0*PI
+
+ Filter FreeWheelLag;
+ double FreeWheelTransmission; // state, 0: free, 1:locked
+
+ double ThrusterMoment;
+ double EngineMoment; // estimated MOI of gear and engine, influences acceleration
+ double EngineFriction; // estimated friction in gear and possibly engine
+
+ double ClutchCtrlNorm; // also in FGThruster.h
+ double BrakeCtrlNorm;
+ double MaxBrakePower;
+
+ double EngineRPM;
+ double ThrusterRPM;
+ FGPropertyManager* PropertyManager;
+
+};
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
class FGRotor : public FGThruster {
enum eCtrlMapping {eMainCtrl=0, eTailCtrl, eTandemCtrl};
void SetRPM(double rpm) { RPM = rpm; }
/// Retrieves the RPMs of the Engine, as seen from this rotor.
- double GetEngineRPM(void) const { return GearRatio*RPM; } // bit of a hack.
- void SetEngineRPM(double rpm) { RPM = rpm/GearRatio; } // bit of a hack.
+ double GetEngineRPM(void) const {return EngineRPM;} //{ return GearRatio*RPM; }
+ void SetEngineRPM(double rpm) {EngineRPM = rpm;} //{ RPM = rpm/GearRatio; }
/// Tells the rotor's gear ratio, usually the engine asks for this.
double GetGearRatio(void) { return GearRatio; }
/// Retrieves the thrust of the rotor.
double GetCT(void) const { return C_T; }
/// Retrieves the torque
double GetTorque(void) const { return Torque; }
- /// Retrieves the state of the free-wheeling-unit (FWU).
- double GetFreeWheelTransmission(void) const { return FreeWheelTransmission; }
/// Downwash angle - currently only valid for a rotor that spins horizontally
double GetThetaDW(void) const { return theta_downwash; }
double GetLateralCtrl(void) const { return LateralCtrl; }
/// Retrieves the longitudinal control input in radians.
double GetLongitudinalCtrl(void) const { return LongitudinalCtrl; }
- /// Retrieves the normalized brake control input.
- double GetBrakeCtrl(void) const { return BrakeCtrlNorm; }
/// Sets the collective control input in radians.
void SetCollectiveCtrl(double c) { CollectiveCtrl = c; }
void SetLateralCtrl(double c) { LateralCtrl = c; }
/// Sets the longitudinal control input in radians.
void SetLongitudinalCtrl(double c) { LongitudinalCtrl = c; }
- /// Sets the normalized brake control input.
- void SetBrakeCtrl(double c) { BrakeCtrlNorm = c; }
// Stubs. Only main rotor RPM is returned
string GetThrusterLabels(int id, string delimeter);
void Configure(Element* rotor_element);
- // true entry points
- void CalcStatePart1(void);
- void CalcStatePart2(double PowerAvailable);
+ void CalcRotorState(void);
// rotor dynamics
void calc_flow_and_thrust(double theta_0, double Uw, double Ww, double flow_scale = 1.0);
void calc_drag_and_side_forces(double theta_0);
void calc_torque(double theta_0);
- void calc_freewheel_state(double pwr_in, double pwr_out);
-
// transformations
FGColumnVector3 hub_vel_body2ca( const FGColumnVector3 &uvw, const FGColumnVector3 &pqr,
double a_ic = 0.0 , double b_ic = 0.0 );
double Radius;
int BladeNum;
+ // rpm control
double Sense;
double NominalRPM;
double MinimalRPM;
int ExternalRPM;
int RPMdefinition;
FGPropertyManager* ExtRPMsource;
+ double SourceGearRatio;
double BladeChord;
double LiftCurveSlope;
double LateralCtrl;
double LongitudinalCtrl;
- double BrakeCtrlNorm, MaxBrakePower;
-
- // free-wheeling-unit (FWU)
- int FreeWheelPresent; // 'installed' or not
- double FreeWheelThresh; // when to release
- Filter FreeWheelLag;
- double FreeWheelTransmission; // state, 0: free, 1:locked
-
+ // interaction with engine
+ FGTransmission *Transmission;
+ double EngineRPM;
+ double MaxBrakePower;
+ double GearLoss;
+ double GearMoment;
};
projects / flightgear.git / commitdiff