src/FDM/JSBSim/FGState.cpp

   1 /*******************************************************************************
   2
   3  Module:       FGState.cpp
   4  Author:       Jon Berndt
   5  Date started: 11/17/98
   6  Called by:    FGFDMExec and accessed by all models.
   7
   8  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
   9
  10  This program is free software; you can redistribute it and/or modify it under
  11  the terms of the GNU General Public License as published by the Free Software
  12  Foundation; either version 2 of the License, or (at your option) any later
  13  version.
  14
  15  This program is distributed in the hope that it will be useful, but WITHOUT
  16  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  17  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  18  details.
  19
  20  You should have received a copy of the GNU General Public License along with
  21  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  22  Place - Suite 330, Boston, MA  02111-1307, USA.
  23
  24  Further information about the GNU General Public License can also be found on
  25  the world wide web at http://www.gnu.org.
  26
  27 FUNCTIONAL DESCRIPTION
  28 --------------------------------------------------------------------------------
  29 See header file.
  30
  31 HISTORY
  32 --------------------------------------------------------------------------------
  33 11/17/98   JSB   Created
  34
  35 ********************************************************************************
  36 INCLUDES
  37 *******************************************************************************/
  38
  39 #ifdef FGFS
  40 #  include <Include/compiler.h>
  41 #  ifdef FG_HAVE_STD_INCLUDES
  42 #    include <cmath>
  43 #  else
  44 #    include <math.h>
  45 #  endif
  46 #else
  47 #  include <cmath>
  48 #endif
  49
  50 #include "FGState.h"
  51 #include "FGFDMExec.h"
  52 #include "FGAtmosphere.h"
  53 #include "FGFCS.h"
  54 #include "FGAircraft.h"
  55 #include "FGTranslation.h"
  56 #include "FGRotation.h"
  57 #include "FGPosition.h"
  58 #include "FGAuxiliary.h"
  59 #include "FGOutput.h"
  60
  61 /*******************************************************************************
  62 ************************************ CODE **************************************
  63 *******************************************************************************/
  64
  65
  66 FGState::FGState(FGFDMExec* fdex)
  67 {
  68   FDMExec = fdex;
  69
  70   Vt = 0.0;
  71   latitude = longitude = 0.0;
  72   adot = bdot = 0.0;
  73   h = 0.0;
  74   a = 1000.0;
  75   qbar = 0.0;
  76   sim_time = dt = 0.1;
  77 }
  78
  79
  80 FGState::~FGState(void)
  81 {
  82 }
  83
  84
  85 bool FGState::Reset(string path, string fname)
  86 {
  87   string resetDef;
  88   float U, V, W;
  89   float phi, tht, psi;
  90   float alpha, beta, gamma;
  91   float Q0, Q1, Q2, Q3;
  92   float T[4][4];
  93
  94   resetDef = path + "/" + FDMExec->GetAircraft()->GetAircraftName() + "/" + fname;
  95
  96   ifstream resetfile(resetDef.c_str());
  97
  98   if (resetfile) {
  99     resetfile >> U;
 100     resetfile >> V;
 101     resetfile >> W;
 102     resetfile >> latitude;
 103     resetfile >> longitude;
 104     resetfile >> phi;
 105     resetfile >> tht;
 106     resetfile >> psi;
 107     resetfile >> h;
 108     resetfile.close();
 109
 110     Initialize(U, V, W, phi, tht, psi, latitude, longitude, h);
 111
 112     return true;
 113   } else {
 114     cerr << "Unable to load reset file " << fname << endl;
 115     return false;
 116   }
 117 }
 118
 119
 120 void FGState::Initialize(float U, float V, float W,
 121                          float phi, float tht, float psi,
 122                          float Latitude, float Longitude, float H)
 123 {
 124   float alpha, beta, gamma;
 125   float Q0, Q1, Q2, Q3;
 126   float T[4][4];
 127
 128   latitude = Latitude;
 129   longitude = Longitude;
 130   h = H;
 131
 132 // Change all angular measurements from degrees (as in config file) to radians
 133
 134   gamma = 0.0;
 135   if (W != 0.0)
 136     alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
 137   else
 138     alpha = 0.0;
 139   if (V != 0.0)
 140     beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
 141   else
 142     beta = 0.0;
 143
 144   latitude  *= M_PI / 180.0;
 145   longitude *= M_PI / 180.0;
 146   phi       *= M_PI / 180.0;
 147   tht       *= M_PI / 180.0;
 148   psi       *= M_PI / 180.0;
 149
 150   FDMExec->GetTranslation()->SetUVW(U, V, W);
 151   FDMExec->GetRotation()->SetEuler(phi, tht, psi);
 152   FDMExec->GetTranslation()->SetABG(alpha, beta, gamma);
 153
 154   Vt = sqrt(U*U + V*V + W*W);
 155   qbar = 0.5*(U*U + V*V + W*W)*FDMExec->GetAtmosphere()->CalcRho(h);
 156
 157   Q0 =  sin(psi*0.5)*sin(tht*0.5)*sin(phi*0.5) + cos(psi*0.5)*cos(tht*0.5)*cos(phi*0.5);
 158   Q1 = -sin(psi*0.5)*sin(tht*0.5)*cos(phi*0.5) + cos(psi*0.5)*cos(tht*0.5)*sin(phi*0.5);
 159   Q2 =  sin(psi*0.5)*cos(tht*0.5)*sin(phi*0.5) + cos(psi*0.5)*sin(tht*0.5)*cos(phi*0.5);
 160   Q3 =  sin(psi*0.5)*cos(tht*0.5)*cos(phi*0.5) - cos(psi*0.5)*sin(tht*0.5)*sin(phi*0.5);
 161
 162   FDMExec->GetRotation()->SetQ0123(Q0, Q1, Q2, Q3);
 163
 164   T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3;
 165   T[1][2] = 2*(Q1*Q2 + Q0*Q3);
 166   T[1][3] = 2*(Q1*Q3 - Q0*Q2);
 167   T[2][1] = 2*(Q1*Q2 - Q0*Q3);
 168   T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3;
 169   T[2][3] = 2*(Q2*Q3 + Q0*Q1);
 170   T[3][1] = 2*(Q1*Q3 + Q0*Q2);
 171   T[3][2] = 2*(Q2*Q3 - Q0*Q1);
 172   T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3;
 173
 174   FDMExec->GetPosition()->SetT(T[1][1], T[1][2], T[1][3],
 175                                T[2][1], T[2][2], T[2][3],
 176                                T[3][1], T[3][2], T[3][3]);
 177   DisplayData();
 178 }
 179
 180
 181 bool FGState::StoreData(string fname)
 182 {
 183   ofstream datafile(fname.c_str());
 184
 185   if (datafile) {
 186     datafile << FDMExec->GetTranslation()->GetU();
 187     datafile << FDMExec->GetTranslation()->GetV();
 188     datafile << FDMExec->GetTranslation()->GetW();
 189     datafile << latitude;
 190     datafile << longitude;
 191     datafile << FDMExec->GetRotation()->Getphi();
 192     datafile << FDMExec->GetRotation()->Gettht();
 193     datafile << FDMExec->GetRotation()->Getpsi();
 194     datafile << h;
 195     datafile.close();
 196     return true;
 197   } else {
 198     cerr << "Could not open dump file " << fname << endl;
 199     return false;
 200   }
 201 }
 202
 203
 204 bool FGState::DumpData(string fname)
 205 {
 206   ofstream datafile(fname.c_str());
 207
 208   if (datafile) {
 209     datafile << "U: " << FDMExec->GetTranslation()->GetU() << endl;
 210     datafile << "V: " << FDMExec->GetTranslation()->GetV() << endl;
 211     datafile << "W: " << FDMExec->GetTranslation()->GetW() << endl;
 212     datafile << "P: " << FDMExec->GetRotation()->GetP() << endl;
 213     datafile << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
 214     datafile << "R: " << FDMExec->GetRotation()->GetR() << endl;
 215     datafile << "L: " << FDMExec->GetAircraft()->GetL() << endl;
 216     datafile << "M: " << FDMExec->GetAircraft()->GetM() << endl;
 217     datafile << "N: " << FDMExec->GetAircraft()->GetN() << endl;
 218     datafile << "latitude: " << latitude << endl;
 219     datafile << "longitude: " << longitude << endl;
 220     datafile << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
 221     datafile << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
 222     datafile << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
 223     datafile << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
 224     datafile << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
 225     datafile << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
 226     datafile << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
 227     datafile << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
 228     datafile << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
 229     datafile << "h: " << h << endl;
 230     datafile << "a: " << a << endl;
 231     datafile << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
 232     datafile << "qbar: " << qbar << endl;
 233     datafile << "sim_time: " << sim_time << endl;
 234     datafile << "dt: " << dt << endl;
 235     datafile << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
 236     datafile.close();
 237     return true;
 238   } else {
 239     return false;
 240   }
 241 }
 242
 243
 244 bool FGState::DisplayData(void)
 245 {
 246   cout << "U: " << FDMExec->GetTranslation()->GetU() << endl;
 247   cout << "V: " << FDMExec->GetTranslation()->GetV() << endl;
 248   cout << "W: " << FDMExec->GetTranslation()->GetW() << endl;
 249   cout << "P: " << FDMExec->GetRotation()->GetP() << endl;
 250   cout << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
 251   cout << "R: " << FDMExec->GetRotation()->GetR() << endl;
 252   cout << "L: " << FDMExec->GetAircraft()->GetL() << endl;
 253   cout << "M: " << FDMExec->GetAircraft()->GetM() << endl;
 254   cout << "N: " << FDMExec->GetAircraft()->GetN() << endl;
 255   cout << "Vt: " << Vt << endl;
 256   cout << "latitude: " << latitude << endl;
 257   cout << "longitude: " << longitude << endl;
 258   cout << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
 259   cout << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
 260   cout << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
 261   cout << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
 262   cout << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
 263   cout << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
 264   cout << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
 265   cout << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
 266   cout << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
 267   cout << "h: " << h << endl;
 268   cout << "a: " << a << endl;
 269   cout << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
 270   cout << "qbar: " << qbar << endl;
 271   cout << "sim_time: " << sim_time << endl;
 272   cout << "dt: " << dt << endl;
 273   cout << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
 274
 275   return true;
 276 }