From 76b4de7f1fd97e042834ca711abdfabb154be9ea Mon Sep 17 00:00:00 2001 From: curt Date: Mon, 31 Jul 2000 15:05:08 +0000 Subject: [PATCH] Updates to JSBSim. --- src/FDM/JSBSim.cxx | 46 ++++++++++++++++++++++++++--------- src/FDM/JSBSim/FGPosition.cpp | 4 ++- src/FDM/JSBSim/FGPosition.h | 7 +++++- src/FDM/JSBSim/FGRotation.cpp | 19 ++++++++++++++- src/FDM/JSBSim/FGRotation.h | 20 ++++++++++++++- src/FDM/flight.hxx | 34 +++++++++++++------------- 6 files changed, 98 insertions(+), 32 deletions(-) diff --git a/src/FDM/JSBSim.cxx b/src/FDM/JSBSim.cxx index b3303625e..bca731b59 100644 --- a/src/FDM/JSBSim.cxx +++ b/src/FDM/JSBSim.cxx @@ -256,6 +256,11 @@ 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) ); @@ -264,17 +269,18 @@ int FGJSBsim::copy_from_JSBsim() { FDMExec.GetTranslation()->GetUVWdot()(2), FDMExec.GetTranslation()->GetUVWdot()(3) ); - set_Accels_CG_Body_N ( FDMExec.GetTranslation()->GetNcg()(1), - FDMExec.GetTranslation()->GetNcg()(2), - FDMExec.GetTranslation()->GetNcg()(3) ); + //set_Accels_CG_Body_N ( FDMExec.GetTranslation()->GetNcg()(1), + // FDMExec.GetTranslation()->GetNcg()(2), + // FDMExec.GetTranslation()->GetNcg()(3) ); + // set_Accels_Pilot_Body( FDMExec.GetAuxiliary()->GetPilotAccel()(1), FDMExec.GetAuxiliary()->GetPilotAccel()(2), FDMExec.GetAuxiliary()->GetPilotAccel()(3) ); - set_Accels_Pilot_Body_N( FDMExec.GetAuxiliary()->GetNpilot()(1), - FDMExec.GetAuxiliary()->GetNpilot()(2), - FDMExec.GetAuxiliary()->GetNpilot()(3) ); + //set_Accels_Pilot_Body_N( FDMExec.GetAuxiliary()->GetNpilot()(1), + // FDMExec.GetAuxiliary()->GetNpilot()(2), + // FDMExec.GetAuxiliary()->GetNpilot()(3) ); @@ -304,11 +310,13 @@ int FGJSBsim::copy_from_JSBsim() { FDMExec.GetState()->GetParameter(FG_PITCHRATE), FDMExec.GetState()->GetParameter(FG_YAWRATE) ); - /* HUH!?! */ set_Euler_Rates( FDMExec.GetRotation()->Getphi(), - FDMExec.GetRotation()->Gettht(), - FDMExec.GetRotation()->Getpsi() ); + set_Euler_Rates( FDMExec.GetRotation()->GetEulerRates()(1), + FDMExec.GetRotation()->GetEulerRates()(2), + FDMExec.GetRotation()->GetEulerRates()(3) ); - // ***FIXME*** set_Geocentric_Rates( Latitude_dot, Longitude_dot, Radius_dot ); + set_Geocentric_Rates( FDMExec.GetPosition()->GetLatitudeDot(), + FDMExec.GetPosition()->GetLongitudeDot(), + FDMExec.GetPosition()->Gethdot() ); set_Mach_number( FDMExec.GetTranslation()->GetMach()); @@ -339,10 +347,26 @@ int FGJSBsim::copy_from_JSBsim() { 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() ); + + + /* **FIXME*** */ set_Sea_level_radius( sl_radius2 * METER_TO_FEET ); /* **FIXME*** */ set_Earth_position_angle( 0.0 ); diff --git a/src/FDM/JSBSim/FGPosition.cpp b/src/FDM/JSBSim/FGPosition.cpp index 7805e1293..bc73e33f5 100644 --- a/src/FDM/JSBSim/FGPosition.cpp +++ b/src/FDM/JSBSim/FGPosition.cpp @@ -88,7 +88,8 @@ extern double globalSeaLevelRadius; FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex), vUVW(3), - vVel(3) + vVel(3), + vVelDot(3) { Name = "FGPosition"; LongitudeDot = LatitudeDot = RadiusDot = 0.0; @@ -169,6 +170,7 @@ void FGPosition::GetState(void) { vUVW = Translation->GetUVW(); Vt = Translation->GetVt(); vVel = State->GetTb2l()*vUVW; + vVelDot = State->GetTb2l() * Translation->GetUVWdot(); b = Aircraft->GetWingSpan(); diff --git a/src/FDM/JSBSim/FGPosition.h b/src/FDM/JSBSim/FGPosition.h index 07a9a6c85..eaa7e9dec 100644 --- a/src/FDM/JSBSim/FGPosition.h +++ b/src/FDM/JSBSim/FGPosition.h @@ -50,9 +50,11 @@ CLASS DECLARATION *******************************************************************************/ class FGPosition : public FGModel { + FGColumnVector vUVW; FGColumnVector vVel; - + FGColumnVector vVelDot; + double Vee, invMass, invRadius; double Radius, h; double LatitudeDot, LongitudeDot, RadiusDot; @@ -73,6 +75,7 @@ public: ~FGPosition(void); inline FGColumnVector GetVel(void) { return vVel; } + inline FGColumnVector GetVelDot(void) { return vVelDot; } inline FGColumnVector GetUVW(void) { return vUVW; } inline double GetVn(void) { return vVel(eX); } inline double GetVe(void) { return vVel(eY); } @@ -81,7 +84,9 @@ public: inline double Geth(void) { return h; } inline double Gethdot(void) { return RadiusDot; } inline double GetLatitude(void) { return Latitude; } + inline double GetLatitudeDot(void) { return LatitudeDot; } inline double GetLongitude(void) { return Longitude; } + inline double GetLongitudeDot(void) { return LongitudeDot; } inline double GetRunwayRadius(void) { return RunwayRadius; } inline double GetDistanceAGL(void) { return DistanceAGL; } inline double GetGamma(void) { return gamma; } diff --git a/src/FDM/JSBSim/FGRotation.cpp b/src/FDM/JSBSim/FGRotation.cpp index 6041ef4ec..327be4310 100644 --- a/src/FDM/JSBSim/FGRotation.cpp +++ b/src/FDM/JSBSim/FGRotation.cpp @@ -75,9 +75,12 @@ FGRotation::FGRotation(FGFDMExec* fdmex) : FGModel(fdmex), vPQR(3), vPQRdot(3), vEuler(3), + vEulerRates(3), vMoments(3) { Name = "FGRotation"; + cTht=cPhi=cPsi=1.0; + sTht=sPhi=sPsi=0.0; } /******************************************************************************/ @@ -91,6 +94,7 @@ FGRotation::~FGRotation(void) bool FGRotation::Run(void) { float L2, N1; + float tTheta; static FGColumnVector vlastPQRdot(3); if (!FGModel::Run()) { @@ -108,7 +112,20 @@ bool FGRotation::Run(void) State->IntegrateQuat(vPQR, rate); State->CalcMatrices(); vEuler = State->CalcEuler(); - + + + cTht=cos(vEuler(eTht)); sTht=sin(vEuler(eTht)); + cPhi=cos(vEuler(ePhi)); sPhi=sin(vEuler(ePhi)); + cPsi=cos(vEuler(ePsi)); sPsi=sin(vEuler(ePsi)); + + + vEulerRates(eTht) = vPQR(2)*cPhi - vPQR(3)*sPhi; + if(cTht != 0.0) { + tTheta=sTht/cTht; // what's cheaper: / or tan() ? + vEulerRates(ePhi) = vPQR(1) + (vPQR(2)*sPhi + vPQR(3)*cPhi)*tTheta; + vEulerRates(ePsi) = (vPQR(2)*sPhi + vPQR(3)*cPhi)/cTht; + } + vlastPQRdot = vPQRdot; } else { diff --git a/src/FDM/JSBSim/FGRotation.h b/src/FDM/JSBSim/FGRotation.h index d2fb58b6f..f40d54930 100644 --- a/src/FDM/JSBSim/FGRotation.h +++ b/src/FDM/JSBSim/FGRotation.h @@ -85,7 +85,12 @@ class FGRotation : public FGModel FGColumnVector vPQRdot; FGColumnVector vMoments; FGColumnVector vEuler; - + FGColumnVector vEulerRates; + + float cTht,sTht; + float cPhi,sPhi; + float cPsi,sPsi; + float Ixx, Iyy, Izz, Ixz; float dt; @@ -100,11 +105,24 @@ public: inline FGColumnVector GetPQR(void) {return vPQR;} inline FGColumnVector GetPQRdot(void) {return vPQRdot;} inline FGColumnVector GetEuler(void) {return vEuler;} + inline FGColumnVector GetEulerRates(void) { return vEulerRates; } inline void SetPQR(FGColumnVector tt) {vPQR = tt;} inline void SetEuler(FGColumnVector tt) {vEuler = tt;} + inline float Getphi(void) {return vEuler(1);} inline float Gettht(void) {return vEuler(2);} inline float Getpsi(void) {return vEuler(3);} + + inline float GetCosphi(void) {return cPhi;} + inline float GetCostht(void) {return cTht;} + inline float GetCospsi(void) {return cPsi;} + + inline float GetSinphi(void) {return sPhi;} + inline float GetSintht(void) {return sTht;} + inline float GetSinpsi(void) {return sPsi;} + + + }; /******************************************************************************/ diff --git a/src/FDM/flight.hxx b/src/FDM/flight.hxx index efd04db45..dad9bacd8 100644 --- a/src/FDM/flight.hxx +++ b/src/FDM/flight.hxx @@ -341,26 +341,26 @@ public: } FG_VECTOR_3 n_cg_body_v; - inline double * get_N_cg_body_v() { return n_cg_body_v; } - inline double get_N_X_cg() const { return n_cg_body_v[0]; } - inline double get_N_Y_cg() const { return n_cg_body_v[1]; } - inline double get_N_Z_cg() const { return n_cg_body_v[2]; } - inline void set_Accels_CG_Body_N( double x, double y, double z ) { - n_cg_body_v[0] = x; - n_cg_body_v[1] = y; - n_cg_body_v[2] = z; - } + // inline double * get_N_cg_body_v() { return n_cg_body_v; } + // inline double get_N_X_cg() const { return n_cg_body_v[0]; } + // inline double get_N_Y_cg() const { return n_cg_body_v[1]; } + // inline double get_N_Z_cg() const { return n_cg_body_v[2]; } + // inline void set_Accels_CG_Body_N( double x, double y, double z ) { + // n_cg_body_v[0] = x; + // n_cg_body_v[1] = y; + // n_cg_body_v[2] = z; + // } FG_VECTOR_3 n_pilot_body_v; // inline double * get_N_pilot_body_v() { return n_pilot_body_v; } - inline double get_N_X_pilot() const { return n_pilot_body_v[0]; } - inline double get_N_Y_pilot() const { return n_pilot_body_v[1]; } - inline double get_N_Z_pilot() const { return n_pilot_body_v[2]; } - inline void set_Accels_Pilot_Body_N( double x, double y, double z ) { - n_pilot_body_v[0] = x; - n_pilot_body_v[1] = y; - n_pilot_body_v[2] = z; - } + // inline double get_N_X_pilot() const { return n_pilot_body_v[0]; } + // inline double get_N_Y_pilot() const { return n_pilot_body_v[1]; } + // inline double get_N_Z_pilot() const { return n_pilot_body_v[2]; } + // inline void set_Accels_Pilot_Body_N( double x, double y, double z ) { + // n_pilot_body_v[0] = x; + // n_pilot_body_v[1] = y; + // n_pilot_body_v[2] = z; + // } double nlf; //Normal Load Factor double get_Nlf(void) { return nlf; } -- 2.39.5