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 // Change all angular measurements from degrees (as in config file) to radians
 111
 112     gamma = 0.0;
 113     if (W != 0.0)
 114       alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
 115     else
 116       alpha = 0.0;
 117     if (V != 0.0)
 118       beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
 119     else
 120       beta = 0.0;
 121
 122     latitude  *= M_PI / 180.0;
 123     longitude *= M_PI / 180.0;
 124     phi       *= M_PI / 180.0;
 125     tht       *= M_PI / 180.0;
 126     psi       *= M_PI / 180.0;
 127
 128     FDMExec->GetTranslation()->SetUVW(U, V, W);
 129     FDMExec->GetRotation()->SetEuler(phi, tht, psi);
 130     FDMExec->GetTranslation()->SetABG(alpha, beta, gamma);
 131
 132     Vt = sqrt(U*U + V*V + W*W);
 133     qbar = 0.5*(U*U + V*V + W*W)*FDMExec->GetAtmosphere()->CalcRho(h);
 134
 135     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);
 136     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);
 137     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);
 138     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);
 139
 140     FDMExec->GetRotation()->SetQ0123(Q0, Q1, Q2, Q3);
 141
 142     T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3;
 143     T[1][2] = 2*(Q1*Q2 + Q0*Q3);
 144     T[1][3] = 2*(Q1*Q3 - Q0*Q2);
 145     T[2][1] = 2*(Q1*Q2 - Q0*Q3);
 146     T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3;
 147     T[2][3] = 2*(Q2*Q3 + Q0*Q1);
 148     T[3][1] = 2*(Q1*Q3 + Q0*Q2);
 149     T[3][2] = 2*(Q2*Q3 - Q0*Q1);
 150     T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3;
 151
 152     FDMExec->GetPosition()->SetT(T[1][1], T[1][2], T[1][3],
 153                                  T[2][1], T[2][2], T[2][3],
 154                                  T[3][1], T[3][2], T[3][3]);
 155
 156     return true;
 157   } else {
 158     cerr << "Unable to load reset file " << fname << endl;
 159     return false;
 160   }
 161 }
 162
 163
 164 bool FGState::StoreData(string fname)
 165 {
 166   ofstream datafile(fname.c_str());
 167
 168   if (datafile) {
 169     datafile << FDMExec->GetTranslation()->GetU();
 170     datafile << FDMExec->GetTranslation()->GetV();
 171     datafile << FDMExec->GetTranslation()->GetW();
 172     datafile << latitude;
 173     datafile << longitude;
 174     datafile << FDMExec->GetRotation()->Getphi();
 175     datafile << FDMExec->GetRotation()->Gettht();
 176     datafile << FDMExec->GetRotation()->Getpsi();
 177     datafile << h;
 178     datafile.close();
 179     return true;
 180   } else {
 181     cerr << "Could not open dump file " << fname << endl;
 182     return false;
 183   }
 184 }
 185
 186
 187 bool FGState::DumpData(string fname)
 188 {
 189   ofstream datafile(fname.c_str());
 190
 191   if (datafile) {
 192     datafile << "U: " << FDMExec->GetTranslation()->GetU() << endl;
 193     datafile << "V: " << FDMExec->GetTranslation()->GetV() << endl;
 194     datafile << "W: " << FDMExec->GetTranslation()->GetW() << endl;
 195     datafile << "P: " << FDMExec->GetRotation()->GetP() << endl;
 196     datafile << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
 197     datafile << "R: " << FDMExec->GetRotation()->GetR() << endl;
 198     datafile << "L: " << FDMExec->GetAircraft()->GetL() << endl;
 199     datafile << "M: " << FDMExec->GetAircraft()->GetM() << endl;
 200     datafile << "N: " << FDMExec->GetAircraft()->GetN() << endl;
 201     datafile << "latitude: " << latitude << endl;
 202     datafile << "longitude: " << longitude << endl;
 203     datafile << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
 204     datafile << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
 205     datafile << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
 206     datafile << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
 207     datafile << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
 208     datafile << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
 209     datafile << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
 210     datafile << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
 211     datafile << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
 212     datafile << "h: " << h << endl;
 213     datafile << "a: " << a << endl;
 214     datafile << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
 215     datafile << "qbar: " << qbar << endl;
 216     datafile << "sim_time: " << sim_time << endl;
 217     datafile << "dt: " << dt << endl;
 218     datafile << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
 219     datafile.close();
 220     return true;
 221   } else {
 222     return false;
 223   }
 224 }
 225
 226
 227 bool FGState::DisplayData(void)
 228 {
 229   cout << "U: " << FDMExec->GetTranslation()->GetU() << endl;
 230   cout << "V: " << FDMExec->GetTranslation()->GetV() << endl;
 231   cout << "W: " << FDMExec->GetTranslation()->GetW() << endl;
 232   cout << "P: " << FDMExec->GetRotation()->GetP() << endl;
 233   cout << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
 234   cout << "R: " << FDMExec->GetRotation()->GetR() << endl;
 235   cout << "L: " << FDMExec->GetAircraft()->GetL() << endl;
 236   cout << "M: " << FDMExec->GetAircraft()->GetM() << endl;
 237   cout << "N: " << FDMExec->GetAircraft()->GetN() << endl;
 238   cout << "Vt: " << Vt << endl;
 239   cout << "latitude: " << latitude << endl;
 240   cout << "longitude: " << longitude << endl;
 241   cout << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
 242   cout << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
 243   cout << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
 244   cout << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
 245   cout << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
 246   cout << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
 247   cout << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
 248   cout << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
 249   cout << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
 250   cout << "h: " << h << endl;
 251   cout << "a: " << a << endl;
 252   cout << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
 253   cout << "qbar: " << qbar << endl;
 254   cout << "sim_time: " << sim_time << endl;
 255   cout << "dt: " << dt << endl;
 256   cout << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
 257
 258   return true;
 259 }