]> git.mxchange.org Git - flightgear.git/blobdiff - src/FDM/JSBSim/FGState.cpp
Fixes to jsbsim.
[flightgear.git] / src / FDM / JSBSim / FGState.cpp
index 2170072a53844485237902c342ca84ca18594db8..285d844c09ecd9eb3145ad69a6378f354f11f466 100644 (file)
@@ -4,34 +4,34 @@
  Author:       Jon Berndt
  Date started: 11/17/98
  Called by:    FGFDMExec and accessed by all models.
-
  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
-
  This program is free software; you can redistribute it and/or modify it under
  the terms of the GNU General Public License as published by the Free Software
  Foundation; either version 2 of the License, or (at your option) any later
  version.
-
  This program is distributed in the hope that it will be useful, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  details.
-
  You should have received a copy of the GNU General Public License along with
  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  Place - Suite 330, Boston, MA  02111-1307, USA.
-
  Further information about the GNU General Public License can also be found on
  the world wide web at http://www.gnu.org.
-
 FUNCTIONAL DESCRIPTION
 --------------------------------------------------------------------------------
 See header file.
-
 HISTORY
 --------------------------------------------------------------------------------
 11/17/98   JSB   Created
-
 ********************************************************************************
 INCLUDES
 *******************************************************************************/
@@ -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"
@@ -64,18 +69,14 @@ INCLUDES
 
 
 FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
-                                    mTl2b(3,3),
-                                    mTs2b(3,3),
-                                    vQtrn(4)
+    mTl2b(3,3),
+    mTs2b(3,3),
+    vQtrn(4)
 {
   FDMExec = fdex;
 
-  Vt = 0.0;
-  latitude = longitude = 0.0;
   adot = bdot = 0.0;
-  h = 0.0;
   a = 1000.0;
-  qbar = 0.0;
   sim_time = 0.0;
   dt = 1.0/120.0;
 
@@ -106,28 +107,28 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
   coeffdef["FG_SPDBRAKE_CMD"]  = 16777216L   ;
   coeffdef["FG_SPOILERS_CMD"]  = 33554432L   ;
   coeffdef["FG_FLAPS_CMD"]     = 67108864L   ;
-  coeffdef["FG_SPARE3"]        = 134217728L  ;
-  coeffdef["FG_SPARE4"]        = 268435456L  ;
-  coeffdef["FG_SPARE5"]        = 536870912L  ;
-  coeffdef["FG_SPARE6"]        = 1073741824L ;
+  coeffdef["FG_THROTTLE_CMD"]  = 134217728L  ;
+  coeffdef["FG_THROTTLE_POS"]  = 268435456L  ;
+  coeffdef["FG_HOVERB"]        = 536870912L  ;
+  coeffdef["FG_PITCH_TRIM_CMD"] = 1073741824L ;
 }
 
 /******************************************************************************/
 
-FGState::~FGState(void)
-{
-}
+FGState::~FGState(void) {}
 
 //***************************************************************************
 //
 // Reset: Assume all angles READ FROM FILE IN DEGREES !!
 //
 
-bool FGState::Reset(string path, string acname, string fname)
-{
+
+
+bool FGState::Reset(string path, string acname, string fname) {
   string resetDef;
   float U, V, W;
   float phi, tht, psi;
+  float latitude, longitude, h;
 
   resetDef = path + "/" + acname + "/" + fname;
 
@@ -145,6 +146,10 @@ bool FGState::Reset(string path, string acname, string fname)
     resetfile >> h;
     resetfile.close();
 
+    FDMExec->GetPosition()->SetLatitude(latitude*DEGTORAD);
+    FDMExec->GetPosition()->SetLongitude(longitude*DEGTORAD);
+    FDMExec->GetPosition()->Seth(h);
+
     Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD,
                latitude*DEGTORAD, longitude*DEGTORAD, h);
 
@@ -162,18 +167,19 @@ bool FGState::Reset(string path, string acname, string fname)
 
 void FGState::Initialize(float U, float V, float W,
                          float phi, float tht, float psi,
-                         float Latitude, float Longitude, float H)
-{
+                         float Latitude, float Longitude, float H) {
   FGColumnVector vUVW(3);
+  FGColumnVector vLocalVelNED(3);
   FGColumnVector vEuler(3);
-  float alpha, beta, gamma;
+  float alpha, beta;
+  float qbar, Vt;
+
+  FDMExec->GetPosition()->SetLatitude(Latitude);
+  FDMExec->GetPosition()->SetLongitude(Longitude);
+  FDMExec->GetPosition()->Seth(H);
 
-  latitude = Latitude;
-  longitude = Longitude;
-  h = H;
   FDMExec->GetAtmosphere()->Run();
 
-  gamma = 0.0;
   if (W != 0.0)
     alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
   else
@@ -185,23 +191,31 @@ void FGState::Initialize(float U, float V, float W,
 
   vUVW << U << V << W;
   FDMExec->GetTranslation()->SetUVW(vUVW);
+
   vEuler << phi << tht << psi;
   FDMExec->GetRotation()->SetEuler(vEuler);
-  FDMExec->GetTranslation()->SetABG(alpha, beta, gamma);
+
+  FDMExec->GetTranslation()->SetAB(alpha, beta);
 
   Vt = sqrt(U*U + V*V + W*W);
+  FDMExec->GetTranslation()->SetVt(Vt);
+
   qbar = 0.5*(U*U + V*V + W*W)*FDMExec->GetAtmosphere()->GetDensity();
+  FDMExec->GetTranslation()->Setqbar(qbar);
 
   InitMatrices(phi, tht, psi);
+
+  vLocalVelNED = mTb2l*vUVW;
+  FDMExec->GetPosition()->SetvVel(vLocalVelNED);
 }
 
 /******************************************************************************/
 
-void FGState::Initialize(FGInitialCondition *FGIC)
-{
+void FGState::Initialize(FGInitialCondition *FGIC) {
 
   float tht,psi,phi;
-  float U,V,W;
+  float U, V, W, h;
+  float latitude, longitude;
 
   latitude = FGIC->GetLatitudeRadIC();
   longitude = FGIC->GetLongitudeRadIC();
@@ -218,20 +232,19 @@ void FGState::Initialize(FGInitialCondition *FGIC)
 
 /******************************************************************************/
 
-bool FGState::StoreData(string fname)
-{
+bool FGState::StoreData(string fname) {
   ofstream datafile(fname.c_str());
 
   if (datafile) {
     datafile << (FDMExec->GetTranslation()->GetUVW())(1);
     datafile << (FDMExec->GetTranslation()->GetUVW())(2);
     datafile << (FDMExec->GetTranslation()->GetUVW())(3);
-    datafile << latitude;
-    datafile << longitude;
+    datafile << FDMExec->GetPosition()->GetLatitude();
+    datafile << FDMExec->GetPosition()->GetLongitude();
     datafile << (FDMExec->GetRotation()->GetEuler())(1);
     datafile << (FDMExec->GetRotation()->GetEuler())(2);
     datafile << (FDMExec->GetRotation()->GetEuler())(3);
-    datafile << h;
+    datafile << FDMExec->GetPosition()->Geth();
     datafile.close();
     return true;
   } else {
@@ -242,15 +255,13 @@ bool FGState::StoreData(string fname)
 
 /******************************************************************************/
 
-float FGState::GetParameter(string val_string)
-{
+float FGState::GetParameter(string val_string) {
   return GetParameter(coeffdef[val_string]);
 }
 
 /******************************************************************************/
 
-int FGState::GetParameterIndex(string val_string)
-{
+int FGState::GetParameterIndex(string val_string) {
   return coeffdef[val_string];
 }
 
@@ -258,11 +269,10 @@ int FGState::GetParameterIndex(string val_string)
 //
 // NEED WORK BELOW TO ADD NEW PARAMETERS !!!
 //
-float FGState::GetParameter(int val_idx)
-{
+float FGState::GetParameter(int val_idx) {
   switch(val_idx) {
   case FG_QBAR:
-    return Getqbar();
+    return FDMExec->GetTranslation()->Getqbar();
   case FG_WINGAREA:
     return FDMExec->GetAircraft()->GetWingArea();
   case FG_WINGSPAN:
@@ -308,21 +318,34 @@ float FGState::GetParameter(int val_idx)
   case FG_FLAPS_CMD:
     return FDMExec->GetFCS()->GetDfCmd();
   case FG_MACH:
-    return GetMach();
+    return FDMExec->GetTranslation()->GetMach();
   case FG_ALTITUDE:
-    return Geth();
+    return FDMExec->GetPosition()->Geth();
   case FG_BI2VEL:
-    return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * 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 * 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:
+    return FDMExec->GetFCS()->GetThrottlePos(0);
+  case FG_HOVERB:
+    return FDMExec->GetPosition()->GetHOverB();
+  case FG_PITCH_TRIM_CMD:
+    return FDMExec->GetFCS()->GetPitchTrimCmd();
   }
   return 0;
 }
 
 /******************************************************************************/
 
-void FGState::SetParameter(int val_idx, float val)
-{
+void FGState::SetParameter(int val_idx, float val) {
   switch(val_idx) {
   case FG_ELEVATOR_POS:
     FDMExec->GetFCS()->SetDePos(val);
@@ -334,21 +357,22 @@ void FGState::SetParameter(int val_idx, float val)
     FDMExec->GetFCS()->SetDrPos(val);
     break;
   case FG_SPDBRAKE_POS:
-    FDMExec->GetFCS()->SetDrPos(val);
+    FDMExec->GetFCS()->SetDsbPos(val);
     break;
   case FG_SPOILERS_POS:
-    FDMExec->GetFCS()->SetDrPos(val);
+    FDMExec->GetFCS()->SetDspPos(val);
     break;
   case FG_FLAPS_POS:
-    FDMExec->GetFCS()->SetDrPos(val);
+    FDMExec->GetFCS()->SetDfPos(val);
     break;
+  case FG_THROTTLE_POS:
+    FDMExec->GetFCS()->SetThrottlePos(-1,val);
   }
 }
 
 /******************************************************************************/
 
-void FGState::InitMatrices(float phi, float tht, float psi)
-{
+void FGState::InitMatrices(float phi, float tht, float psi) {
   float thtd2, psid2, phid2;
   float Sthtd2, Spsid2, Sphid2;
   float Cthtd2, Cpsid2, Cphid2;
@@ -384,8 +408,7 @@ void FGState::InitMatrices(float phi, float tht, float psi)
 
 /******************************************************************************/
 
-void FGState::CalcMatrices(void)
-{
+void FGState::CalcMatrices(void) {
   float Q0Q0, Q1Q1, Q2Q2, Q3Q3;
   float Q0Q1, Q0Q2, Q0Q3, Q1Q2;
   float Q1Q3, Q2Q3;
@@ -401,24 +424,23 @@ 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();
 }
 
 /******************************************************************************/
 
-void FGState::IntegrateQuat(FGColumnVector vPQR, int rate)
-{
+void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) {
   static FGColumnVector vlastQdot(4);
   static FGColumnVector vQdot(4);
 
@@ -436,17 +458,15 @@ void FGState::IntegrateQuat(FGColumnVector vPQR, int rate)
 
 /******************************************************************************/
 
-FGColumnVector FGState::CalcEuler(void)
-{
+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));
+  if (mTl2b(3,3) == 0.0) mTl2b(3,3) = 0.0000001;
+  if (mTl2b(1,1) == 0.0) mTl2b(1,1) = 0.0000001;
 
-  vEuler(eTht) = asin(-mTb2l(1,3));
-
-  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;
 
@@ -455,8 +475,7 @@ FGColumnVector FGState::CalcEuler(void)
 
 /******************************************************************************/
 
-FGMatrix FGState::GetTs2b(float alpha, float beta)
-{
+FGMatrix FGState::GetTs2b(float alpha, float beta) {
   float ca, cb, sa, sb;
 
   ca = cos(alpha);
@@ -467,7 +486,7 @@ FGMatrix FGState::GetTs2b(float alpha, float beta)
   mTs2b(1,1) = -ca*cb;
   mTs2b(1,2) = -ca*sb;
   mTs2b(1,3) = sa;
-  mTs2b(2,1) = sb;
+  mTs2b(2,1) = -sb;
   mTs2b(2,2) = cb;
   mTs2b(2,3) = 0.0;
   mTs2b(3,1) = -sa*cb;