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 // Change all angular measurements from degrees (as in config file) to radians
114 alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
118 beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
122 latitude *= M_PI / 180.0;
123 longitude *= M_PI / 180.0;
128 FDMExec->GetTranslation()->SetUVW(U, V, W);
129 FDMExec->GetRotation()->SetEuler(phi, tht, psi);
130 FDMExec->GetTranslation()->SetABG(alpha, beta, gamma);
132 Vt = sqrt(U*U + V*V + W*W);
133 qbar = sqrt(U*U + V*V + W*W);
135 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);
136 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);
137 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);
138 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);
140 FDMExec->GetRotation()->SetQ0123(Q0, Q1, Q2, Q3);
142 T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3;
143 T[1][2] = 2*(Q1*Q2 + Q0*Q3);
144 T[1][3] = 2*(Q1*Q3 - Q0*Q2);
145 T[2][1] = 2*(Q1*Q2 - Q0*Q3);
146 T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3;
147 T[2][3] = 2*(Q2*Q3 + Q0*Q1);
148 T[3][1] = 2*(Q1*Q3 + Q0*Q2);
149 T[3][2] = 2*(Q2*Q3 - Q0*Q1);
150 T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3;
152 FDMExec->GetPosition()->SetT(T[1][1], T[1][2], T[1][3],
153 T[2][1], T[2][2], T[2][3],
154 T[3][1], T[3][2], T[3][3]);
158 cerr << "Unable to load reset file " << fname << endl;
164 bool FGState::StoreData(string fname)
166 ofstream datafile(fname.c_str());
169 datafile << FDMExec->GetTranslation()->GetU();
170 datafile << FDMExec->GetTranslation()->GetV();
171 datafile << FDMExec->GetTranslation()->GetW();
172 datafile << latitude;
173 datafile << longitude;
174 datafile << FDMExec->GetRotation()->Getphi();
175 datafile << FDMExec->GetRotation()->Gettht();
176 datafile << FDMExec->GetRotation()->Getpsi();
181 cerr << "Could not open dump file " << fname << endl;
187 bool FGState::DumpData(string fname)
189 ofstream datafile(fname.c_str());
192 datafile << "U: " << FDMExec->GetTranslation()->GetU() << endl;
193 datafile << "V: " << FDMExec->GetTranslation()->GetV() << endl;
194 datafile << "W: " << FDMExec->GetTranslation()->GetW() << endl;
195 datafile << "P: " << FDMExec->GetRotation()->GetP() << endl;
196 datafile << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
197 datafile << "R: " << FDMExec->GetRotation()->GetR() << endl;
198 datafile << "L: " << FDMExec->GetAircraft()->GetL() << endl;
199 datafile << "M: " << FDMExec->GetAircraft()->GetM() << endl;
200 datafile << "N: " << FDMExec->GetAircraft()->GetN() << endl;
201 datafile << "latitude: " << latitude << endl;
202 datafile << "longitude: " << longitude << endl;
203 datafile << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
204 datafile << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
205 datafile << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
206 datafile << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
207 datafile << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
208 datafile << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
209 datafile << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
210 datafile << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
211 datafile << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
212 datafile << "h: " << h << endl;
213 datafile << "a: " << a << endl;
214 datafile << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
215 datafile << "qbar: " << qbar << endl;
216 datafile << "sim_time: " << sim_time << endl;
217 datafile << "dt: " << dt << endl;
218 datafile << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
227 bool FGState::DisplayData(void)
229 cout << "U: " << FDMExec->GetTranslation()->GetU() << endl;
230 cout << "V: " << FDMExec->GetTranslation()->GetV() << endl;
231 cout << "W: " << FDMExec->GetTranslation()->GetW() << endl;
232 cout << "P: " << FDMExec->GetRotation()->GetP() << endl;
233 cout << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
234 cout << "R: " << FDMExec->GetRotation()->GetR() << endl;
235 cout << "L: " << FDMExec->GetAircraft()->GetL() << endl;
236 cout << "M: " << FDMExec->GetAircraft()->GetM() << endl;
237 cout << "N: " << FDMExec->GetAircraft()->GetN() << endl;
238 cout << "Vt: " << Vt << endl;
239 cout << "latitude: " << latitude << endl;
240 cout << "longitude: " << longitude << endl;
241 cout << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
242 cout << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
243 cout << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
244 cout << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
245 cout << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
246 cout << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
247 cout << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
248 cout << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
249 cout << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
250 cout << "h: " << h << endl;
251 cout << "a: " << a << endl;
252 cout << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
253 cout << "qbar: " << qbar << endl;
254 cout << "sim_time: " << sim_time << endl;
255 cout << "dt: " << dt << endl;
256 cout << "m: " << FDMExec->GetAircraft()->GetMass() << endl;