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 = 0.0;
  77   dt = 1.0/120.0;
  78 }
  79
  80
  81 FGState::~FGState(void)
  82 {
  83 }
  84
  85
  86 //***************************************************************************
  87 //
  88 // Reset: Assume all angles READ FROM FILE IN DEGREES !!
  89 //
  90
  91 bool FGState::Reset(string path, string fname)
  92 {
  93   string resetDef;
  94   float U, V, W;
  95   float phi, tht, psi;
  96
  97   resetDef = path + "/" + FDMExec->GetAircraft()->GetAircraftName() + "/" + fname;
  98
  99   ifstream resetfile(resetDef.c_str());
 100
 101   if (resetfile) {
 102     resetfile >> U;
 103     resetfile >> V;
 104     resetfile >> W;
 105     resetfile >> latitude;
 106     resetfile >> longitude;
 107     resetfile >> phi;
 108     resetfile >> tht;
 109     resetfile >> psi;
 110     resetfile >> h;
 111     resetfile.close();
 112
 113     Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD,
 114                latitude*DEGTORAD, longitude*DEGTORAD, h);
 115
 116     return true;
 117   } else {
 118     cerr << "Unable to load reset file " << fname << endl;
 119     return false;
 120   }
 121 }
 122
 123 //***************************************************************************
 124 //
 125 // Initialize: Assume all angles GIVEN IN RADIANS !!
 126 //
 127
 128 void FGState::Initialize(float U, float V, float W,
 129                          float phi, float tht, float psi,
 130                          float Latitude, float Longitude, float H)
 131 {
 132   float alpha, beta, gamma;
 133   float Q0, Q1, Q2, Q3;
 134   float T[4][4];
 135
 136   latitude = Latitude;
 137   longitude = Longitude;
 138   h = H;
 139
 140   gamma = 0.0;
 141   if (W != 0.0)
 142     alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
 143   else
 144     alpha = 0.0;
 145   if (V != 0.0)
 146     beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
 147   else
 148     beta = 0.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 void FGState::Initialize(FGInitialCondition *FGIC)
 182 {
 183
 184   float tht,psi,phi;
 185   float U,V,W;
 186
 187   latitude = FGIC->GetLatitudeRadIC();
 188   longitude = FGIC->GetLongitudeRadIC();
 189   h = FGIC->GetAltitudeFtIC();
 190   U = FGIC->GetUBodyFpsIC();
 191   V = FGIC->GetVBodyFpsIC();
 192   W = FGIC->GetWBodyFpsIC();
 193   tht = FGIC->GetThetaRadIC();
 194   phi = FGIC->GetPhiRadIC();
 195   psi = FGIC->GetPsiRadIC();
 196
 197   Initialize(U, V, W, phi, tht, psi,latitude, longitude, h);
 198 }
 199
 200
 201 bool FGState::StoreData(string fname)
 202 {
 203   ofstream datafile(fname.c_str());
 204
 205   if (datafile) {
 206     datafile << FDMExec->GetTranslation()->GetU();
 207     datafile << FDMExec->GetTranslation()->GetV();
 208     datafile << FDMExec->GetTranslation()->GetW();
 209     datafile << latitude;
 210     datafile << longitude;
 211     datafile << FDMExec->GetRotation()->Getphi();
 212     datafile << FDMExec->GetRotation()->Gettht();
 213     datafile << FDMExec->GetRotation()->Getpsi();
 214     datafile << h;
 215     datafile.close();
 216     return true;
 217   } else {
 218     cerr << "Could not open dump file " << fname << endl;
 219     return false;
 220   }
 221 }
 222
 223
 224 bool FGState::DumpData(string fname)
 225 {
 226   ofstream datafile(fname.c_str());
 227
 228   if (datafile) {
 229     datafile << "U: " << FDMExec->GetTranslation()->GetU() << endl;
 230     datafile << "V: " << FDMExec->GetTranslation()->GetV() << endl;
 231     datafile << "W: " << FDMExec->GetTranslation()->GetW() << endl;
 232     datafile << "P: " << FDMExec->GetRotation()->GetP() << endl;
 233     datafile << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
 234     datafile << "R: " << FDMExec->GetRotation()->GetR() << endl;
 235     datafile << "L: " << FDMExec->GetAircraft()->GetL() << endl;
 236     datafile << "M: " << FDMExec->GetAircraft()->GetM() << endl;
 237     datafile << "N: " << FDMExec->GetAircraft()->GetN() << endl;
 238     datafile << "latitude: " << latitude << endl;
 239     datafile << "longitude: " << longitude << endl;
 240     datafile << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
 241     datafile << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
 242     datafile << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
 243     datafile << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
 244     datafile << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
 245     datafile << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
 246     datafile << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
 247     datafile << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
 248     datafile << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
 249     datafile << "h: " << h << endl;
 250     datafile << "a: " << a << endl;
 251     datafile << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
 252     datafile << "qbar: " << qbar << endl;
 253     datafile << "sim_time: " << sim_time << endl;
 254     datafile << "dt: " << dt << endl;
 255     datafile << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
 256     datafile.close();
 257     return true;
 258   } else {
 259     return false;
 260   }
 261 }
 262
 263
 264 bool FGState::DisplayData(void)
 265 {
 266   cout << "U: " << FDMExec->GetTranslation()->GetU() << endl;
 267   cout << "V: " << FDMExec->GetTranslation()->GetV() << endl;
 268   cout << "W: " << FDMExec->GetTranslation()->GetW() << endl;
 269   cout << "P: " << FDMExec->GetRotation()->GetP()*RADTODEG << endl;
 270   cout << "Q: " << FDMExec->GetRotation()->GetQ()*RADTODEG << endl;
 271   cout << "R: " << FDMExec->GetRotation()->GetR()*RADTODEG << endl;
 272   cout << "L: " << FDMExec->GetAircraft()->GetL() << endl;
 273   cout << "M: " << FDMExec->GetAircraft()->GetM() << endl;
 274   cout << "N: " << FDMExec->GetAircraft()->GetN() << endl;
 275   cout << "Vt: " << Vt << endl;
 276   cout << "latitude: " << latitude << endl;
 277   cout << "longitude: " << longitude << endl;
 278   cout << "alpha: " << FDMExec->GetTranslation()->Getalpha()*RADTODEG << endl;
 279   cout << "beta: " << FDMExec->GetTranslation()->Getbeta()*RADTODEG << endl;
 280   cout << "gamma: " << FDMExec->GetTranslation()->Getgamma()*RADTODEG << endl;
 281   cout << "phi: " << FDMExec->GetRotation()->Getphi()*RADTODEG << endl;
 282   cout << "tht: " << FDMExec->GetRotation()->Gettht()*RADTODEG << endl;
 283   cout << "psi: " << FDMExec->GetRotation()->Getpsi()*RADTODEG << endl;
 284   cout << "Pdot: " << FDMExec->GetRotation()->GetPdot()*RADTODEG << endl;
 285   cout << "Qdot: " << FDMExec->GetRotation()->GetQdot()*RADTODEG << endl;
 286   cout << "Rdot: " << FDMExec->GetRotation()->GetRdot()*RADTODEG << endl;
 287   cout << "h: " << h << endl;
 288   cout << "a: " << a << endl;
 289   cout << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
 290   cout << "qbar: " << qbar << endl;
 291   cout << "sim_time: " << sim_time << endl;
 292   cout << "dt: " << dt << endl;
 293   cout << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
 294
 295   return true;
 296 }