Updates to JSBSim.

diff --git a/src/FDM/JSBSim.cxx b/src/FDM/JSBSim.cxx
index b3303625e1b8d3a62b561cadcbdf9543b396620e..bca731b59ccc39b90f45866977ce3b170212e6ca 100644 (file)
--- 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 7805e1293cdcac54488409bc7d618863ebaa19b5..bc73e33f55d27230f09d285ed6cbb4f421872f5c 100644 (file)
--- 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 07a9a6c856a9a14aafa519eee6b72c71440c8e84..eaa7e9dec3d0257d20219f42fd29a132f0e42468 100644 (file)
--- 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 6041ef4ec451926fb83ca415d5ce68a4a93cbb8b..327be431093915cd67720e305ed94a514793d9a6 100644 (file)
--- 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 d2fb58b6f31782373c9110fd730db86df8d37008..f40d549307f428edd5fd987c8089dcdb486f91ab 100644 (file)
--- 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 efd04db45ea47bae6dd00feae40918db2183ca86..dad9bacd80eb30b2eb4205ef0727e7d6e0f133fd 100644 (file)
--- 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; }