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