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 --------------------------------------------------------------------------------
36 --------------------------------------------------------------------------------
40 ********************************************************************************
42 *******************************************************************************/
45 #include "FGAircraft.h"
49 /*******************************************************************************
50 ************************************ CODE **************************************
51 *******************************************************************************/
54 FGState::FGState(void)
56 U = V = W = Fx = Fy = Fz = 0.0;
57 P = Q = R = L = M = N = 0.0;
58 Q0 = Q1 = Q2 = Q3 = 0.0;
59 Ixx = Iyy = Izz = Ixz = 0.0;
61 latitude = longitude = 0.0;
62 alpha = beta = gamma = 0.0;
64 phi = tht = psi = 0.0;
65 Udot = Vdot = Wdot = 0.0;
66 Pdot = Qdot = Rdot = 0.0;
74 const float EarthRad = 20925650.0;
78 FGState::~FGState(void)
83 bool FGState::Reset(char* fname)
87 sprintf(resetDef, "/h/curt/projects/FlightGear/Simulator/FDM/JSBsim/aircraft/%s/%s", Aircraft->GetAircraftName(), fname);
89 ifstream resetfile(resetDef);
95 resetfile >> latitude;
96 resetfile >> longitude;
103 // Change all angular measurements from degrees (as in config file) to radians
106 alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
108 beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
110 latitude *= M_PI / 180.0;
111 longitude *= M_PI / 180.0;
112 alpha *= M_PI / 180.0;
113 beta *= M_PI / 180.0;
114 gamma *= M_PI / 180.0;
119 Vt = sqrt(U*U + V*V + W*W);
120 qbar = sqrt(U*U + V*V + W*W);
122 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);
123 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);
124 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);
125 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);
127 T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3;
128 T[1][2] = 2*(Q1*Q2 + Q0*Q3);
129 T[1][3] = 2*(Q1*Q3 - Q0*Q2);
130 T[2][1] = 2*(Q1*Q2 - Q0*Q3);
131 T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3;
132 T[2][3] = 2*(Q2*Q3 + Q0*Q1);
133 T[3][1] = 2*(Q1*Q3 + Q0*Q2);
134 T[3][2] = 2*(Q2*Q3 - Q0*Q1);
135 T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3;
139 cerr << "Unable to load reset file " << fname << endl;
145 bool FGState::StoreData(char* fname)
147 ofstream datafile(fname);
162 datafile << latitude;
163 datafile << longitude;
180 datafile << sim_time;
186 cerr << "Could not open dump file " << fname << endl;
192 bool FGState::DumpData(char* fname)
194 ofstream datafile(fname);
197 datafile << "U: " << U << endl;
198 datafile << "V: " << V << endl;
199 datafile << "W: " << W << endl;
200 datafile << "Fx: " << Fx << endl;
201 datafile << "Fy: " << Fy << endl;
202 datafile << "Fz: " << Fz << endl;
203 datafile << "P: " << P << endl;
204 datafile << "Q: " << Q << endl;
205 datafile << "R: " << R << endl;
206 datafile << "L: " << L << endl;
207 datafile << "M: " << M << endl;
208 datafile << "N: " << N << endl;
209 datafile << "latitude: " << latitude << endl;
210 datafile << "longitude: " << longitude << endl;
211 datafile << "alpha: " << alpha << endl;
212 datafile << "beta: " << beta << endl;
213 datafile << "gamma: " << gamma << endl;
214 datafile << "phi: " << phi << endl;
215 datafile << "tht: " << tht << endl;
216 datafile << "psi: " << psi << endl;
217 datafile << "Udot: " << Udot << endl;
218 datafile << "Vdot: " << Vdot << endl;
219 datafile << "Wdot: " << Wdot << endl;
220 datafile << "Pdot: " << Pdot << endl;
221 datafile << "Qdot: " << Qdot << endl;
222 datafile << "Rdot: " << Rdot << endl;
223 datafile << "h: " << h << endl;
224 datafile << "a: " << a << endl;
225 datafile << "rho: " << rho << endl;
226 datafile << "qbar: " << qbar << endl;
227 datafile << "sim_time: " << sim_time << endl;
228 datafile << "dt: " << dt << endl;
229 datafile << "g: " << g << endl;
230 datafile << "m: " << m << endl;
231 datafile << "Ixx: " << Ixx << endl;
232 datafile << "Iyy: " << Iyy << endl;
233 datafile << "Izz: " << Izz << endl;
234 datafile << "Ixz: " << Ixz << endl;
243 bool FGState::DisplayData(void)
245 cout << "U: " << U << endl;
246 cout << "V: " << V << endl;
247 cout << "W: " << W << endl;
248 cout << "Fx: " << Fx << endl;
249 cout << "Fy: " << Fy << endl;
250 cout << "Fz: " << Fz << endl;
251 cout << "P: " << P << endl;
252 cout << "Q: " << Q << endl;
253 cout << "R: " << R << endl;
254 cout << "L: " << L << endl;
255 cout << "M: " << M << endl;
256 cout << "N: " << N << endl;
257 cout << "Vt: " << Vt << endl;
258 cout << "latitude: " << latitude << endl;
259 cout << "longitude: " << longitude << endl;
260 cout << "alpha: " << alpha << endl;
261 cout << "beta: " << beta << endl;
262 cout << "gamma: " << gamma << endl;
263 cout << "phi: " << phi << endl;
264 cout << "tht: " << tht << endl;
265 cout << "psi: " << psi << endl;
266 cout << "Udot: " << Udot << endl;
267 cout << "Vdot: " << Vdot << endl;
268 cout << "Wdot: " << Wdot << endl;
269 cout << "Pdot: " << Pdot << endl;
270 cout << "Qdot: " << Qdot << endl;
271 cout << "Rdot: " << Rdot << endl;
272 cout << "h: " << h << endl;
273 cout << "a: " << a << endl;
274 cout << "rho: " << rho << endl;
275 cout << "qbar: " << qbar << endl;
276 cout << "sim_time: " << sim_time << endl;
277 cout << "dt: " << dt << endl;
278 cout << "g: " << g << endl;
279 cout << "m: " << m << endl;
280 cout << "Ixx: " << Ixx << endl;
281 cout << "Iyy: " << Iyy << endl;
282 cout << "Izz: " << Izz << endl;
283 cout << "Ixz: " << Ixz << endl;