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 ARGUMENTS
  32 --------------------------------------------------------------------------------
  33
  34
  35 HISTORY
  36 --------------------------------------------------------------------------------
  37
  38 11/17/98   JSB   Created
  39
  40 ********************************************************************************
  41 INCLUDES
  42 *******************************************************************************/
  43
  44 #include "FGState.h"
  45 #include "FGAircraft.h"
  46
  47 #include <math.h>
  48
  49 /*******************************************************************************
  50 ************************************ CODE **************************************
  51 *******************************************************************************/
  52
  53
  54 FGState::FGState(void)
  55 {
  56   U = V = W = Fx = Fy = Fz = 0.0;
  57   P = Q = R = L = M = N = 0.0;
  58   Q0 = Q1 = Q2 = Q3 = 0.0;
  59   Ixx = Iyy = Izz = Ixz = 0.0;
  60   Vt = 0.0;
  61   latitude = longitude = 0.0;
  62   alpha = beta = gamma = 0.0;
  63   adot = bdot = 0.0;
  64   phi = tht = psi = 0.0;
  65   Udot = Vdot = Wdot = 0.0;
  66   Pdot = Qdot = Rdot = 0.0;
  67   h = 0.0;
  68   a = 1000.0;
  69   rho = qbar = 0.0;
  70   sim_time = dt = 0.1;
  71   m = 0.0;
  72   g = 32.174;
  73
  74   const float EarthRad     = 20925650.0;
  75 }
  76
  77
  78 FGState::~FGState(void)
  79 {
  80 }
  81
  82
  83 bool FGState::Reset(char* fname)
  84 {
  85   char resetDef[200];
  86
  87   sprintf(resetDef, "/h/curt/projects/FlightGear/Simulator/FDM/JSBsim/aircraft/%s/%s", Aircraft->GetAircraftName(), fname);
  88
  89   ifstream resetfile(resetDef);
  90
  91   if (resetfile) {
  92     resetfile >> U;
  93     resetfile >> V;
  94     resetfile >> W;
  95     resetfile >> latitude;
  96     resetfile >> longitude;
  97     resetfile >> phi;
  98     resetfile >> tht;
  99     resetfile >> psi;
 100     resetfile >> h;
 101     resetfile.close();
 102
 103 // Change all angular measurements from degrees (as in config file) to radians
 104
 105     if (W != 0.0)
 106       alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
 107     if (V != 0.0)
 108       beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
 109
 110     latitude  *= M_PI / 180.0;
 111     longitude *= M_PI / 180.0;
 112     alpha     *= M_PI / 180.0;
 113     beta      *= M_PI / 180.0;
 114     gamma     *= M_PI / 180.0;
 115     phi       *= M_PI / 180.0;
 116     tht       *= M_PI / 180.0;
 117     psi       *= M_PI / 180.0;
 118
 119     Vt = sqrt(U*U + V*V + W*W);
 120     qbar = sqrt(U*U + V*V + W*W);
 121
 122     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);
 123     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);
 124     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);
 125     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);
 126
 127     T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3;
 128     T[1][2] = 2*(Q1*Q2 + Q0*Q3);
 129     T[1][3] = 2*(Q1*Q3 - Q0*Q2);
 130     T[2][1] = 2*(Q1*Q2 - Q0*Q3);
 131     T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3;
 132     T[2][3] = 2*(Q2*Q3 + Q0*Q1);
 133     T[3][1] = 2*(Q1*Q3 + Q0*Q2);
 134     T[3][2] = 2*(Q2*Q3 - Q0*Q1);
 135     T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3;
 136
 137     return true;
 138   } else {
 139     cerr << "Unable to load reset file " << fname << endl;
 140     return false;
 141   }
 142 }
 143
 144
 145 bool FGState::StoreData(char* fname)
 146 {
 147   ofstream datafile(fname);
 148
 149   if (datafile) {
 150     datafile << U;
 151     datafile << V;
 152     datafile << W;
 153     datafile << Fx;
 154     datafile << Fy;
 155     datafile << Fz;
 156     datafile << P;
 157     datafile << Q;
 158     datafile << R;
 159     datafile << L;
 160     datafile << M;
 161     datafile << N;
 162     datafile << latitude;
 163     datafile << longitude;
 164     datafile << alpha;
 165     datafile << beta;
 166     datafile << gamma;
 167     datafile << phi;
 168     datafile << tht;
 169     datafile << psi;
 170     datafile << Udot;
 171     datafile << Vdot;
 172     datafile << Wdot;
 173     datafile << Pdot;
 174     datafile << Qdot;
 175     datafile << Rdot;
 176     datafile << h;
 177     datafile << a;
 178     datafile << rho;
 179     datafile << qbar;
 180     datafile << sim_time;
 181     datafile << dt;
 182     datafile << g;
 183     datafile.close();
 184     return true;
 185   } else {
 186     cerr << "Could not open dump file " << fname << endl;
 187     return false;
 188   }
 189 }
 190
 191
 192 bool FGState::DumpData(char* fname)
 193 {
 194   ofstream datafile(fname);
 195
 196   if (datafile) {
 197     datafile << "U: " << U << endl;
 198     datafile << "V: " << V << endl;
 199     datafile << "W: " << W << endl;
 200     datafile << "Fx: " << Fx << endl;
 201     datafile << "Fy: " << Fy << endl;
 202     datafile << "Fz: " << Fz << endl;
 203     datafile << "P: " << P << endl;
 204     datafile << "Q: " << Q << endl;
 205     datafile << "R: " << R << endl;
 206     datafile << "L: " << L << endl;
 207     datafile << "M: " << M << endl;
 208     datafile << "N: " << N << endl;
 209     datafile << "latitude: " << latitude << endl;
 210     datafile << "longitude: " << longitude << endl;
 211     datafile << "alpha: " << alpha << endl;
 212     datafile << "beta: " << beta << endl;
 213     datafile << "gamma: " << gamma << endl;
 214     datafile << "phi: " << phi << endl;
 215     datafile << "tht: " << tht << endl;
 216     datafile << "psi: " << psi << endl;
 217     datafile << "Udot: " << Udot << endl;
 218     datafile << "Vdot: " << Vdot << endl;
 219     datafile << "Wdot: " << Wdot << endl;
 220     datafile << "Pdot: " << Pdot << endl;
 221     datafile << "Qdot: " << Qdot << endl;
 222     datafile << "Rdot: " << Rdot << endl;
 223     datafile << "h: " << h << endl;
 224     datafile << "a: " << a << endl;
 225     datafile << "rho: " << rho << endl;
 226     datafile << "qbar: " << qbar << endl;
 227     datafile << "sim_time: " << sim_time << endl;
 228     datafile << "dt: " << dt << endl;
 229     datafile << "g: " << g << endl;
 230     datafile << "m: " << m << endl;
 231     datafile << "Ixx: " << Ixx << endl;
 232     datafile << "Iyy: " << Iyy << endl;
 233     datafile << "Izz: " << Izz << endl;
 234     datafile << "Ixz: " << Ixz << endl;
 235     datafile.close();
 236     return true;
 237   } else {
 238     return false;
 239   }
 240 }
 241
 242
 243 bool FGState::DisplayData(void)
 244 {
 245   cout << "U: " << U << endl;
 246   cout << "V: " << V << endl;
 247   cout << "W: " << W << endl;
 248   cout << "Fx: " << Fx << endl;
 249   cout << "Fy: " << Fy << endl;
 250   cout << "Fz: " << Fz << endl;
 251   cout << "P: " << P << endl;
 252   cout << "Q: " << Q << endl;
 253   cout << "R: " << R << endl;
 254   cout << "L: " << L << endl;
 255   cout << "M: " << M << endl;
 256   cout << "N: " << N << endl;
 257   cout << "Vt: " << Vt << endl;
 258   cout << "latitude: " << latitude << endl;
 259   cout << "longitude: " << longitude << endl;
 260   cout << "alpha: " << alpha << endl;
 261   cout << "beta: " << beta << endl;
 262   cout << "gamma: " << gamma << endl;
 263   cout << "phi: " << phi << endl;
 264   cout << "tht: " << tht << endl;
 265   cout << "psi: " << psi << endl;
 266   cout << "Udot: " << Udot << endl;
 267   cout << "Vdot: " << Vdot << endl;
 268   cout << "Wdot: " << Wdot << endl;
 269   cout << "Pdot: " << Pdot << endl;
 270   cout << "Qdot: " << Qdot << endl;
 271   cout << "Rdot: " << Rdot << endl;
 272   cout << "h: " << h << endl;
 273   cout << "a: " << a << endl;
 274   cout << "rho: " << rho << endl;
 275   cout << "qbar: " << qbar << endl;
 276   cout << "sim_time: " << sim_time << endl;
 277   cout << "dt: " << dt << endl;
 278   cout << "g: " << g << endl;
 279   cout << "m: " << m << endl;
 280   cout << "Ixx: " << Ixx << endl;
 281   cout << "Iyy: " << Iyy << endl;
 282   cout << "Izz: " << Izz << endl;
 283   cout << "Ixz: " << Ixz << endl;
 284
 285   return true;
 286 }