Fixes to jsbsim.

[flightgear.git] / src / FDM / JSBSim / FGState.cpp

diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp
index 6a60656ec22b2dad813429008a5c907f5f4781f0..285d844c09ecd9eb3145ad69a6378f354f11f466 100644 (file)
--- a/src/FDM/JSBSim/FGState.cpp
+++ b/src/FDM/JSBSim/FGState.cpp
@@ -47,6 +47,11 @@ INCLUDES
 #  include <cmath>
 #endif
 
+#ifndef M_PI         // support for silly Microsoft compiler
+#  include <simgear/constants.h>
+#  define M_PI FG_PI
+#endif 
+
 #include "FGState.h"
 #include "FGFDMExec.h"
 #include "FGAtmosphere.h"
@@ -66,7 +71,8 @@ INCLUDES
 FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
     mTl2b(3,3),
     mTs2b(3,3),
-vQtrn(4) {
+    vQtrn(4)
+{
   FDMExec = fdex;
 
   adot = bdot = 0.0;
@@ -316,9 +322,15 @@ float FGState::GetParameter(int val_idx) {
   case FG_ALTITUDE:
     return FDMExec->GetPosition()->Geth();
   case FG_BI2VEL:
-    return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * FDMExec->GetTranslation()->GetVt());
+    if(FDMExec->GetTranslation()->GetVt() > 0)
+        return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * FDMExec->GetTranslation()->GetVt());
+    else
+        return 0;
   case FG_CI2VEL:
-    return FDMExec->GetAircraft()->Getcbar()/(2.0 * FDMExec->GetTranslation()->GetVt());
+    if(FDMExec->GetTranslation()->GetVt() > 0)
+        return FDMExec->GetAircraft()->Getcbar()/(2.0 * FDMExec->GetTranslation()->GetVt());
+    else
+        return 0;
   case FG_THROTTLE_CMD:
     return FDMExec->GetFCS()->GetThrottleCmd(0);
   case FG_THROTTLE_POS:
@@ -412,18 +424,18 @@ void FGState::CalcMatrices(void) {
   Q1Q3 = vQtrn(2)*vQtrn(4);
   Q2Q3 = vQtrn(3)*vQtrn(4);
 
-  mTb2l(1,1) = Q0Q0 + Q1Q1 - Q2Q2 - Q3Q3;
-  mTb2l(1,2) = 2*(Q1Q2 + Q0Q3);
-  mTb2l(1,3) = 2*(Q1Q3 - Q0Q2);
-  mTb2l(2,1) = 2*(Q1Q2 - Q0Q3);
-  mTb2l(2,2) = Q0Q0 - Q1Q1 + Q2Q2 - Q3Q3;
-  mTb2l(2,3) = 2*(Q2Q3 + Q0Q1);
-  mTb2l(3,1) = 2*(Q1Q3 + Q0Q2);
-  mTb2l(3,2) = 2*(Q2Q3 - Q0Q1);
-  mTb2l(3,3) = Q0Q0 - Q1Q1 - Q2Q2 + Q3Q3;
-
-  mTl2b = mTb2l;
-  mTl2b.T();
+  mTl2b(1,1) = Q0Q0 + Q1Q1 - Q2Q2 - Q3Q3;
+  mTl2b(1,2) = 2*(Q1Q2 + Q0Q3);
+  mTl2b(1,3) = 2*(Q1Q3 - Q0Q2);
+  mTl2b(2,1) = 2*(Q1Q2 - Q0Q3);
+  mTl2b(2,2) = Q0Q0 - Q1Q1 + Q2Q2 - Q3Q3;
+  mTl2b(2,3) = 2*(Q2Q3 + Q0Q1);
+  mTl2b(3,1) = 2*(Q1Q3 + Q0Q2);
+  mTl2b(3,2) = 2*(Q2Q3 - Q0Q1);
+  mTl2b(3,3) = Q0Q0 - Q1Q1 - Q2Q2 + Q3Q3;
+
+  mTb2l = mTl2b;
+  mTb2l.T();
 }
 
 /******************************************************************************/
@@ -449,13 +461,12 @@ void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) {
 FGColumnVector FGState::CalcEuler(void) {
   static FGColumnVector vEuler(3);
 
-  if (mTb2l(3,3) == 0)    vEuler(ePhi) = 0.0;
-  else                    vEuler(ePhi) = atan2(mTb2l(2,3), mTb2l(3,3));
-
-  vEuler(eTht) = asin(-mTb2l(1,3));
+  if (mTl2b(3,3) == 0.0) mTl2b(3,3) = 0.0000001;
+  if (mTl2b(1,1) == 0.0) mTl2b(1,1) = 0.0000001;
 
-  if (mTb2l(1,1) == 0.0)  vEuler(ePsi) = 0.0;
-  else                    vEuler(ePsi) = atan2(mTb2l(1,2), mTb2l(1,1));
+  vEuler(ePhi) = atan2(mTl2b(2,3), mTl2b(3,3));
+  vEuler(eTht) = asin(-mTl2b(1,3));
+  vEuler(ePsi) = atan2(mTl2b(1,2), mTl2b(1,1));
 
   if (vEuler(ePsi) < 0.0) vEuler(ePsi) += 2*M_PI;