From 342842537d4ab58a1efc2903cbccb011ea138097 Mon Sep 17 00:00:00 2001 From: curt Date: Fri, 4 Aug 2000 16:08:55 +0000 Subject: [PATCH] Fixes to jsbsim. --- src/Cockpit/cockpit.cxx | 111 +++++++++++++++++----------------- src/FDM/JSBSim.cxx | 50 ++++----------- src/FDM/JSBSim/FGMatrix.cpp | 18 ++++-- src/FDM/JSBSim/FGTrimLong.cpp | 2 + src/Joystick/joystick.cxx | 2 + 5 files changed, 87 insertions(+), 96 deletions(-) diff --git a/src/Cockpit/cockpit.cxx b/src/Cockpit/cockpit.cxx index eecbf69ee..6a9142b21 100644 --- a/src/Cockpit/cockpit.cxx +++ b/src/Cockpit/cockpit.cxx @@ -319,67 +319,70 @@ char *dmshh_format(double degrees) } #endif // 0 -/****************************************************************************/ -/* Convert degrees to dd mm'ss.s'' (DMS-Format) */ -/****************************************************************************/ -static char *toDMS(float a) -{ - int neg = 0; - float d, m, s; - static char dms[16]; - if (a < 0.0f) { - a = -a; - neg = 1; - } - d = (float) ((int) a); - a = (a - d) * 60.0f; - m = (float) ((int) a); - s = (a - m) * 60.0f; - - if (s > 59.5f) { - s = 0.0f; - m += 1.0f; - } - if (m > 59.5f) { - m = 0.0f; - d += 1.0f; - } - if (neg) - d = -d; - - sprintf(dms, "%.0f*%02.0f %04.1f", d, m, s); - return dms; +/************************************************************************ + Convert degrees to dd mm.mmm' (DMM-Format) + Description: Converts using a round-off factor tailored to the required + precision of the minutes field (three decimal places). Round-off + prevents function from returning a minutes value of 60. + + Input arguments: Coordinate value in decimal degrees + +************************************************************************/ +static char *toDM(float dd) +{ + static char dm[16]; + double tempdd; + double mn; + double sign = 1; + int deg; + + if (dd < 0) { + sign = -1; + } + /* round for minutes expressed to three decimal places */ + tempdd = fabs(dd) + (5.0E-4 / 60.0); + deg = (int)tempdd; + mn = fabs( (tempdd - (double)(deg)) * 60.0 - 4.999E-4 ); + deg *= (int)sign; + sprintf(dm, "%d*%06.3f", deg, mn); + return dm; } -/****************************************************************************/ -/* Convert degrees to dd mm.mmm' (DMM-Format) */ -/****************************************************************************/ -static char *toDM(float a) -{ - int neg = 0; - float d, m; - static char dm[16]; - - if (a < 0.0f) { - a = -a; - neg = 1; - } +/************************************************************************ + Convert degrees to dd mm'ss.s'' (DMS-Format) + Description: Converts using a round-off factor tailored to the required + precision of the seconds field (one decimal place). Round-off + prevents function from returning a seconds value of 60. - d = (float) ( (int) a); - m = (a - d) * 60.0f; - - if (m > 59.5f) { - m = 0.0f; - d += 1.0f; - } - if (neg) d = -d; - - sprintf(dm, "%.0f*%06.3f", d, m); - return dm; + Input arguments: Coordinate value in decimal degrees + +************************************************************************/ +static char *toDMS(float dd) +{ + static char dms[16]; + double tempdd, tempmin; + int deg; + int mn; + double sec; + double sign = 1; + + if(dd < 0) { + sign = -1; + } + /* round up for seconds expressed to one decimal place */ + tempdd = fabs(dd) + (0.05 / 3600.0); + deg = (int)tempdd; + tempmin = (tempdd - (double)(deg)) * 60.0; + mn = (int)tempmin; + sec = fabs( (tempmin - (double)(mn)) * 60.0 - 0.049 ); + deg *= (int)sign; + sprintf(dms, "%d*%02d %04.1f", deg, mn, sec); + return dms; } + // Have to set the LatLon display type //static char *(*fgLatLonFormat)(float) = toDM; static char *(*fgLatLonFormat)(float); diff --git a/src/FDM/JSBSim.cxx b/src/FDM/JSBSim.cxx index 376afdfc2..5f3a77b2f 100644 --- a/src/FDM/JSBSim.cxx +++ b/src/FDM/JSBSim.cxx @@ -87,6 +87,13 @@ int FGJSBsim::init( double dt ) { return 0; } + FDMExec.GetAtmosphere()->SetExTemperature(get_Static_temperature()); + FDMExec.GetAtmosphere()->SetExPressure(get_Static_pressure()); + FDMExec.GetAtmosphere()->SetExDensity(get_Density()); + FDMExec.GetAtmosphere()->SetWindNED(get_V_north_airmass(), + get_V_east_airmass(), + get_V_down_airmass()); + FDMExec.GetAtmosphere()->UseInternal(); FG_LOG( FG_FLIGHT, FG_INFO, " Initializing JSBSim with:" ); @@ -160,8 +167,7 @@ int FGJSBsim::init( double dt ) { FG_LOG( FG_FLIGHT, FG_INFO, "Finished initializing JSBSim" ); copy_from_JSBsim(); - - + return 1; } @@ -257,11 +263,6 @@ int FGJSBsim::copy_from_JSBsim() { FDMExec.GetAircraft()->GetXYZcg()(2), FDMExec.GetAircraft()->GetXYZcg()(3) ); - - set_Accels_Local( FDMExec.GetPosition()->GetVelDot()(1), - FDMExec.GetPosition()->GetVelDot()(2), - FDMExec.GetPosition()->GetVelDot()(3) ); - set_Accels_Body ( FDMExec.GetTranslation()->GetUVWdot()(1), FDMExec.GetTranslation()->GetUVWdot()(2), FDMExec.GetTranslation()->GetUVWdot()(3) ); @@ -274,7 +275,6 @@ int FGJSBsim::copy_from_JSBsim() { // FDMExec.GetTranslation()->GetNcg()(2), // FDMExec.GetTranslation()->GetNcg()(3) ); // - set_Accels_Pilot_Body( FDMExec.GetAuxiliary()->GetPilotAccel()(1), FDMExec.GetAuxiliary()->GetPilotAccel()(2), FDMExec.GetAuxiliary()->GetPilotAccel()(3) ); @@ -311,13 +311,11 @@ int FGJSBsim::copy_from_JSBsim() { FDMExec.GetState()->GetParameter(FG_PITCHRATE), FDMExec.GetState()->GetParameter(FG_YAWRATE) ); - set_Euler_Rates( FDMExec.GetRotation()->GetEulerRates()(1), - FDMExec.GetRotation()->GetEulerRates()(2), - FDMExec.GetRotation()->GetEulerRates()(3) ); + /* HUH!?! */ set_Euler_Rates( FDMExec.GetRotation()->Getphi(), + FDMExec.GetRotation()->Gettht(), + FDMExec.GetRotation()->Getpsi() ); - set_Geocentric_Rates( FDMExec.GetPosition()->GetLatitudeDot(), - FDMExec.GetPosition()->GetLongitudeDot(), - FDMExec.GetPosition()->Gethdot() ); + // ***FIXME*** set_Geocentric_Rates( Latitude_dot, Longitude_dot, Radius_dot ); set_Mach_number( FDMExec.GetTranslation()->GetMach()); @@ -345,35 +343,13 @@ int FGJSBsim::copy_from_JSBsim() { set_Euler_Angles( FDMExec.GetRotation()->Getphi(), FDMExec.GetRotation()->Gettht(), FDMExec.GetRotation()->Getpsi() ); - - for(int i=0; i<3; i++ ) { - for (int j=0; j<3; j++ ) { - set_T_Local_to_Body(i,j,FDMExec.GetState()->GetTl2b()(i,j)); - } - } set_Alpha( FDMExec.GetTranslation()->Getalpha() ); set_Beta( FDMExec.GetTranslation()->Getbeta() ); - - set_Cos_phi( FDMExec.GetRotation()->GetCosphi() ); - //set_Sin_phi ( FDMExec.GetRotation()->GetSinpphi() ); - - set_Cos_theta( FDMExec.GetRotation()->GetCostht() ); - //set_Sin_theta ( FDMExec.GetRotation()->GetSintht() ); - - //set_Cos_psi( FDMExec.GetRotation()->GetCospsi() ); - //set_Sin_psi ( FDMExec.GetRotation()->GetSinpsi() ); - + set_Gamma_vert_rad( FDMExec.GetPosition()->GetGamma() ); // set_Gamma_horiz_rad( Gamma_horiz_rad ); - - set_Density( FDMExec.GetAtmosphere()->GetDensity() ); - set_Static_pressure( FDMExec.GetAtmosphere()->GetPressure() ); - set_Static_temperature ( FDMExec.GetAtmosphere()->GetTemperature() ); - - set_Earth_position_angle( FDMExec.GetAuxiliary()->GetEarthPositionAngle() ); - /* **FIXME*** */ set_Sea_level_radius( sl_radius2 * METER_TO_FEET ); /* **FIXME*** */ set_Earth_position_angle( 0.0 ); diff --git a/src/FDM/JSBSim/FGMatrix.cpp b/src/FDM/JSBSim/FGMatrix.cpp index 6e51fa17d..5b4f5da10 100644 --- a/src/FDM/JSBSim/FGMatrix.cpp +++ b/src/FDM/JSBSim/FGMatrix.cpp @@ -344,22 +344,24 @@ FGMatrix FGMatrix::operator/(const double scalar) } } - } - return Quot; + } else + cerr << "Attempt to divide by zero in method FGMatrix::operator/(const double scalar), object at " << this << endl; + return Quot; } /******************************************************************************/ void FGMatrix::operator/=(const double scalar) { - + if(scalar != 0) { for (unsigned int i=1; i<=Rows(); i++) { for (unsigned int j=1; j<=Cols(); j++) { data[i][j]/=scalar; } } - } + } else + cerr << "Attempt to divide by zero in method FGMatrix::operator/=(const double scalar), object " << this << endl; } /******************************************************************************/ @@ -519,9 +521,15 @@ FGColumnVector FGColumnVector::operator/(const double scalar) { FGColumnVector Quotient(Rows()); if(scalar != 0) { + + for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar; - } + + } else + cerr << "Attempt to divide by zero in method FGColumnVector::operator/(const double scalar), object " << this << endl; return Quotient; + + } /******************************************************************************/ diff --git a/src/FDM/JSBSim/FGTrimLong.cpp b/src/FDM/JSBSim/FGTrimLong.cpp index 91f0a29f1..7bc91686a 100644 --- a/src/FDM/JSBSim/FGTrimLong.cpp +++ b/src/FDM/JSBSim/FGTrimLong.cpp @@ -406,7 +406,9 @@ bool FGTrimLong::DoTrim(void) { if(checkLimits(udotf,dth,0,1) == false) { cout << " Sorry, udot doesn't appear to be trimmable" << endl; cout << " Resetting throttles to zero" << endl; + setThrottlesPct(0); fdmex->GetFCS()->SetThrottleCmd(-1,0); + fdmex->RunIC(fgic); total_its=k; k=Ncycles; //force the trim to fail } diff --git a/src/Joystick/joystick.cxx b/src/Joystick/joystick.cxx index 18f81ac1f..b0daf424b 100644 --- a/src/Joystick/joystick.cxx +++ b/src/Joystick/joystick.cxx @@ -30,6 +30,8 @@ # include #endif +#include + #include #include