fly/flightgear
1
0
Fork 0
Code Activity
flightgear/JSBsim/FGState.cpp
curt bef268a4cd Initial revision.
1999-02-05 21:26:01 +00:00

286 lines
8 KiB
C++
Raw Blame History

/*******************************************************************************
Module: FGState.cpp
Author: Jon Berndt
Date started: 11/17/98
Called by: FGFDMExec and accessed by all models.
------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
See header file.
ARGUMENTS
--------------------------------------------------------------------------------
HISTORY
--------------------------------------------------------------------------------
11/17/98 JSB Created
********************************************************************************
INCLUDES
*******************************************************************************/
#include "FGState.h"
#include "FGAircraft.h"
#include <math.h>
/*******************************************************************************
************************************ CODE **************************************
*******************************************************************************/
FGState::FGState(void)
{
U = V = W = Fx = Fy = Fz = 0.0;
P = Q = R = L = M = N = 0.0;
Q0 = Q1 = Q2 = Q3 = 0.0;
Ixx = Iyy = Izz = Ixz = 0.0;
Vt = 0.0;
latitude = longitude = 0.0;
alpha = beta = gamma = 0.0;
adot = bdot = 0.0;
phi = tht = psi = 0.0;
Udot = Vdot = Wdot = 0.0;
Pdot = Qdot = Rdot = 0.0;
h = 0.0;
a = 1000.0;
rho = qbar = 0.0;
sim_time = dt = 0.1;
m = 0.0;
g = 32.174;
const float EarthRad = 20925650.0;
}
FGState::~FGState(void)
{
}
bool FGState::Reset(char* fname)
{
char resetDef[200];
sprintf(resetDef, "/h/curt/projects/FlightGear/Simulator/FDM/JSBsim/aircraft/%s/%s", Aircraft->GetAircraftName(), fname);
ifstream resetfile(resetDef);
if (resetfile) {
resetfile >> U;
resetfile >> V;
resetfile >> W;
resetfile >> latitude;
resetfile >> longitude;
resetfile >> phi;
resetfile >> tht;
resetfile >> psi;
resetfile >> h;
resetfile.close();
// Change all angular measurements from degrees (as in config file) to radians
if (W != 0.0)
alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
if (V != 0.0)
beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
latitude *= M_PI / 180.0;
longitude *= M_PI / 180.0;
alpha *= M_PI / 180.0;
beta *= M_PI / 180.0;
gamma *= M_PI / 180.0;
phi *= M_PI / 180.0;
tht *= M_PI / 180.0;
psi *= M_PI / 180.0;
Vt = sqrt(U*U + V*V + W*W);
qbar = sqrt(U*U + V*V + W*W);
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);
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);
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);
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);
T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3;
T[1][2] = 2*(Q1*Q2 + Q0*Q3);
T[1][3] = 2*(Q1*Q3 - Q0*Q2);
T[2][1] = 2*(Q1*Q2 - Q0*Q3);
T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3;
T[2][3] = 2*(Q2*Q3 + Q0*Q1);
T[3][1] = 2*(Q1*Q3 + Q0*Q2);
T[3][2] = 2*(Q2*Q3 - Q0*Q1);
T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3;
return true;
} else {
cerr << "Unable to load reset file " << fname << endl;
return false;
}
}
bool FGState::StoreData(char* fname)
{
ofstream datafile(fname);
if (datafile) {
datafile << U;
datafile << V;
datafile << W;
datafile << Fx;
datafile << Fy;
datafile << Fz;
datafile << P;
datafile << Q;
datafile << R;
datafile << L;
datafile << M;
datafile << N;
datafile << latitude;
datafile << longitude;
datafile << alpha;
datafile << beta;
datafile << gamma;
datafile << phi;
datafile << tht;
datafile << psi;
datafile << Udot;
datafile << Vdot;
datafile << Wdot;
datafile << Pdot;
datafile << Qdot;
datafile << Rdot;
datafile << h;
datafile << a;
datafile << rho;
datafile << qbar;
datafile << sim_time;
datafile << dt;
datafile << g;
datafile.close();
return true;
} else {
cerr << "Could not open dump file " << fname << endl;
return false;
}
}
bool FGState::DumpData(char* fname)
{
ofstream datafile(fname);
if (datafile) {
datafile << "U: " << U << endl;
datafile << "V: " << V << endl;
datafile << "W: " << W << endl;
datafile << "Fx: " << Fx << endl;
datafile << "Fy: " << Fy << endl;
datafile << "Fz: " << Fz << endl;
datafile << "P: " << P << endl;
datafile << "Q: " << Q << endl;
datafile << "R: " << R << endl;
datafile << "L: " << L << endl;
datafile << "M: " << M << endl;
datafile << "N: " << N << endl;
datafile << "latitude: " << latitude << endl;
datafile << "longitude: " << longitude << endl;
datafile << "alpha: " << alpha << endl;
datafile << "beta: " << beta << endl;
datafile << "gamma: " << gamma << endl;
datafile << "phi: " << phi << endl;
datafile << "tht: " << tht << endl;
datafile << "psi: " << psi << endl;
datafile << "Udot: " << Udot << endl;
datafile << "Vdot: " << Vdot << endl;
datafile << "Wdot: " << Wdot << endl;
datafile << "Pdot: " << Pdot << endl;
datafile << "Qdot: " << Qdot << endl;
datafile << "Rdot: " << Rdot << endl;
datafile << "h: " << h << endl;
datafile << "a: " << a << endl;
datafile << "rho: " << rho << endl;
datafile << "qbar: " << qbar << endl;
datafile << "sim_time: " << sim_time << endl;
datafile << "dt: " << dt << endl;
datafile << "g: " << g << endl;
datafile << "m: " << m << endl;
datafile << "Ixx: " << Ixx << endl;
datafile << "Iyy: " << Iyy << endl;
datafile << "Izz: " << Izz << endl;
datafile << "Ixz: " << Ixz << endl;
datafile.close();
return true;
} else {
return false;
}
}
bool FGState::DisplayData(void)
{
cout << "U: " << U << endl;
cout << "V: " << V << endl;
cout << "W: " << W << endl;
cout << "Fx: " << Fx << endl;
cout << "Fy: " << Fy << endl;
cout << "Fz: " << Fz << endl;
cout << "P: " << P << endl;
cout << "Q: " << Q << endl;
cout << "R: " << R << endl;
cout << "L: " << L << endl;
cout << "M: " << M << endl;
cout << "N: " << N << endl;
cout << "Vt: " << Vt << endl;
cout << "latitude: " << latitude << endl;
cout << "longitude: " << longitude << endl;
cout << "alpha: " << alpha << endl;
cout << "beta: " << beta << endl;
cout << "gamma: " << gamma << endl;
cout << "phi: " << phi << endl;
cout << "tht: " << tht << endl;
cout << "psi: " << psi << endl;
cout << "Udot: " << Udot << endl;
cout << "Vdot: " << Vdot << endl;
cout << "Wdot: " << Wdot << endl;
cout << "Pdot: " << Pdot << endl;
cout << "Qdot: " << Qdot << endl;
cout << "Rdot: " << Rdot << endl;
cout << "h: " << h << endl;
cout << "a: " << a << endl;
cout << "rho: " << rho << endl;
cout << "qbar: " << qbar << endl;
cout << "sim_time: " << sim_time << endl;
cout << "dt: " << dt << endl;
cout << "g: " << g << endl;
cout << "m: " << m << endl;
cout << "Ixx: " << Ixx << endl;
cout << "Iyy: " << Iyy << endl;
cout << "Izz: " << Izz << endl;
cout << "Ixz: " << Ixz << endl;
return true;
}
View git blame Copy permalink