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 #include <math.h>
  40
  41 #include <string>
  42
  43 #include "FGState.h"
  44 #include "FGFDMExec.h"
  45 #include "FGAtmosphere.h"
  46 #include "FGFCS.h"
  47 #include "FGAircraft.h"
  48 #include "FGTranslation.h"
  49 #include "FGRotation.h"
  50 #include "FGPosition.h"
  51 #include "FGAuxiliary.h"
  52 #include "FGOutput.h"
  53
  54 /*******************************************************************************
  55 ************************************ CODE **************************************
  56 *******************************************************************************/
  57
  58
  59 FGState::FGState(FGFDMExec* fdex)
  60 {
  61   FDMExec = fdex;
  62
  63   Vt = 0.0;
  64   latitude = longitude = 0.0;
  65   adot = bdot = 0.0;
  66   h = 0.0;
  67   a = 1000.0;
  68   qbar = 0.0;
  69   sim_time = dt = 0.1;
  70 }
  71
  72
  73 FGState::~FGState(void)
  74 {
  75 }
  76
  77
  78 bool FGState::Reset(const string& path, const string& fname)
  79 {
  80   string resetDef;
  81   float U, V, W;
  82   float phi, tht, psi;
  83   float alpha, beta, gamma;
  84   float Q0, Q1, Q2, Q3;
  85   float T[4][4];
  86
  87   resetDef = path;
  88   resetDef += "/";
  89   resetDef += FDMExec->GetAircraft()->GetAircraftName();
  90   resetDef += "/" + fname;
  91
  92   ifstream resetfile( resetDef.c_str() );
  93
  94   if (resetfile) {
  95     resetfile >> U;
  96     resetfile >> V;
  97     resetfile >> W;
  98     resetfile >> latitude;
  99     resetfile >> longitude;
 100     resetfile >> phi;
 101     resetfile >> tht;
 102     resetfile >> psi;
 103     resetfile >> h;
 104     resetfile.close();
 105
 106 // Change all angular measurements from degrees (as in config file) to radians
 107
 108     gamma = 0.0;
 109     if (W != 0.0)
 110       alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
 111     else
 112       alpha = 0.0;
 113     if (V != 0.0)
 114       beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
 115     else
 116       beta = 0.0;
 117
 118     latitude  *= M_PI / 180.0;
 119     longitude *= M_PI / 180.0;
 120     phi       *= M_PI / 180.0;
 121     tht       *= M_PI / 180.0;
 122     psi       *= M_PI / 180.0;
 123
 124     FDMExec->GetTranslation()->SetUVW(U, V, W);
 125     FDMExec->GetRotation()->SetEuler(phi, tht, psi);
 126     FDMExec->GetTranslation()->SetABG(alpha, beta, gamma);
 127
 128     Vt = sqrt(U*U + V*V + W*W);
 129     qbar = sqrt(U*U + V*V + W*W);
 130
 131     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);
 132     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);
 133     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);
 134     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);
 135
 136     FDMExec->GetRotation()->SetQ0123(Q0, Q1, Q2, Q3);
 137
 138     T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3;
 139     T[1][2] = 2*(Q1*Q2 + Q0*Q3);
 140     T[1][3] = 2*(Q1*Q3 - Q0*Q2);
 141     T[2][1] = 2*(Q1*Q2 - Q0*Q3);
 142     T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3;
 143     T[2][3] = 2*(Q2*Q3 + Q0*Q1);
 144     T[3][1] = 2*(Q1*Q3 + Q0*Q2);
 145     T[3][2] = 2*(Q2*Q3 - Q0*Q1);
 146     T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3;
 147
 148     FDMExec->GetPosition()->SetT(T[1][1], T[1][2], T[1][3],
 149                                  T[2][1], T[2][2], T[2][3],
 150                                  T[3][1], T[3][2], T[3][3]);
 151
 152     return true;
 153   } else {
 154     cerr << "Unable to load reset file " << fname << endl;
 155     return false;
 156   }
 157 }
 158
 159
 160 bool FGState::StoreData(char* fname)
 161 {
 162   ofstream datafile(fname);
 163
 164   if (datafile) {
 165     datafile << FDMExec->GetTranslation()->GetU();
 166     datafile << FDMExec->GetTranslation()->GetV();
 167     datafile << FDMExec->GetTranslation()->GetW();
 168     datafile << latitude;
 169     datafile << longitude;
 170     datafile << FDMExec->GetRotation()->Getphi();
 171     datafile << FDMExec->GetRotation()->Gettht();
 172     datafile << FDMExec->GetRotation()->Getpsi();
 173     datafile << h;
 174     datafile.close();
 175     return true;
 176   } else {
 177     cerr << "Could not open dump file " << fname << endl;
 178     return false;
 179   }
 180 }
 181
 182
 183 bool FGState::DumpData(char* fname)
 184 {
 185   ofstream datafile(fname);
 186
 187   if (datafile) {
 188     datafile << "U: " << FDMExec->GetTranslation()->GetU() << endl;
 189     datafile << "V: " << FDMExec->GetTranslation()->GetV() << endl;
 190     datafile << "W: " << FDMExec->GetTranslation()->GetW() << endl;
 191     datafile << "P: " << FDMExec->GetRotation()->GetP() << endl;
 192     datafile << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
 193     datafile << "R: " << FDMExec->GetRotation()->GetR() << endl;
 194     datafile << "L: " << FDMExec->GetAircraft()->GetL() << endl;
 195     datafile << "M: " << FDMExec->GetAircraft()->GetM() << endl;
 196     datafile << "N: " << FDMExec->GetAircraft()->GetN() << endl;
 197     datafile << "latitude: " << latitude << endl;
 198     datafile << "longitude: " << longitude << endl;
 199     datafile << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
 200     datafile << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
 201     datafile << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
 202     datafile << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
 203     datafile << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
 204     datafile << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
 205     datafile << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
 206     datafile << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
 207     datafile << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
 208     datafile << "h: " << h << endl;
 209     datafile << "a: " << a << endl;
 210     datafile << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
 211     datafile << "qbar: " << qbar << endl;
 212     datafile << "sim_time: " << sim_time << endl;
 213     datafile << "dt: " << dt << endl;
 214     datafile << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
 215     datafile.close();
 216     return true;
 217   } else {
 218     return false;
 219   }
 220 }
 221
 222
 223 bool FGState::DisplayData(void)
 224 {
 225   cout << "U: " << FDMExec->GetTranslation()->GetU() << endl;
 226   cout << "V: " << FDMExec->GetTranslation()->GetV() << endl;
 227   cout << "W: " << FDMExec->GetTranslation()->GetW() << endl;
 228   cout << "P: " << FDMExec->GetRotation()->GetP() << endl;
 229   cout << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
 230   cout << "R: " << FDMExec->GetRotation()->GetR() << endl;
 231   cout << "L: " << FDMExec->GetAircraft()->GetL() << endl;
 232   cout << "M: " << FDMExec->GetAircraft()->GetM() << endl;
 233   cout << "N: " << FDMExec->GetAircraft()->GetN() << endl;
 234   cout << "Vt: " << Vt << endl;
 235   cout << "latitude: " << latitude << endl;
 236   cout << "longitude: " << longitude << endl;
 237   cout << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
 238   cout << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
 239   cout << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
 240   cout << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
 241   cout << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
 242   cout << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
 243   cout << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
 244   cout << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
 245   cout << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
 246   cout << "h: " << h << endl;
 247   cout << "a: " << a << endl;
 248   cout << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
 249   cout << "qbar: " << qbar << endl;
 250   cout << "sim_time: " << sim_time << endl;
 251   cout << "dt: " << dt << endl;
 252   cout << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
 253
 254   return true;
 255 }