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 //***************************************************************************
87 // Reset: Assume all angles READ FROM FILE IN DEGREES !!
90 bool FGState::Reset(string path, string fname)
96 resetDef = path + "/" + FDMExec->GetAircraft()->GetAircraftName() + "/" + fname;
98 ifstream resetfile(resetDef.c_str());
104 resetfile >> latitude;
105 resetfile >> longitude;
112 Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD,
113 latitude*DEGTORAD, longitude*DEGTORAD, h);
117 cerr << "Unable to load reset file " << fname << endl;
122 //***************************************************************************
124 // Initialize: Assume all angles GIVEN IN RADIANS !!
127 void FGState::Initialize(float U, float V, float W,
128 float phi, float tht, float psi,
129 float Latitude, float Longitude, float H)
131 float alpha, beta, gamma;
132 float Q0, Q1, Q2, Q3;
136 longitude = Longitude;
141 alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
145 beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
149 FDMExec->GetTranslation()->SetUVW(U, V, W);
150 FDMExec->GetRotation()->SetEuler(phi, tht, psi);
151 FDMExec->GetTranslation()->SetABG(alpha, beta, gamma);
153 Vt = sqrt(U*U + V*V + W*W);
154 qbar = 0.5*(U*U + V*V + W*W)*FDMExec->GetAtmosphere()->CalcRho(h);
156 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);
157 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);
158 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);
159 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);
161 FDMExec->GetRotation()->SetQ0123(Q0, Q1, Q2, Q3);
163 T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3;
164 T[1][2] = 2*(Q1*Q2 + Q0*Q3);
165 T[1][3] = 2*(Q1*Q3 - Q0*Q2);
166 T[2][1] = 2*(Q1*Q2 - Q0*Q3);
167 T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3;
168 T[2][3] = 2*(Q2*Q3 + Q0*Q1);
169 T[3][1] = 2*(Q1*Q3 + Q0*Q2);
170 T[3][2] = 2*(Q2*Q3 - Q0*Q1);
171 T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3;
173 FDMExec->GetPosition()->SetT(T[1][1], T[1][2], T[1][3],
174 T[2][1], T[2][2], T[2][3],
175 T[3][1], T[3][2], T[3][3]);
180 void FGState::Initialize(FGInitialCondition *FGIC)
186 latitude = FGIC->GetLatitudeRadIC();
187 longitude = FGIC->GetLongitudeRadIC();
188 h = FGIC->GetAltitudeFtIC();
189 U = FGIC->GetUBodyFpsIC();
190 V = FGIC->GetVBodyFpsIC();
191 W = FGIC->GetWBodyFpsIC();
192 tht = FGIC->GetThetaRadIC();
193 phi = FGIC->GetPhiRadIC();
194 psi = FGIC->GetPsiRadIC();
196 Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD,
197 latitude*DEGTORAD, longitude*DEGTORAD, 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;