1 /*******************************************************************************
6 Called by: FGFDMExec and accessed by all models.
8 ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
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
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
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.
24 Further information about the GNU General Public License can also be found on
25 the world wide web at http://www.gnu.org.
27 FUNCTIONAL DESCRIPTION
28 --------------------------------------------------------------------------------
32 --------------------------------------------------------------------------------
35 ********************************************************************************
37 *******************************************************************************/
40 # include <Include/compiler.h>
41 # ifdef FG_HAVE_STD_INCLUDES
51 #include "FGFDMExec.h"
52 #include "FGAtmosphere.h"
54 #include "FGAircraft.h"
55 #include "FGTranslation.h"
56 #include "FGRotation.h"
57 #include "FGPosition.h"
58 #include "FGAuxiliary.h"
61 /*******************************************************************************
62 ************************************ CODE **************************************
63 *******************************************************************************/
66 FGState::FGState(FGFDMExec* fdex)
71 latitude = longitude = 0.0;
80 FGState::~FGState(void)
85 bool FGState::Reset(string path, string fname)
90 float alpha, beta, gamma;
94 resetDef = path + "/" + FDMExec->GetAircraft()->GetAircraftName() + "/" + fname;
96 ifstream resetfile(resetDef.c_str());
102 resetfile >> latitude;
103 resetfile >> longitude;
110 Initialize(U, V, W, phi, tht, psi, latitude, longitude, h);
114 cerr << "Unable to load reset file " << fname << endl;
120 void FGState::Initialize(float U, float V, float W,
121 float phi, float tht, float psi,
122 float Latitude, float Longitude, float H)
124 float alpha, beta, gamma;
125 float Q0, Q1, Q2, Q3;
129 longitude = Longitude;
132 // Change all angular measurements from degrees (as in config file) to radians
136 alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
140 beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
144 latitude *= M_PI / 180.0;
145 longitude *= M_PI / 180.0;
150 FDMExec->GetTranslation()->SetUVW(U, V, W);
151 FDMExec->GetRotation()->SetEuler(phi, tht, psi);
152 FDMExec->GetTranslation()->SetABG(alpha, beta, gamma);
154 Vt = sqrt(U*U + V*V + W*W);
155 qbar = 0.5*(U*U + V*V + W*W)*FDMExec->GetAtmosphere()->CalcRho(h);
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);
162 FDMExec->GetRotation()->SetQ0123(Q0, Q1, Q2, Q3);
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;
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]);
181 bool FGState::StoreData(string fname)
183 ofstream datafile(fname.c_str());
186 datafile << FDMExec->GetTranslation()->GetU();
187 datafile << FDMExec->GetTranslation()->GetV();
188 datafile << FDMExec->GetTranslation()->GetW();
189 datafile << latitude;
190 datafile << longitude;
191 datafile << FDMExec->GetRotation()->Getphi();
192 datafile << FDMExec->GetRotation()->Gettht();
193 datafile << FDMExec->GetRotation()->Getpsi();
198 cerr << "Could not open dump file " << fname << endl;
204 bool FGState::DumpData(string fname)
206 ofstream datafile(fname.c_str());
209 datafile << "U: " << FDMExec->GetTranslation()->GetU() << endl;
210 datafile << "V: " << FDMExec->GetTranslation()->GetV() << endl;
211 datafile << "W: " << FDMExec->GetTranslation()->GetW() << endl;
212 datafile << "P: " << FDMExec->GetRotation()->GetP() << endl;
213 datafile << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
214 datafile << "R: " << FDMExec->GetRotation()->GetR() << endl;
215 datafile << "L: " << FDMExec->GetAircraft()->GetL() << endl;
216 datafile << "M: " << FDMExec->GetAircraft()->GetM() << endl;
217 datafile << "N: " << FDMExec->GetAircraft()->GetN() << endl;
218 datafile << "latitude: " << latitude << endl;
219 datafile << "longitude: " << longitude << endl;
220 datafile << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
221 datafile << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
222 datafile << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
223 datafile << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
224 datafile << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
225 datafile << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
226 datafile << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
227 datafile << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
228 datafile << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
229 datafile << "h: " << h << endl;
230 datafile << "a: " << a << endl;
231 datafile << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
232 datafile << "qbar: " << qbar << endl;
233 datafile << "sim_time: " << sim_time << endl;
234 datafile << "dt: " << dt << endl;
235 datafile << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
244 bool FGState::DisplayData(void)
246 cout << "U: " << FDMExec->GetTranslation()->GetU() << endl;
247 cout << "V: " << FDMExec->GetTranslation()->GetV() << endl;
248 cout << "W: " << FDMExec->GetTranslation()->GetW() << endl;
249 cout << "P: " << FDMExec->GetRotation()->GetP() << endl;
250 cout << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
251 cout << "R: " << FDMExec->GetRotation()->GetR() << endl;
252 cout << "L: " << FDMExec->GetAircraft()->GetL() << endl;
253 cout << "M: " << FDMExec->GetAircraft()->GetM() << endl;
254 cout << "N: " << FDMExec->GetAircraft()->GetN() << endl;
255 cout << "Vt: " << Vt << endl;
256 cout << "latitude: " << latitude << endl;
257 cout << "longitude: " << longitude << endl;
258 cout << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
259 cout << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
260 cout << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
261 cout << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
262 cout << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
263 cout << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
264 cout << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
265 cout << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
266 cout << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
267 cout << "h: " << h << endl;
268 cout << "a: " << a << endl;
269 cout << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
270 cout << "qbar: " << qbar << endl;
271 cout << "sim_time: " << sim_time << endl;
272 cout << "dt: " << dt << endl;
273 cout << "m: " << FDMExec->GetAircraft()->GetMass() << endl;