diff --git a/src/FDM/JSBSim/FGFDMExec.cpp b/src/FDM/JSBSim/FGFDMExec.cpp index 87ee94dad..570baf75f 100644 --- a/src/FDM/JSBSim/FGFDMExec.cpp +++ b/src/FDM/JSBSim/FGFDMExec.cpp @@ -70,7 +70,7 @@ using namespace std; 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; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -85,7 +85,7 @@ FGFDMExec::FGFDMExec(FGPropertyManager* root, unsigned int* fdmctr) : Root(root) Frame = 0; Error = 0; - GroundCallback = 0; + GroundCallback = new FGDefaultGroundCallback(); IC = 0; Trim = 0; Script = 0; @@ -186,6 +186,9 @@ FGFDMExec::~FGFDMExec() if (FDMctr > 0) (*FDMctr)--; + if(GroundCallback) + delete GroundCallback; + Debug(1); } @@ -210,7 +213,7 @@ bool FGFDMExec::Allocate(void) 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); @@ -266,8 +269,6 @@ bool FGFDMExec::DeAllocate(void) delete IC; delete Trim; - delete GroundCallback; - Error = 0; modelLoaded = false; @@ -303,11 +304,11 @@ bool FGFDMExec::Run(void) 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); @@ -563,9 +564,7 @@ void FGFDMExec::Initialize(FGInitialCondition *FGIC) Propagate->InitializeDerivatives(); LoadInputs(eAtmosphere); Atmosphere->Run(false); - Winds->SetWindNED( FGIC->GetWindNFpsIC(), - FGIC->GetWindEFpsIC(), - FGIC->GetWindDFpsIC() ); + Winds->SetWindNED(FGIC->GetWindNEDFpsIC()); Auxiliary->Run(false); } diff --git a/src/FDM/JSBSim/FGFDMExec.h b/src/FDM/JSBSim/FGFDMExec.h index 4295348d0..68dc380d1 100644 --- a/src/FDM/JSBSim/FGFDMExec.h +++ b/src/FDM/JSBSim/FGFDMExec.h @@ -56,7 +56,7 @@ INCLUDES 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 @@ -181,7 +181,7 @@ CLASS DOCUMENTATION property actually maps toa function call of DoTrim(). @author Jon S. Berndt - @version $Revision: 1.71 $ + @version $Revision: 1.72 $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -269,7 +269,8 @@ public: 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. diff --git a/src/FDM/JSBSim/FGJSBBase.cpp b/src/FDM/JSBSim/FGJSBBase.cpp index ccf098b59..f0bc3ed48 100644 --- a/src/FDM/JSBSim/FGJSBBase.cpp +++ b/src/FDM/JSBSim/FGJSBBase.cpp @@ -44,7 +44,7 @@ INCLUDES 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; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -283,5 +283,67 @@ double FGJSBBase::GaussianRandomNumber(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +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 diff --git a/src/FDM/JSBSim/FGJSBBase.h b/src/FDM/JSBSim/FGJSBBase.h index 50735c3f7..269b550d9 100644 --- a/src/FDM/JSBSim/FGJSBBase.h +++ b/src/FDM/JSBSim/FGJSBBase.h @@ -56,7 +56,7 @@ using std::max; 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 @@ -72,7 +72,7 @@ CLASS DOCUMENTATION * 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 $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -256,6 +256,29 @@ public: 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 diff --git a/src/FDM/JSBSim/JSBSim.cxx b/src/FDM/JSBSim/JSBSim.cxx index 77b006f09..0da3ad6f6 100644 --- a/src/FDM/JSBSim/JSBSim.cxx +++ b/src/FDM/JSBSim/JSBSim.cxx @@ -112,6 +112,26 @@ public: 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; }; @@ -371,9 +391,6 @@ void FGJSBsim::init() -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() ); @@ -394,7 +411,6 @@ void FGJSBsim::init() needTrim = startup_trim->getBoolValue(); common_init(); - fgic->SetSeaLevelRadiusFtIC( get_Sea_level_radius() ); copy_to_JSBsim(); fdmex->RunIC(); //loop JSBSim once w/o integrating @@ -661,9 +677,6 @@ bool FGJSBsim::copy_to_JSBsim() } // end FGEngine code block } - - Propagate->SetSeaLevelRadius( get_Sea_level_radius() ); - Atmosphere->SetTemperature(temperature->getDoubleValue(), get_Altitude(), FGAtmosphere::eCelsius); Atmosphere->SetPressureSL(pressureSL->getDoubleValue(), FGAtmosphere::eInchesHg); @@ -1024,11 +1037,10 @@ void FGJSBsim::set_Latitude(double lat) 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); } @@ -1038,10 +1050,10 @@ void FGJSBsim::set_Longitude(double lon) if (needTrim) fgic->SetLongitudeRadIC(lon); - else { + else Propagate->SetLongitude(lon); - FGInterface::set_Longitude(lon); - } + + FGInterface::set_Longitude(lon); } // Sets the altitude above sea level. @@ -1051,10 +1063,10 @@ void FGJSBsim::set_Altitude(double alt) 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) @@ -1064,7 +1076,10 @@ 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(); @@ -1073,9 +1088,9 @@ void FGJSBsim::set_V_calibrated_kts(double vc) 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) @@ -1093,9 +1108,9 @@ 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 ) @@ -1114,9 +1129,9 @@ 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) @@ -1133,9 +1148,9 @@ 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 @@ -1155,9 +1170,9 @@ void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi ) 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 @@ -1178,9 +1193,9 @@ void FGJSBsim::set_Climb_Rate( double roc) 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); } } @@ -1199,45 +1214,9 @@ void FGJSBsim::set_Gamma_vert_rad( double gamma) 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); } } diff --git a/src/FDM/JSBSim/JSBSim.hxx b/src/FDM/JSBSim/JSBSim.hxx index 1e6e09355..80b3f3c3a 100644 --- a/src/FDM/JSBSim/JSBSim.hxx +++ b/src/FDM/JSBSim/JSBSim.hxx @@ -297,7 +297,6 @@ private: 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); diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp index fabae4c5c..d787193f5 100644 --- a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp +++ b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp @@ -63,7 +63,7 @@ using namespace std; 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; //****************************************************************************** @@ -74,6 +74,7 @@ FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) : fdmex(FDMExec) if(FDMExec != NULL ) { PropertyManager=fdmex->GetPropertyManager(); + Atmosphere=fdmex->GetAtmosphere(); Constructing = true; bind(); Constructing = false; @@ -105,16 +106,13 @@ void FGInitialCondition::ResetIC(double u0, double v0, double w0, 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(); @@ -132,21 +130,20 @@ void FGInitialCondition::ResetIC(double u0, double v0, double w0, 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.); } //****************************************************************************** @@ -168,12 +165,12 @@ void FGInitialCondition::WriteStateFile(int num) 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; @@ -189,8 +186,8 @@ void FGInitialCondition::WriteStateFile(int num) 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; } @@ -200,7 +197,7 @@ void FGInitialCondition::SetVequivalentKtsIC(double ve) 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; @@ -211,8 +208,11 @@ void FGInitialCondition::SetMachIC(double mach) 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); @@ -225,6 +225,7 @@ void FGInitialCondition::SetVcalibratedKtsIC(double vcas) 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); @@ -271,11 +272,12 @@ void FGInitialCondition::calcAeroAngles(const FGColumnVector3& _vt_NED) 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(); @@ -294,6 +296,7 @@ void FGInitialCondition::SetVgroundFpsIC(double vg) 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; @@ -322,6 +325,7 @@ void FGInitialCondition::SetClimbRateFpsIC(double hdot) 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); @@ -345,6 +349,7 @@ void FGInitialCondition::SetClimbRateFpsIC(double hdot) void FGInitialCondition::SetAlphaRadIC(double alfa) { + const FGMatrix33& Tb2l = orientation.GetTInv(); FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); calcThetaBeta(alfa, _vt_NED); } @@ -356,9 +361,10 @@ void FGInitialCondition::SetAlphaRadIC(double alfa) 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.); @@ -400,13 +406,11 @@ void FGInitialCondition::calcThetaBeta(double alfa, const FGColumnVector3& _vt_N 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; @@ -430,18 +434,24 @@ void FGInitialCondition::calcThetaBeta(double alfa, const FGColumnVector3& _vt_N 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(); @@ -451,7 +461,7 @@ void FGInitialCondition::SetBetaRadIC(double bta) 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.); @@ -462,64 +472,32 @@ void FGInitialCondition::SetBetaRadIC(double bta) 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(); + vOrient(idx) = angle; + orientation = FGQuaternion(vOrient); - 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.); - - 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); } @@ -531,6 +509,8 @@ void FGInitialCondition::SetPsiRadIC(double psy) 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; @@ -552,6 +532,7 @@ void FGInitialCondition::SetBodyVelFpsIC(int idx, double vel) 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; @@ -583,9 +564,10 @@ void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD ) 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; @@ -605,13 +587,14 @@ void FGInitialCondition::SetCrossWindKtsIC(double cross) 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; @@ -632,6 +615,7 @@ void FGInitialCondition::SetHeadWindKtsIC(double head) 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; @@ -647,6 +631,7 @@ void FGInitialCondition::SetWindDownKtsIC(double 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.); @@ -672,6 +657,7 @@ void FGInitialCondition::SetWindMagKtsIC(double mag) 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); @@ -693,25 +679,30 @@ void FGInitialCondition::SetWindDirDegIC(double dir) 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; @@ -723,90 +714,11 @@ void FGInitialCondition::SetAltitudeASLFtIC(double alt) } } -//****************************************************************************** -// 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; @@ -818,6 +730,7 @@ double FGInitialCondition::GetWindDirDegIC(void) const 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; @@ -828,6 +741,7 @@ double FGInitialCondition::GetNEDWindFpsIC(int idx) const 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; @@ -838,6 +752,7 @@ double FGInitialCondition::GetWindFpsIC(void) const 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; @@ -850,10 +765,13 @@ double FGInitialCondition::GetBodyWindFpsIC(int idx) const 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); } //****************************************************************************** @@ -861,8 +779,8 @@ double FGInitialCondition::GetVcalibratedKtsIC(void) const 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); } @@ -871,7 +789,7 @@ double FGInitialCondition::GetVequivalentKtsIC(void) const 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; } @@ -880,6 +798,7 @@ double FGInitialCondition::GetMachIC(void) const double FGInitialCondition::GetBodyVelFpsIC(int idx) const { + const FGMatrix33& Tl2b = orientation.GetT(); FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED; return _vUVW_BODY(idx); @@ -923,6 +842,8 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath) 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"); @@ -937,9 +858,6 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath) running_elements = document->FindNextElement("running"); } - fdmex->RunIC(); - fdmex->GetPropagate()->DumpState(); - return result; } @@ -963,17 +881,16 @@ bool FGInitialCondition::Load_v1(void) else if (document->FindElement("altitudeMSL")) // This is feet above sea level position.SetRadius(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT") + sea_level_radius); - if (document->FindElement("phi")) - phi = document->FindElementValueAsNumberConvertTo("phi", "RAD"); - if (document->FindElement("theta")) - theta = document->FindElementValueAsNumberConvertTo("theta", "RAD"); - if (document->FindElement("psi")) - psi = document->FindElementValueAsNumberConvertTo("psi", "RAD"); + FGColumnVector3 vOrient = orientation.GetEuler(); - FGQuaternion Quat(phi, theta, psi); - Quat.Normalize(); - Tl2b = Quat.GetT(); - Tb2l = Quat.GetTInv(); + if (document->FindElement("phi")) + vOrient(ePhi) = document->FindElementValueAsNumberConvertTo("phi", "RAD"); + if (document->FindElement("theta")) + vOrient(eTht) = document->FindElementValueAsNumberConvertTo("theta", "RAD"); + if (document->FindElement("psi")) + vOrient(ePsi) = document->FindElementValueAsNumberConvertTo("psi", "RAD"); + + orientation = FGQuaternion(vOrient); if (document->FindElement("ubody")) SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo("ubody", "FT/SEC")); @@ -1024,9 +941,7 @@ bool FGInitialCondition::Load_v1(void) -radInv*vUVW_NED(eNorth), -radInv*vUVW_NED(eEast)*position.GetTanLatitude() ); - p = vOmegaLocal(eP); - q = vOmegaLocal(eR); - r = vOmegaLocal(eQ); + vPQR_body = vOmegaLocal; return result; } @@ -1111,7 +1026,6 @@ bool FGInitialCondition::Load_v2(void) // 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); @@ -1132,7 +1046,7 @@ bool FGInitialCondition::Load_v2(void) QuatI2Body.Normalize(); FGQuaternion QuatLocal2I = position.GetTl2i(); QuatLocal2I.Normalize(); - QuatLocal2Body = QuatLocal2I * QuatI2Body; + orientation = QuatLocal2I * QuatI2Body; } else if (frame == "ecef") { @@ -1150,11 +1064,11 @@ bool FGInitialCondition::Load_v2(void) 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 { @@ -1165,13 +1079,6 @@ bool FGInitialCondition::Load_v2(void) } } - 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) @@ -1183,6 +1090,8 @@ bool FGInitialCondition::Load_v2(void) 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"); @@ -1224,8 +1133,8 @@ bool FGInitialCondition::Load_v2(void) // - 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. @@ -1242,11 +1151,11 @@ bool FGInitialCondition::Load_v2(void) 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 @@ -1254,17 +1163,13 @@ bool FGInitialCondition::Load_v2(void) 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; } @@ -1317,7 +1222,9 @@ void FGInitialCondition::bind(void) &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, @@ -1414,7 +1321,9 @@ void FGInitialCondition::bind(void) &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, diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.h b/src/FDM/JSBSim/initialization/FGInitialCondition.h index 3b349e424..03f11f064 100644 --- a/src/FDM/JSBSim/initialization/FGInitialCondition.h +++ b/src/FDM/JSBSim/initialization/FGInitialCondition.h @@ -54,7 +54,7 @@ INCLUDES 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 @@ -65,6 +65,7 @@ namespace JSBSim { class FGFDMExec; class FGMatrix33; class FGColumnVector3; +class FGAtmosphere; typedef enum { setvt, setvc, setve, setmach, setuvw, setned, setvg } speedset; @@ -213,7 +214,7 @@ CLASS DOCUMENTATION @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 $" */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -349,15 +350,15 @@ public: /** 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 */ @@ -411,17 +412,17 @@ public: @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 @@ -473,6 +474,14 @@ public: @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); } @@ -497,10 +506,18 @@ public: @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); } @@ -525,17 +542,21 @@ public: @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 */ @@ -546,21 +567,21 @@ public: @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 */ @@ -588,9 +609,9 @@ public: @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 */ @@ -600,13 +621,21 @@ public: @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 */ @@ -632,21 +661,22 @@ public: 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); @@ -654,13 +684,12 @@ private: 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); diff --git a/src/FDM/JSBSim/input_output/FGGroundCallback.cpp b/src/FDM/JSBSim/input_output/FGGroundCallback.cpp index 8933101ca..069750c14 100644 --- a/src/FDM/JSBSim/input_output/FGGroundCallback.cpp +++ b/src/FDM/JSBSim/input_output/FGGroundCallback.cpp @@ -39,34 +39,22 @@ namespace JSBSim { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -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 { @@ -75,9 +63,11 @@ double FGGroundCallback::GetAGLevel(double t, const FGLocation& loc, 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 diff --git a/src/FDM/JSBSim/input_output/FGGroundCallback.h b/src/FDM/JSBSim/input_output/FGGroundCallback.h index bb6780915..10063f9b9 100644 --- a/src/FDM/JSBSim/input_output/FGGroundCallback.h +++ b/src/FDM/JSBSim/input_output/FGGroundCallback.h @@ -45,7 +45,7 @@ INCLUDES 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 { @@ -53,13 +53,13 @@ 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 $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -69,31 +69,88 @@ CLASS DECLARATION 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(); + 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) { } - /** 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;} -private: - /// Reference radius. - double mReferenceRadius; }; +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// 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: + + double mSeaLevelRadius; + double mTerrainLevelRadius; +}; + + } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% #endif diff --git a/src/FDM/JSBSim/math/FGLocation.cpp b/src/FDM/JSBSim/math/FGLocation.cpp index ab88f3735..ce10adfef 100644 --- a/src/FDM/JSBSim/math/FGLocation.cpp +++ b/src/FDM/JSBSim/math/FGLocation.cpp @@ -45,7 +45,7 @@ INCLUDES 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; @@ -137,6 +137,7 @@ FGLocation::FGLocation(const FGLocation& l) 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. @@ -152,6 +153,10 @@ FGLocation::FGLocation(const FGLocation& l) 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; @@ -173,6 +178,7 @@ const FGLocation& FGLocation::operator=(const FGLocation& l) e = l.e; eps2 = l.eps2; f = l.f; + epa = l.epa; //ag See comment in constructor above if (!mCacheValid) return *this; @@ -183,6 +189,10 @@ const FGLocation& FGLocation::operator=(const FGLocation& l) 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; diff --git a/src/FDM/JSBSim/models/FGAerodynamics.cpp b/src/FDM/JSBSim/models/FGAerodynamics.cpp index 13a9b9f17..ce2494c54 100644 --- a/src/FDM/JSBSim/models/FGAerodynamics.cpp +++ b/src/FDM/JSBSim/models/FGAerodynamics.cpp @@ -48,7 +48,7 @@ using namespace std; 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; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -208,13 +208,15 @@ bool FGAerodynamics::Run(bool Holding) 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 @@ -382,7 +384,7 @@ string FGAerodynamics::GetAeroFunctionValues(const string& delimeter) const 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(); } } diff --git a/src/FDM/JSBSim/models/FGOutput.cpp b/src/FDM/JSBSim/models/FGOutput.cpp index 46c9b15ce..700a96cbd 100644 --- a/src/FDM/JSBSim/models/FGOutput.cpp +++ b/src/FDM/JSBSim/models/FGOutput.cpp @@ -77,7 +77,7 @@ using namespace std; 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) @@ -216,7 +216,7 @@ void FGOutput::SetType(const string& type) { if (type == "CSV") { Type = otCSV; - delimeter = ", "; + delimeter = ","; } else if (type == "TABULAR") { Type = otTab; delimeter = "\t"; diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index 0668ac682..d1ec2b922 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -68,7 +68,7 @@ using namespace std; 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; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -77,8 +77,6 @@ CLASS IMPLEMENTATION FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex), - LocalTerrainRadius(0), - SeaLevelRadius(0), VehicleRadius(0) { Debug(0); @@ -115,10 +113,9 @@ FGPropagate::~FGPropagate(void) 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(); @@ -145,11 +142,7 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) // 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 @@ -161,31 +154,24 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) // 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; @@ -269,8 +255,7 @@ bool FGPropagate::Run(bool Holding) CalculateUVW(); // Set auxilliary state variables - RecomputeLocalTerrainRadius(); - + RecomputeLocalTerrainVelocity(); VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet; @@ -410,39 +395,79 @@ void FGPropagate::SetInertialRates(FGColumnVector3 vRates) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -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()); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -456,7 +481,7 @@ void FGPropagate::SetVState(const VehicleState& vstate) Tec2i = Ti2ec.Transposed(); UpdateLocationMatrices(); SetInertialOrientation(vstate.qAttitudeECI); - RecomputeLocalTerrainRadius(); + RecomputeLocalTerrainVelocity(); VehicleRadius = GetRadius(); VState.vUVW = vstate.vUVW; vVel = Tb2l * VState.vUVW; @@ -469,7 +494,7 @@ void FGPropagate::SetVState(const VehicleState& vstate) void FGPropagate::UpdateVehicleState(void) { - RecomputeLocalTerrainRadius(); + RecomputeLocalTerrainVelocity(); VehicleRadius = GetRadius(); VState.vInertialPosition = Tec2i * VState.vLocation; UpdateLocationMatrices(); diff --git a/src/FDM/JSBSim/models/FGPropagate.h b/src/FDM/JSBSim/models/FGPropagate.h index f262d1fb1..e1384f616 100644 --- a/src/FDM/JSBSim/models/FGPropagate.h +++ b/src/FDM/JSBSim/models/FGPropagate.h @@ -49,7 +49,7 @@ INCLUDES 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 @@ -93,7 +93,7 @@ CLASS DOCUMENTATION @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 $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -293,7 +293,7 @@ public: 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. @@ -377,7 +377,7 @@ public: 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(); } @@ -385,6 +385,8 @@ public: 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 { @@ -502,11 +504,14 @@ public: 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) @@ -520,8 +525,6 @@ public: SetLocation(l); } - void RecomputeLocalTerrainRadius(void); - void NudgeBodyLocation(FGColumnVector3 deltaLoc) { VState.vInertialPosition -= Tb2i*deltaLoc; VState.vLocation -= Tb2ec*deltaLoc; @@ -563,8 +566,9 @@ private: 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; diff --git a/src/FDM/JSBSim/models/atmosphere/FGWinds.h b/src/FDM/JSBSim/models/atmosphere/FGWinds.h index 6ea065af6..8d8aa0552 100644 --- a/src/FDM/JSBSim/models/atmosphere/FGWinds.h +++ b/src/FDM/JSBSim/models/atmosphere/FGWinds.h @@ -47,7 +47,7 @@ INCLUDES 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 @@ -150,6 +150,9 @@ public: /// 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; } diff --git a/src/FDM/JSBSim/models/propulsion/FGPiston.cpp b/src/FDM/JSBSim/models/propulsion/FGPiston.cpp index 304c00971..0f47018a6 100644 --- a/src/FDM/JSBSim/models/propulsion/FGPiston.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGPiston.cpp @@ -50,7 +50,7 @@ using namespace std; 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; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -434,6 +434,8 @@ void FGPiston::Calculate(void) 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(); @@ -618,10 +620,7 @@ void FGPiston::doMAP(void) // 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; @@ -799,10 +798,7 @@ void FGPiston::doEGT(void) } 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; } @@ -841,12 +837,9 @@ void FGPiston::doCHT(void) 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; + } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -880,10 +873,7 @@ void FGPiston::doOilTemperature(void) 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); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/models/propulsion/FGPiston.h b/src/FDM/JSBSim/models/propulsion/FGPiston.h index 2174b806a..9c0386487 100644 --- a/src/FDM/JSBSim/models/propulsion/FGPiston.h +++ b/src/FDM/JSBSim/models/propulsion/FGPiston.h @@ -46,7 +46,7 @@ INCLUDES 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 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -198,7 +198,7 @@ boostspeed they refer to: @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 $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -332,6 +332,7 @@ private: // // 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 diff --git a/src/FDM/JSBSim/models/propulsion/FGPropeller.cpp b/src/FDM/JSBSim/models/propulsion/FGPropeller.cpp index ead7dec9b..f2c0167e1 100644 --- a/src/FDM/JSBSim/models/propulsion/FGPropeller.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGPropeller.cpp @@ -45,7 +45,7 @@ using namespace std; 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; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -124,7 +124,7 @@ FGPropeller::FGPropeller(FGFDMExec* exec, Element* prop_element, int num) 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) { diff --git a/src/FDM/JSBSim/models/propulsion/FGRotor.cpp b/src/FDM/JSBSim/models/propulsion/FGRotor.cpp index 0a733fb83..515c73200 100644 --- a/src/FDM/JSBSim/models/propulsion/FGRotor.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGRotor.cpp @@ -47,6 +47,7 @@ INCLUDES #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; @@ -55,7 +56,7 @@ using std::cout; 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; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -69,6 +70,113 @@ CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +// 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 @@ -78,7 +186,7 @@ FGRotor::FGRotor(FGFDMExec *exec, Element* rotor_element, int num) 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), @@ -93,9 +201,8 @@ FGRotor::FGRotor(FGFDMExec *exec, Element* rotor_element, int num) 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; @@ -158,12 +265,58 @@ FGRotor::FGRotor(FGFDMExec *exec, Element* rotor_element, int num) // 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, @@ -175,9 +328,6 @@ FGRotor::FGRotor(FGFDMExec *exec, Element* rotor_element, int num) // 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(); @@ -188,6 +338,7 @@ FGRotor::FGRotor(FGFDMExec *exec, Element* rotor_element, int num) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FGRotor::~FGRotor(){ + if (Transmission) delete Transmission; Debug(1); } @@ -306,17 +457,6 @@ void FGRotor::Configure(Element* rotor_element) 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]; @@ -564,7 +704,7 @@ FGColumnVector3 FGRotor::body_moments(double a_ic, double b_ic) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGRotor::CalcStatePart1(void) +void FGRotor::CalcRotorState(void) { double A_IC; // lateral (roll) control in radians double B_IC; // longitudinal (pitch) control in radians @@ -583,7 +723,7 @@ void FGRotor::CalcStatePart1(void) // 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 @@ -632,64 +772,24 @@ void FGRotor::CalcStatePart1(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -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); + + EngineRPM = Transmission->GetEngineRPM() * GearRatio; + RPM = Transmission->GetThrusterRPM(); + } else { + EngineRPM = RPM * GearRatio; } - DeltaPower = EnginePower * ClutchCtrlNorm * FWmult - PowerRequired; - - CalcStatePart2(DeltaPower); + RPM = Constrain(MinimalRPM, RPM, MaximalRPM); // trim again return Thrust; } @@ -767,22 +867,17 @@ bool FGRotor::BindModel(void) 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 { @@ -860,7 +955,7 @@ void FGRotor::Debug(int from) 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; } } @@ -876,6 +971,8 @@ void FGRotor::Debug(int from) 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; @@ -884,12 +981,6 @@ void FGRotor::Debug(int from) } 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 diff --git a/src/FDM/JSBSim/models/propulsion/FGRotor.h b/src/FDM/JSBSim/models/propulsion/FGRotor.h index 9e0bb0511..a2512ced9 100644 --- a/src/FDM/JSBSim/models/propulsion/FGRotor.h +++ b/src/FDM/JSBSim/models/propulsion/FGRotor.h @@ -46,7 +46,7 @@ INCLUDES 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 @@ -80,6 +80,8 @@ CLASS DOCUMENTATION <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> @@ -120,13 +122,18 @@ CLASS DOCUMENTATION 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. @@ -134,10 +141,6 @@ CLASS DOCUMENTATION 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> @@ -154,7 +157,7 @@ CLASS DOCUMENTATION <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> @@ -178,9 +181,23 @@ CLASS DOCUMENTATION <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 @@ -205,7 +222,7 @@ CLASS DOCUMENTATION </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 $ */ @@ -214,6 +231,57 @@ CLASS DOCUMENTATION 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}; @@ -241,8 +309,8 @@ public: 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. @@ -267,8 +335,6 @@ public: 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; } @@ -281,8 +347,6 @@ public: 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; } @@ -290,8 +354,6 @@ public: 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); @@ -308,9 +370,7 @@ private: 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); @@ -319,8 +379,6 @@ private: 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 ); @@ -341,6 +399,7 @@ private: double Radius; int BladeNum; + // rpm control double Sense; double NominalRPM; double MinimalRPM; @@ -348,6 +407,7 @@ private: int ExternalRPM; int RPMdefinition; FGPropertyManager* ExtRPMsource; + double SourceGearRatio; double BladeChord; double LiftCurveSlope; @@ -400,14 +460,12 @@ private: 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; };