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;
81 FGState::~FGState(void)
86 //***************************************************************************
88 // Reset: Assume all angles READ FROM FILE IN DEGREES !!
91 bool FGState::Reset(string path, string fname)
97 resetDef = path + "/" + FDMExec->GetAircraft()->GetAircraftName() + "/" + fname;
99 ifstream resetfile(resetDef.c_str());
105 resetfile >> latitude;
106 resetfile >> longitude;
113 Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD,
114 latitude*DEGTORAD, longitude*DEGTORAD, h);
118 cerr << "Unable to load reset file " << fname << endl;
123 //***************************************************************************
125 // Initialize: Assume all angles GIVEN IN RADIANS !!
128 void FGState::Initialize(float U, float V, float W,
129 float phi, float tht, float psi,
130 float Latitude, float Longitude, float H)
132 float alpha, beta, gamma;
133 float Q0, Q1, Q2, Q3;
137 longitude = Longitude;
142 alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
146 beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.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 void FGState::Initialize(FGInitialCondition *FGIC)
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();
197 Initialize(U, V, W, phi, tht, psi,latitude, longitude, h);
201 bool FGState::StoreData(string fname)
203 ofstream datafile(fname.c_str());
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();
218 cerr << "Could not open dump file " << fname << endl;
224 bool FGState::DumpData(string fname)
226 ofstream datafile(fname.c_str());
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;
264 bool FGState::DisplayData(void)
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;