Branch data Line data Source code
1 : : /*******************************************************************************
2 : :
3 : : Header: FGInitialCondition.cpp
4 : : Author: Tony Peden
5 : : Date started: 7/1/99
6 : :
7 : : ------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
8 : :
9 : : This program is free software; you can redistribute it and/or modify it under
10 : : the terms of the GNU Lesser General Public License as published by the Free Software
11 : : Foundation; either version 2 of the License, or (at your option) any later
12 : : version.
13 : :
14 : : This program is distributed in the hope that it will be useful, but WITHOUT
15 : : ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16 : : FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
17 : : details.
18 : :
19 : : You should have received a copy of the GNU Lesser General Public License along with
20 : : this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21 : : Place - Suite 330, Boston, MA 02111-1307, USA.
22 : :
23 : : Further information about the GNU Lesser General Public License can also be found on
24 : : the world wide web at http://www.gnu.org.
25 : :
26 : :
27 : : HISTORY
28 : : --------------------------------------------------------------------------------
29 : : 7/1/99 TP Created
30 : :
31 : :
32 : : FUNCTIONAL DESCRIPTION
33 : : --------------------------------------------------------------------------------
34 : :
35 : : The purpose of this class is to take a set of initial conditions and provide
36 : : a kinematically consistent set of body axis velocity components, euler
37 : : angles, and altitude. This class does not attempt to trim the model i.e.
38 : : the sim will most likely start in a very dynamic state (unless, of course,
39 : : you have chosen your IC's wisely) even after setting it up with this class.
40 : :
41 : : ********************************************************************************
42 : : INCLUDES
43 : : *******************************************************************************/
44 : :
45 : : #include "FGInitialCondition.h"
46 : : #include "FGFDMExec.h"
47 : : #include "models/FGInertial.h"
48 : : #include "models/FGAtmosphere.h"
49 : : #include "models/FGAerodynamics.h"
50 : : #include "models/FGPropagate.h"
51 : : #include "input_output/FGPropertyManager.h"
52 : : #include "input_output/FGXMLElement.h"
53 : : #include "models/FGPropulsion.h"
54 : : #include "input_output/FGXMLParse.h"
55 : : #include "math/FGQuaternion.h"
56 : : #include <iostream>
57 : : #include <fstream>
58 : : #include <cstdlib>
59 : : #include "input_output/string_utilities.h"
60 : :
61 : : using namespace std;
62 : :
63 : : namespace JSBSim {
64 : :
65 : : static const char *IdSrc = "$Id: FGInitialCondition.cpp,v 1.40 2010/06/02 04:05:13 jberndt Exp $";
66 : : static const char *IdHdr = ID_INITIALCONDITION;
67 : :
68 : : //******************************************************************************
69 : :
70 : 1 : FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) : fdmex(FDMExec)
71 : : {
72 : 1 : InitializeIC();
73 : :
74 [ + - # # ]: 1 : if(FDMExec != NULL ) {
75 : 1 : fdmex->GetPropagate()->SetAltitudeASL(altitudeASL);
76 : 1 : fdmex->GetAtmosphere()->Run();
77 : 1 : PropertyManager=fdmex->GetPropertyManager();
78 : 1 : Constructing = true;
79 : 1 : bind();
80 : 1 : Constructing = false;
81 : : } else {
82 : 0 : cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
83 : : }
84 : :
85 : 1 : Debug(0);
86 : 1 : }
87 : :
88 : : //******************************************************************************
89 : :
90 : 1 : FGInitialCondition::~FGInitialCondition()
91 : : {
92 : 1 : Debug(1);
93 : 2 : }
94 : :
95 : : //******************************************************************************
96 : :
97 : : void FGInitialCondition::ResetIC(double u0, double v0, double w0,
98 : : double p0, double q0, double r0,
99 : : double alpha0, double beta0,
100 : : double phi0, double theta0, double psi0,
101 : : double latRad0, double lonRad0, double altAGLFt0,
102 : 0 : double gamma0)
103 : : {
104 : 0 : InitializeIC();
105 : :
106 : 0 : u = u0; v = v0; w = w0;
107 : 0 : p = p0; q = q0; r = r0;
108 : 0 : alpha = alpha0; beta = beta0;
109 : 0 : phi = phi0; theta = theta0; psi = psi0;
110 : 0 : gamma = gamma0;
111 : :
112 : 0 : latitude = latRad0;
113 : 0 : longitude = lonRad0;
114 : 0 : SetAltitudeAGLFtIC(altAGLFt0);
115 : :
116 : 0 : cphi = cos(phi); sphi = sin(phi); // phi, rad
117 : 0 : ctheta = cos(theta); stheta = sin(theta); // theta, rad
118 : 0 : cpsi = cos(psi); spsi = sin(psi); // psi, rad
119 : :
120 : 0 : FGQuaternion Quat( phi, theta, psi );
121 : 0 : Quat.Normalize();
122 : :
123 : 0 : const FGMatrix33& _Tl2b = Quat.GetT(); // local to body frame
124 : 0 : const FGMatrix33& _Tb2l = Quat.GetTInv(); // body to local
125 : :
126 : 0 : FGColumnVector3 _vUVW_BODY(u,v,w);
127 : 0 : FGColumnVector3 _vUVW_NED = _Tb2l * _vUVW_BODY;
128 : 0 : FGColumnVector3 _vWIND_NED(wnorth,weast,wdown);
129 : : // FGColumnVector3 _vUVWAero = _Tl2b * ( _vUVW_NED + _vWIND_NED );
130 : :
131 : 0 : uw=_vWIND_NED(1); vw=_vWIND_NED(2); ww=_vWIND_NED(3);
132 : :
133 : 0 : }
134 : :
135 : : //******************************************************************************
136 : :
137 : 1 : void FGInitialCondition::InitializeIC(void)
138 : : {
139 : 1 : vt=vc=ve=vg=0;
140 : 1 : mach=0;
141 : 1 : alpha=beta=gamma=0;
142 : 1 : theta=phi=psi=0;
143 : 1 : altitudeASL=hdot=0;
144 : 1 : latitude=longitude=0;
145 : 1 : u=v=w=0;
146 : 1 : p=q=r=0;
147 : 1 : uw=vw=ww=0;
148 : 1 : vnorth=veast=vdown=0;
149 : 1 : wnorth=weast=wdown=0;
150 : 1 : whead=wcross=0;
151 : 1 : wdir=wmag=0;
152 : 1 : lastSpeedSet=setvt;
153 : 1 : lastWindSet=setwned;
154 : 2 : radius_to_vehicle = sea_level_radius = fdmex->GetInertial()->GetRefRadius();
155 : 1 : terrain_elevation = 0;
156 : :
157 : 1 : targetNlfIC = 1.0;
158 : :
159 : 1 : salpha=sbeta=stheta=sphi=spsi=sgamma=0;
160 : 1 : calpha=cbeta=ctheta=cphi=cpsi=cgamma=1;
161 : 1 : }
162 : :
163 : : //******************************************************************************
164 : :
165 : 0 : void FGInitialCondition::WriteStateFile(int num)
166 : : {
167 [ # # ]: 0 : if (Constructing) return;
168 : :
169 : 0 : string filename = fdmex->GetFullAircraftPath();
170 : :
171 [ # # ]: 0 : if (filename.empty())
172 : : filename = "initfile.xml";
173 : : else
174 : 0 : filename.append("/initfile.xml");
175 : :
176 : 0 : ofstream outfile(filename.c_str());
177 : 0 : FGPropagate* Propagate = fdmex->GetPropagate();
178 : :
179 [ # # ]: 0 : if (outfile.is_open()) {
180 : 0 : outfile << "<?xml version=\"1.0\"?>" << endl;
181 : 0 : outfile << "<initialize name=\"reset00\">" << endl;
182 : 0 : outfile << " <ubody unit=\"FT/SEC\"> " << Propagate->GetUVW(eX) << " </ubody> " << endl;
183 : 0 : outfile << " <vbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eY) << " </vbody> " << endl;
184 : 0 : outfile << " <wbody unit=\"FT/SEC\"> " << Propagate->GetUVW(eZ) << " </wbody> " << endl;
185 : 0 : outfile << " <phi unit=\"DEG\"> " << Propagate->GetEuler(ePhi) << " </phi>" << endl;
186 : 0 : outfile << " <theta unit=\"DEG\"> " << Propagate->GetEuler(eTht) << " </theta>" << endl;
187 : 0 : outfile << " <psi unit=\"DEG\"> " << Propagate->GetEuler(ePsi) << " </psi>" << endl;
188 : 0 : outfile << " <longitude unit=\"DEG\"> " << Propagate->GetLongitudeDeg() << " </longitude>" << endl;
189 : 0 : outfile << " <latitude unit=\"DEG\"> " << Propagate->GetLatitudeDeg() << " </latitude>" << endl;
190 : 0 : outfile << " <altitude unit=\"FT\"> " << Propagate->GetAltitudeASL() << " </altitude>" << endl;
191 : 0 : outfile << "</initialize>" << endl;
192 : 0 : outfile.close();
193 : : } else {
194 : 0 : cerr << "Could not open and/or write the state to the initial conditions file: " << filename << endl;
195 : 0 : }
196 : : }
197 : :
198 : : //******************************************************************************
199 : :
200 : 0 : void FGInitialCondition::SetVcalibratedKtsIC(double tt) {
201 : :
202 [ # # ]: 0 : if(getMachFromVcas(&mach,tt*ktstofps)) {
203 : : //cout << "Mach: " << mach << endl;
204 : 0 : lastSpeedSet=setvc;
205 : 0 : vc=tt*ktstofps;
206 : 0 : vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
207 : 0 : ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
208 : : //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl;
209 : : }
210 : : else {
211 : 0 : cout << "Failed to get Mach number for given Vc and altitude, Vc unchanged." << endl;
212 : 0 : cout << "Please mail the set of initial conditions used to apeden@earthlink.net" << endl;
213 : : }
214 : 0 : }
215 : :
216 : : //******************************************************************************
217 : :
218 : 0 : void FGInitialCondition::SetVequivalentKtsIC(double tt) {
219 : 0 : ve=tt*ktstofps;
220 : 0 : lastSpeedSet=setve;
221 : 0 : vt=ve*1/sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
222 : 0 : mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
223 : 0 : vc=calcVcas(mach);
224 : 0 : }
225 : :
226 : : //******************************************************************************
227 : :
228 : 0 : void FGInitialCondition::SetVgroundFpsIC(double tt) {
229 : : double ua,va,wa;
230 : : double vxz;
231 : :
232 : 0 : vg=tt;
233 : 0 : lastSpeedSet=setvg;
234 : 0 : vnorth = vg*cos(psi); veast = vg*sin(psi); vdown = 0;
235 : 0 : calcUVWfromNED();
236 : 0 : ua = u + uw; va = v + vw; wa = w + ww;
237 : 0 : vt = sqrt( ua*ua + va*va + wa*wa );
238 : 0 : alpha = beta = 0;
239 : 0 : vxz = sqrt( u*u + w*w );
240 [ # # ]: 0 : if( w != 0 ) alpha = atan2( w, u );
241 [ # # ]: 0 : if( vxz != 0 ) beta = atan2( v, vxz );
242 : 0 : mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
243 : 0 : vc=calcVcas(mach);
244 : 0 : ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
245 : 0 : }
246 : :
247 : : //******************************************************************************
248 : :
249 : 1 : void FGInitialCondition::SetVtrueFpsIC(double tt) {
250 : 1 : vt=tt;
251 : 1 : lastSpeedSet=setvt;
252 : 2 : mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
253 : 1 : vc=calcVcas(mach);
254 : 1 : ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
255 : 1 : }
256 : :
257 : : //******************************************************************************
258 : :
259 : 0 : void FGInitialCondition::SetMachIC(double tt) {
260 : 0 : mach=tt;
261 : 0 : lastSpeedSet=setmach;
262 : 0 : vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
263 : 0 : vc=calcVcas(mach);
264 : 0 : ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
265 : : //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl;
266 : 0 : }
267 : :
268 : : //******************************************************************************
269 : :
270 : 0 : void FGInitialCondition::SetClimbRateFpmIC(double tt) {
271 : 0 : SetClimbRateFpsIC(tt/60.0);
272 : 0 : }
273 : :
274 : : //******************************************************************************
275 : :
276 : 0 : void FGInitialCondition::SetClimbRateFpsIC(double tt) {
277 : :
278 [ # # ]: 0 : if(vt > 0.1) {
279 : 0 : hdot=tt;
280 : 0 : gamma=asin(hdot/vt);
281 : 0 : sgamma=sin(gamma); cgamma=cos(gamma);
282 : : }
283 : 0 : }
284 : :
285 : : //******************************************************************************
286 : :
287 : 0 : void FGInitialCondition::SetFlightPathAngleRadIC(double tt) {
288 : 0 : gamma=tt;
289 : 0 : sgamma=sin(gamma); cgamma=cos(gamma);
290 : 0 : getTheta();
291 : 0 : hdot=vt*sgamma;
292 : 0 : }
293 : :
294 : : //******************************************************************************
295 : :
296 : 0 : void FGInitialCondition::SetAlphaRadIC(double tt) {
297 : 0 : alpha=tt;
298 : 0 : salpha=sin(alpha); calpha=cos(alpha);
299 : 0 : getTheta();
300 : 0 : }
301 : :
302 : : //******************************************************************************
303 : :
304 : 1 : void FGInitialCondition::SetThetaRadIC(double tt) {
305 : 1 : theta=tt;
306 : 1 : stheta=sin(theta); ctheta=cos(theta);
307 : 1 : getAlpha();
308 : 1 : }
309 : :
310 : : //******************************************************************************
311 : :
312 : 0 : void FGInitialCondition::SetBetaRadIC(double tt) {
313 : 0 : beta=tt;
314 : 0 : sbeta=sin(beta); cbeta=cos(beta);
315 : 0 : getTheta();
316 : :
317 : 0 : }
318 : :
319 : : //******************************************************************************
320 : :
321 : 1 : void FGInitialCondition::SetPhiRadIC(double tt) {
322 : 1 : phi=tt;
323 : 1 : sphi=sin(phi); cphi=cos(phi);
324 : 1 : getTheta();
325 : 1 : }
326 : :
327 : : //******************************************************************************
328 : :
329 : 1 : void FGInitialCondition::SetPsiRadIC(double tt) {
330 : 1 : psi=tt;
331 : 1 : spsi=sin(psi); cpsi=cos(psi);
332 : 1 : calcWindUVW();
333 : 1 : }
334 : :
335 : : //******************************************************************************
336 : :
337 : 1 : void FGInitialCondition::SetUBodyFpsIC(double tt) {
338 : 1 : u=tt;
339 : 1 : vt=sqrt(u*u + v*v + w*w);
340 : 1 : lastSpeedSet=setuvw;
341 : 1 : }
342 : :
343 : : //******************************************************************************
344 : :
345 : 1 : void FGInitialCondition::SetVBodyFpsIC(double tt) {
346 : 1 : v=tt;
347 : 1 : vt=sqrt(u*u + v*v + w*w);
348 : 1 : lastSpeedSet=setuvw;
349 : 1 : }
350 : :
351 : : //******************************************************************************
352 : :
353 : 1 : void FGInitialCondition::SetWBodyFpsIC(double tt) {
354 : 1 : w=tt;
355 : 1 : vt=sqrt( u*u + v*v + w*w );
356 : 1 : lastSpeedSet=setuvw;
357 : 1 : }
358 : :
359 : :
360 : : //******************************************************************************
361 : :
362 : 0 : void FGInitialCondition::SetVNorthFpsIC(double tt) {
363 : : double ua,va,wa;
364 : : double vxz;
365 : 0 : vnorth = tt;
366 : 0 : calcUVWfromNED();
367 : 0 : ua = u + uw; va = v + vw; wa = w + ww;
368 : 0 : vt = sqrt( ua*ua + va*va + wa*wa );
369 : 0 : alpha = beta = 0;
370 : 0 : vxz = sqrt( u*u + w*w );
371 [ # # ]: 0 : if( w != 0 ) alpha = atan2( w, u );
372 [ # # ]: 0 : if( vxz != 0 ) beta = atan2( v, vxz );
373 : 0 : mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
374 : 0 : vc=calcVcas(mach);
375 : 0 : ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
376 : 0 : lastSpeedSet=setned;
377 : 0 : }
378 : :
379 : : //******************************************************************************
380 : :
381 : 0 : void FGInitialCondition::SetVEastFpsIC(double tt) {
382 : : double ua,va,wa;
383 : : double vxz;
384 : 0 : veast = tt;
385 : 0 : calcUVWfromNED();
386 : 0 : ua = u + uw; va = v + vw; wa = w + ww;
387 : 0 : vt = sqrt( ua*ua + va*va + wa*wa );
388 : 0 : alpha = beta = 0;
389 : 0 : vxz = sqrt( u*u + w*w );
390 [ # # ]: 0 : if( w != 0 ) alpha = atan2( w, u );
391 [ # # ]: 0 : if( vxz != 0 ) beta = atan2( v, vxz );
392 : 0 : mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
393 : 0 : vc=calcVcas(mach);
394 : 0 : ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
395 : 0 : lastSpeedSet=setned;
396 : 0 : }
397 : :
398 : : //******************************************************************************
399 : :
400 : 0 : void FGInitialCondition::SetVDownFpsIC(double tt) {
401 : : double ua,va,wa;
402 : : double vxz;
403 : 0 : vdown = tt;
404 : 0 : calcUVWfromNED();
405 : 0 : ua = u + uw; va = v + vw; wa = w + ww;
406 : 0 : vt = sqrt( ua*ua + va*va + wa*wa );
407 : 0 : alpha = beta = 0;
408 : 0 : vxz = sqrt( u*u + w*w );
409 [ # # ]: 0 : if( w != 0 ) alpha = atan2( w, u );
410 [ # # ]: 0 : if( vxz != 0 ) beta = atan2( v, vxz );
411 : 0 : mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
412 : 0 : vc=calcVcas(mach);
413 : 0 : ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
414 : 0 : SetClimbRateFpsIC(-1*vdown);
415 : 0 : lastSpeedSet=setned;
416 : 0 : }
417 : :
418 : : //******************************************************************************
419 : :
420 : 2 : double FGInitialCondition::GetUBodyFpsIC(void) const {
421 [ - + ]: 2 : if (lastSpeedSet == setvg || lastSpeedSet == setned)
422 : 0 : return u;
423 : : else
424 : 2 : return vt*calpha*cbeta - uw;
425 : : }
426 : :
427 : : //******************************************************************************
428 : :
429 : 2 : double FGInitialCondition::GetVBodyFpsIC(void) const {
430 [ - + ]: 2 : if (lastSpeedSet == setvg || lastSpeedSet == setned)
431 : 0 : return v;
432 : : else {
433 : 2 : return vt*sbeta - vw;
434 : : }
435 : : }
436 : :
437 : : //******************************************************************************
438 : :
439 : 2 : double FGInitialCondition::GetWBodyFpsIC(void) const {
440 [ - + ]: 2 : if (lastSpeedSet == setvg || lastSpeedSet == setned)
441 : 0 : return w;
442 : : else
443 : 2 : return vt*salpha*cbeta -ww;
444 : : }
445 : :
446 : : //******************************************************************************
447 : :
448 : 0 : void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD ) {
449 : 0 : wnorth = wN; weast = wE; wdown = wD;
450 : 0 : lastWindSet = setwned;
451 : 0 : calcWindUVW();
452 [ # # ]: 0 : if(lastSpeedSet == setvg)
453 : 0 : SetVgroundFpsIC(vg);
454 : 0 : }
455 : :
456 : : //******************************************************************************
457 : :
458 : 0 : void FGInitialCondition::SetCrossWindKtsIC(double cross){
459 : 0 : wcross=cross*ktstofps;
460 : 0 : lastWindSet=setwhc;
461 : 0 : calcWindUVW();
462 [ # # ]: 0 : if(lastSpeedSet == setvg)
463 : 0 : SetVgroundFpsIC(vg);
464 : :
465 : 0 : }
466 : :
467 : : //******************************************************************************
468 : :
469 : : // positive from left
470 : 0 : void FGInitialCondition::SetHeadWindKtsIC(double head){
471 : 0 : whead=head*ktstofps;
472 : 0 : lastWindSet=setwhc;
473 : 0 : calcWindUVW();
474 [ # # ]: 0 : if(lastSpeedSet == setvg)
475 : 0 : SetVgroundFpsIC(vg);
476 : :
477 : 0 : }
478 : :
479 : : //******************************************************************************
480 : :
481 : 0 : void FGInitialCondition::SetWindDownKtsIC(double wD) {
482 : 0 : wdown=wD;
483 : 0 : calcWindUVW();
484 [ # # ]: 0 : if(lastSpeedSet == setvg)
485 : 0 : SetVgroundFpsIC(vg);
486 : 0 : }
487 : :
488 : : //******************************************************************************
489 : :
490 : 0 : void FGInitialCondition::SetWindMagKtsIC(double mag) {
491 : 0 : wmag=mag*ktstofps;
492 : 0 : lastWindSet=setwmd;
493 : 0 : calcWindUVW();
494 [ # # ]: 0 : if(lastSpeedSet == setvg)
495 : 0 : SetVgroundFpsIC(vg);
496 : 0 : }
497 : :
498 : : //******************************************************************************
499 : :
500 : 0 : void FGInitialCondition::SetWindDirDegIC(double dir) {
501 : 0 : wdir=dir*degtorad;
502 : 0 : lastWindSet=setwmd;
503 : 0 : calcWindUVW();
504 [ # # ]: 0 : if(lastSpeedSet == setvg)
505 : 0 : SetVgroundFpsIC(vg);
506 : 0 : }
507 : :
508 : :
509 : : //******************************************************************************
510 : :
511 : 1 : void FGInitialCondition::calcWindUVW(void) {
512 : :
513 [ - - + ]: 1 : switch(lastWindSet) {
514 : : case setwmd:
515 : 0 : wnorth=wmag*cos(wdir);
516 : 0 : weast=wmag*sin(wdir);
517 : 0 : break;
518 : : case setwhc:
519 : 0 : wnorth=whead*cos(psi) + wcross*cos(psi+M_PI/2);
520 : 0 : weast=whead*sin(psi) + wcross*sin(psi+M_PI/2);
521 : : break;
522 : : case setwned:
523 : : break;
524 : : }
525 : : uw=wnorth*ctheta*cpsi +
526 : : weast*ctheta*spsi -
527 : 1 : wdown*stheta;
528 : : vw=wnorth*( sphi*stheta*cpsi - cphi*spsi ) +
529 : : weast*( sphi*stheta*spsi + cphi*cpsi ) +
530 : 1 : wdown*sphi*ctheta;
531 : : ww=wnorth*(cphi*stheta*cpsi + sphi*spsi) +
532 : : weast*(cphi*stheta*spsi - sphi*cpsi) +
533 : 1 : wdown*cphi*ctheta;
534 : :
535 : :
536 : : /* cout << "FGInitialCondition::calcWindUVW: wnorth, weast, wdown "
537 : : << wnorth << ", " << weast << ", " << wdown << endl;
538 : : cout << "FGInitialCondition::calcWindUVW: theta, phi, psi "
539 : : << theta << ", " << phi << ", " << psi << endl;
540 : : cout << "FGInitialCondition::calcWindUVW: uw, vw, ww "
541 : : << uw << ", " << vw << ", " << ww << endl; */
542 : :
543 : 1 : }
544 : :
545 : : //******************************************************************************
546 : :
547 : 1 : void FGInitialCondition::SetAltitudeASLFtIC(double tt)
548 : : {
549 : 1 : altitudeASL=tt;
550 : 1 : fdmex->GetPropagate()->SetAltitudeASL(altitudeASL);
551 : 1 : fdmex->GetAtmosphere()->Run();
552 : : //lets try to make sure the user gets what they intended
553 : :
554 [ + - - - : 1 : switch(lastSpeedSet) {
- - ]
555 : : case setned:
556 : : case setuvw:
557 : : case setvt:
558 : 1 : SetVtrueKtsIC(vt*fpstokts);
559 : 1 : break;
560 : : case setvc:
561 : 0 : SetVcalibratedKtsIC(vc*fpstokts);
562 : 0 : break;
563 : : case setve:
564 : 0 : SetVequivalentKtsIC(ve*fpstokts);
565 : 0 : break;
566 : : case setmach:
567 : 0 : SetMachIC(mach);
568 : 0 : break;
569 : : case setvg:
570 : 0 : SetVgroundFpsIC(vg);
571 : : break;
572 : : }
573 : 1 : }
574 : :
575 : : //******************************************************************************
576 : :
577 : 1 : void FGInitialCondition::SetAltitudeAGLFtIC(double tt)
578 : : {
579 : 1 : SetAltitudeASLFtIC(terrain_elevation + tt);
580 : 1 : }
581 : :
582 : : //******************************************************************************
583 : :
584 : 0 : void FGInitialCondition::SetSeaLevelRadiusFtIC(double tt)
585 : : {
586 : 0 : sea_level_radius = tt;
587 : 0 : }
588 : :
589 : : //******************************************************************************
590 : :
591 : 0 : void FGInitialCondition::SetTerrainElevationFtIC(double tt)
592 : : {
593 : 0 : terrain_elevation=tt;
594 : 0 : }
595 : :
596 : : //******************************************************************************
597 : :
598 : 0 : void FGInitialCondition::calcUVWfromNED(void)
599 : : {
600 : : u=vnorth*ctheta*cpsi +
601 : : veast*ctheta*spsi -
602 : 0 : vdown*stheta;
603 : : v=vnorth*( sphi*stheta*cpsi - cphi*spsi ) +
604 : : veast*( sphi*stheta*spsi + cphi*cpsi ) +
605 : 0 : vdown*sphi*ctheta;
606 : : w=vnorth*( cphi*stheta*cpsi + sphi*spsi ) +
607 : : veast*( cphi*stheta*spsi - sphi*cpsi ) +
608 : 0 : vdown*cphi*ctheta;
609 : 0 : }
610 : :
611 : : //******************************************************************************
612 : :
613 : 0 : bool FGInitialCondition::getMachFromVcas(double *Mach,double vcas) {
614 : :
615 : 0 : bool result=false;
616 : 0 : double guess=1.5;
617 : 0 : xlo=xhi=0;
618 : 0 : xmin=0;xmax=50;
619 : 0 : sfunc=&FGInitialCondition::calcVcas;
620 [ # # ]: 0 : if(findInterval(vcas,guess)) {
621 [ # # ]: 0 : if(solve(&mach,vcas))
622 : 0 : result=true;
623 : : }
624 : 0 : return result;
625 : : }
626 : :
627 : : //******************************************************************************
628 : :
629 : 1 : bool FGInitialCondition::getAlpha(void) {
630 : 1 : bool result=false;
631 : 1 : double guess=theta-gamma;
632 : :
633 [ + - ]: 1 : if(vt < 0.01) return 0;
634 : :
635 : 0 : xlo=xhi=0;
636 : 0 : xmin=fdmex->GetAerodynamics()->GetAlphaCLMin();
637 : 0 : xmax=fdmex->GetAerodynamics()->GetAlphaCLMax();
638 : 0 : sfunc=&FGInitialCondition::GammaEqOfAlpha;
639 [ # # ]: 0 : if(findInterval(0,guess)){
640 [ # # ]: 0 : if(solve(&alpha,0)){
641 : 0 : result=true;
642 : 0 : salpha=sin(alpha);
643 : 0 : calpha=cos(alpha);
644 : : }
645 : : }
646 : 0 : calcWindUVW();
647 : 1 : return result;
648 : : }
649 : :
650 : : //******************************************************************************
651 : :
652 : 1 : bool FGInitialCondition::getTheta(void) {
653 : 1 : bool result=false;
654 : 1 : double guess=alpha+gamma;
655 : :
656 [ + - ]: 1 : if(vt < 0.01) return 0;
657 : :
658 : 0 : xlo=xhi=0;
659 : 0 : xmin=-89;xmax=89;
660 : 0 : sfunc=&FGInitialCondition::GammaEqOfTheta;
661 [ # # ]: 0 : if(findInterval(0,guess)){
662 [ # # ]: 0 : if(solve(&theta,0)){
663 : 0 : result=true;
664 : 0 : stheta=sin(theta);
665 : 0 : ctheta=cos(theta);
666 : : }
667 : : }
668 : 0 : calcWindUVW();
669 : 1 : return result;
670 : : }
671 : :
672 : : //******************************************************************************
673 : :
674 : 0 : double FGInitialCondition::GammaEqOfTheta(double Theta) {
675 : : double a,b,c;
676 : : double sTheta,cTheta;
677 : :
678 : : //theta=Theta; stheta=sin(theta); ctheta=cos(theta);
679 : 0 : sTheta=sin(Theta); cTheta=cos(Theta);
680 : 0 : calcWindUVW();
681 : 0 : a=wdown + vt*calpha*cbeta + uw;
682 : 0 : b=vt*sphi*sbeta + vw*sphi;
683 : 0 : c=vt*cphi*salpha*cbeta + ww*cphi;
684 : 0 : return vt*sgamma - ( a*sTheta - (b+c)*cTheta);
685 : : }
686 : :
687 : : //******************************************************************************
688 : :
689 : 0 : double FGInitialCondition::GammaEqOfAlpha(double Alpha) {
690 : : double a,b,c;
691 : : double sAlpha,cAlpha;
692 : 0 : sAlpha=sin(Alpha); cAlpha=cos(Alpha);
693 : 0 : a=wdown + vt*cAlpha*cbeta + uw;
694 : 0 : b=vt*sphi*sbeta + vw*sphi;
695 : 0 : c=vt*cphi*sAlpha*cbeta + ww*cphi;
696 : :
697 : 0 : return vt*sgamma - ( a*stheta - (b+c)*ctheta );
698 : : }
699 : :
700 : : //******************************************************************************
701 : :
702 : 1 : double FGInitialCondition::calcVcas(double Mach) {
703 : :
704 : 2 : double p=fdmex->GetAtmosphere()->GetPressure();
705 : 2 : double psl=fdmex->GetAtmosphere()->GetPressureSL();
706 : 2 : double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
707 : : double pt,A,B,D,vcas;
708 : :
709 [ - + ]: 1 : if (Mach < 0) Mach=0;
710 [ + - ]: 1 : if (Mach < 1) //calculate total pressure assuming isentropic flow
711 : 1 : pt=p*pow((1 + 0.2*Mach*Mach),3.5);
712 : : else {
713 : : // shock in front of pitot tube, we'll assume its normal and use
714 : : // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
715 : : // pressure behind the shock to the static pressure in front of
716 : : // the normal shock assumption should not be a bad one -- most supersonic
717 : : // aircraft place the pitot probe out front so that it is the forward
718 : : // most point on the aircraft. The real shock would, of course, take
719 : : // on something like the shape of a rounded-off cone but, here again,
720 : : // the assumption should be good since the opening of the pitot probe
721 : : // is very small and, therefore, the effects of the shock curvature
722 : : // should be small as well. AFAIK, this approach is fairly well accepted
723 : : // within the aerospace community
724 : :
725 : 0 : B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);
726 : :
727 : : // The denominator above is zero for Mach ~ 0.38, for which
728 : : // we'll never be here, so we're safe
729 : :
730 : 0 : D = (2.8*Mach*Mach-0.4)*0.4167;
731 : 0 : pt = p*pow(B,3.5)*D;
732 : : }
733 : :
734 : 1 : A = pow(((pt-p)/psl+1),0.28571);
735 : 1 : vcas = sqrt(7*psl/rhosl*(A-1));
736 : : //cout << "calcVcas: vcas= " << vcas*fpstokts << " mach= " << Mach << " pressure: " << pt << endl;
737 : 1 : return vcas;
738 : : }
739 : :
740 : : //******************************************************************************
741 : :
742 : 0 : bool FGInitialCondition::findInterval(double x,double guess) {
743 : : //void find_interval(inter_params &ip,eqfunc f,double y,double constant, int &flag){
744 : :
745 : 0 : int i=0;
746 : 0 : bool found=false;
747 : : double flo,fhi,fguess;
748 : : double lo,hi,step;
749 : 0 : step=0.1;
750 [ # # ]: 0 : fguess=(this->*sfunc)(guess)-x;
751 : 0 : lo=hi=guess;
752 [ # # ][ # # ]: 0 : do {
753 : 0 : step=2*step;
754 : 0 : lo-=step;
755 : 0 : hi+=step;
756 [ # # ]: 0 : if(lo < xmin) lo=xmin;
757 [ # # ]: 0 : if(hi > xmax) hi=xmax;
758 : 0 : i++;
759 [ # # ]: 0 : flo=(this->*sfunc)(lo)-x;
760 [ # # ]: 0 : fhi=(this->*sfunc)(hi)-x;
761 [ # # ]: 0 : if(flo*fhi <=0) { //found interval with root
762 : 0 : found=true;
763 [ # # ]: 0 : if(flo*fguess <= 0) { //narrow interval down a bit
764 : 0 : hi=lo+step; //to pass solver interval that is as
765 : : //small as possible
766 : : }
767 [ # # ]: 0 : else if(fhi*fguess <= 0) {
768 : 0 : lo=hi-step;
769 : : }
770 : : }
771 : : //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
772 : : }
773 : : while((found == 0) && (i <= 100));
774 : 0 : xlo=lo;
775 : 0 : xhi=hi;
776 : 0 : return found;
777 : : }
778 : :
779 : : //******************************************************************************
780 : :
781 : 0 : bool FGInitialCondition::solve(double *y,double x)
782 : : {
783 : : double x1,x2,x3,f1,f2,f3,d,d0;
784 : 0 : double eps=1E-5;
785 : 0 : double const relax =0.9;
786 : : int i;
787 : 0 : bool success=false;
788 : :
789 : : //initializations
790 : 0 : d=1;
791 : 0 : x2 = 0;
792 : 0 : x1=xlo;x3=xhi;
793 [ # # ]: 0 : f1=(this->*sfunc)(x1)-x;
794 [ # # ]: 0 : f3=(this->*sfunc)(x3)-x;
795 : 0 : d0=fabs(x3-x1);
796 : :
797 : : //iterations
798 : 0 : i=0;
799 [ # # ][ # # ]: 0 : while ((fabs(d) > eps) && (i < 100)) {
800 : 0 : d=(x3-x1)/d0;
801 : 0 : x2 = x1-d*d0*f1/(f3-f1);
802 : :
803 [ # # ]: 0 : f2=(this->*sfunc)(x2)-x;
804 : : //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
805 : : //cout << " " << f1 << "," << f2 << "," << f3 << endl;
806 : :
807 [ # # ]: 0 : if(fabs(f2) <= 0.001) {
808 : 0 : x1=x3=x2;
809 [ # # ]: 0 : } else if(f1*f2 <= 0.0) {
810 : 0 : x3=x2;
811 : 0 : f3=f2;
812 : 0 : f1=relax*f1;
813 [ # # ]: 0 : } else if(f2*f3 <= 0) {
814 : 0 : x1=x2;
815 : 0 : f1=f2;
816 : 0 : f3=relax*f3;
817 : : }
818 : : //cout << i << endl;
819 : 0 : i++;
820 : : }//end while
821 [ # # ]: 0 : if(i < 100) {
822 : 0 : success=true;
823 : 0 : *y=x2;
824 : : }
825 : :
826 : : //cout << "Success= " << success << " Vcas: " << vcas*fpstokts << " Mach: " << x2 << endl;
827 : 0 : return success;
828 : : }
829 : :
830 : : //******************************************************************************
831 : :
832 : 1 : double FGInitialCondition::GetWindDirDegIC(void) const {
833 [ - + ]: 1 : if(weast != 0.0)
834 : 0 : return atan2(weast,wnorth)*radtodeg;
835 [ - + ]: 1 : else if(wnorth > 0)
836 : 0 : return 0.0;
837 : : else
838 : 1 : return 180.0;
839 : : }
840 : :
841 : : //******************************************************************************
842 : :
843 : 1 : bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
844 : : {
845 : 1 : string sep = "/";
846 [ + - ]: 1 : if( useStoredPath ) {
847 : 2 : init_file_name = fdmex->GetFullAircraftPath() + sep + rstfile + ".xml";
848 : : } else {
849 : 0 : init_file_name = rstfile;
850 : : }
851 : :
852 : 2 : document = LoadXMLDocument(init_file_name);
853 : :
854 : : // Make sure that the document is valid
855 [ - + ]: 1 : if (!document) {
856 : 0 : cerr << "File: " << init_file_name << " could not be read." << endl;
857 : 0 : exit(-1);
858 : : }
859 : :
860 [ - + ]: 2 : if (document->GetName() != string("initialize")) {
861 : 0 : cerr << "File: " << init_file_name << " is not a reset file." << endl;
862 : 0 : exit(-1);
863 : : }
864 : :
865 : 1 : double version = document->GetAttributeValueAsNumber("version");
866 [ + - ]: 1 : if (version == HUGE_VAL) {
867 : 1 : return Load_v1(); // Default to the old version
868 [ # # ]: 0 : } else if (version >= 3.0) {
869 : 0 : cerr << "Only initialization file formats 1 and 2 are currently supported" << endl;
870 : 0 : exit (-1);
871 [ # # ]: 0 : } else if (version >= 2.0) {
872 : 0 : return Load_v2();
873 [ # # ]: 0 : } else if (version >= 1.0) {
874 : 0 : return Load_v1();
875 : 1 : }
876 : :
877 : 0 : }
878 : :
879 : : //******************************************************************************
880 : :
881 : 1 : bool FGInitialCondition::Load_v1(void)
882 : : {
883 : : int n;
884 : :
885 [ + - ]: 1 : if (document->FindElement("latitude"))
886 : 1 : SetLatitudeDegIC(document->FindElementValueAsNumberConvertTo("latitude", "DEG"));
887 [ + - ]: 1 : if (document->FindElement("longitude"))
888 : 1 : SetLongitudeDegIC(document->FindElementValueAsNumberConvertTo("longitude", "DEG"));
889 [ - + ]: 1 : if (document->FindElement("elevation"))
890 : 0 : SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT"));
891 : :
892 [ + - ]: 1 : if (document->FindElement("altitude")) // This is feet above ground level
893 : 1 : SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT"));
894 [ # # ]: 0 : else if (document->FindElement("altitudeAGL")) // This is feet above ground level
895 : 0 : SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
896 [ # # ]: 0 : else if (document->FindElement("altitudeMSL")) // This is feet above sea level
897 : 0 : SetAltitudeASLFtIC(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
898 : :
899 [ + - ]: 1 : if (document->FindElement("ubody"))
900 : 1 : SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo("ubody", "FT/SEC"));
901 [ + - ]: 1 : if (document->FindElement("vbody"))
902 : 1 : SetVBodyFpsIC(document->FindElementValueAsNumberConvertTo("vbody", "FT/SEC"));
903 [ + - ]: 1 : if (document->FindElement("wbody"))
904 : 1 : SetWBodyFpsIC(document->FindElementValueAsNumberConvertTo("wbody", "FT/SEC"));
905 [ - + ]: 1 : if (document->FindElement("vnorth"))
906 : 0 : SetVNorthFpsIC(document->FindElementValueAsNumberConvertTo("vnorth", "FT/SEC"));
907 [ - + ]: 1 : if (document->FindElement("veast"))
908 : 0 : SetVEastFpsIC(document->FindElementValueAsNumberConvertTo("veast", "FT/SEC"));
909 [ - + ]: 1 : if (document->FindElement("vdown"))
910 : 0 : SetVDownFpsIC(document->FindElementValueAsNumberConvertTo("vdown", "FT/SEC"));
911 [ - + ]: 1 : if (document->FindElement("winddir"))
912 : 0 : SetWindDirDegIC(document->FindElementValueAsNumberConvertTo("winddir", "DEG"));
913 [ - + ]: 1 : if (document->FindElement("vwind"))
914 : 0 : SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo("vwind", "KTS"));
915 [ - + ]: 1 : if (document->FindElement("hwind"))
916 : 0 : SetHeadWindKtsIC(document->FindElementValueAsNumberConvertTo("hwind", "KTS"));
917 [ - + ]: 1 : if (document->FindElement("xwind"))
918 : 0 : SetCrossWindKtsIC(document->FindElementValueAsNumberConvertTo("xwind", "KTS"));
919 [ - + ]: 1 : if (document->FindElement("vc"))
920 : 0 : SetVcalibratedKtsIC(document->FindElementValueAsNumberConvertTo("vc", "KTS"));
921 [ - + ]: 1 : if (document->FindElement("vt"))
922 : 0 : SetVtrueKtsIC(document->FindElementValueAsNumberConvertTo("vt", "KTS"));
923 [ - + ]: 1 : if (document->FindElement("mach"))
924 : 0 : SetMachIC(document->FindElementValueAsNumber("mach"));
925 [ + - ]: 1 : if (document->FindElement("phi"))
926 : 1 : SetPhiDegIC(document->FindElementValueAsNumberConvertTo("phi", "DEG"));
927 [ + - ]: 1 : if (document->FindElement("theta"))
928 : 1 : SetThetaDegIC(document->FindElementValueAsNumberConvertTo("theta", "DEG"));
929 [ + - ]: 1 : if (document->FindElement("psi"))
930 : 2 : SetPsiDegIC(document->FindElementValueAsNumberConvertTo("psi", "DEG"));
931 [ - + ]: 1 : if (document->FindElement("alpha"))
932 : 0 : SetAlphaDegIC(document->FindElementValueAsNumberConvertTo("alpha", "DEG"));
933 [ - + ]: 1 : if (document->FindElement("beta"))
934 : 0 : SetBetaDegIC(document->FindElementValueAsNumberConvertTo("beta", "DEG"));
935 [ - + ]: 1 : if (document->FindElement("gamma"))
936 : 0 : SetFlightPathAngleDegIC(document->FindElementValueAsNumberConvertTo("gamma", "DEG"));
937 [ - + ]: 1 : if (document->FindElement("roc"))
938 : 0 : SetClimbRateFpsIC(document->FindElementValueAsNumberConvertTo("roc", "FT/SEC"));
939 [ - + ]: 1 : if (document->FindElement("vground"))
940 : 0 : SetVgroundKtsIC(document->FindElementValueAsNumberConvertTo("vground", "KTS"));
941 [ - + ]: 1 : if (document->FindElement("targetNlf"))
942 : : {
943 : 0 : SetTargetNlfIC(document->FindElementValueAsNumber("targetNlf"));
944 : : }
945 : :
946 : : // Check to see if any engines are specified to be initialized in a running state
947 : 2 : FGPropulsion* propulsion = fdmex->GetPropulsion();
948 : 1 : Element* running_elements = document->FindElement("running");
949 [ - + ]: 1 : while (running_elements) {
950 : 0 : n = int(running_elements->GetDataAsNumber());
951 : 0 : propulsion->InitRunning(n);
952 : 0 : running_elements = document->FindNextElement("running");
953 : : }
954 : :
955 : 1 : fdmex->RunIC();
956 : :
957 : 1 : return true;
958 : : }
959 : :
960 : : //******************************************************************************
961 : :
962 : 0 : bool FGInitialCondition::Load_v2(void)
963 : : {
964 : : int n;
965 : 0 : FGColumnVector3 vLoc, vOrient;
966 : 0 : bool result = true;
967 : 0 : FGInertial* Inertial = fdmex->GetInertial();
968 : 0 : FGPropagate* Propagate = fdmex->GetPropagate();
969 : :
970 [ # # ]: 0 : if (document->FindElement("earth_position_angle")) {
971 : 0 : double epa = document->FindElementValueAsNumberConvertTo("earth_position_angle", "RAD");
972 : : Inertial->SetEarthPositionAngle(epa);
973 : : }
974 : :
975 : : // Initialize vehicle position
976 : : //
977 : : // Allowable frames:
978 : : // - ECI (Earth Centered Inertial)
979 : : // - ECEF (Earth Centered, Earth Fixed)
980 : :
981 : 0 : Element* position = document->FindElement("position");
982 [ # # ]: 0 : if (position) {
983 : 0 : vLoc = position->FindElementTripletConvertTo("FT");
984 : 0 : string frame = position->GetAttributeValue("frame");
985 : 0 : frame = to_lower(frame);
986 [ # # ]: 0 : if (frame == "eci") {
987 : : // Need to transform vLoc to ECEF for storage and use in FGLocation.
988 : 0 : vLoc = Propagate->GetTi2ec()*vLoc;
989 [ # # ]: 0 : } else if (frame == "ecef") {
990 : : // Move vLoc query until after lat/lon/alt query to eliminate spurious warning msgs.
991 [ # # ]: 0 : if (vLoc.Magnitude() == 0.0) {
992 : 0 : Propagate->SetLatitudeDeg(position->FindElementValueAsNumberConvertTo("latitude", "DEG"));
993 : 0 : Propagate->SetLongitudeDeg(position->FindElementValueAsNumberConvertTo("longitude", "DEG"));
994 [ # # ]: 0 : if (position->FindElement("radius")) {
995 : 0 : radius_to_vehicle = position->FindElementValueAsNumberConvertTo("radius", "FT");
996 : 0 : SetAltitudeASLFtIC(radius_to_vehicle - sea_level_radius);
997 [ # # ]: 0 : } else if (position->FindElement("altitudeAGL")) {
998 : 0 : SetAltitudeAGLFtIC(position->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
999 [ # # ]: 0 : } else if (position->FindElement("altitudeMSL")) {
1000 : 0 : SetAltitudeASLFtIC(position->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
1001 : : } else {
1002 : 0 : cerr << endl << " No altitude or radius initial condition is given." << endl;
1003 : 0 : result = false;
1004 : : }
1005 : : }
1006 : : } else {
1007 : 0 : cerr << endl << " Neither ECI nor ECEF frame is specified for initial position." << endl;
1008 : 0 : result = false;
1009 : 0 : }
1010 : : } else {
1011 : 0 : cerr << endl << " Initial position not specified in this initialization file." << endl;
1012 : 0 : result = false;
1013 : : }
1014 : :
1015 : : // End of position initialization
1016 : :
1017 : : // Initialize vehicle orientation
1018 : : // Allowable frames
1019 : : // - ECI (Earth Centered Inertial)
1020 : : // - ECEF (Earth Centered, Earth Fixed)
1021 : : // - Local
1022 : : //
1023 : : // Need to convert the provided orientation to an ECI orientation, using
1024 : : // the given orientation and knowledge of the Earth position angle.
1025 : : // This could be done using matrices (where in the subscript "b/a",
1026 : : // it is meant "b with respect to a", and where b=body frame,
1027 : : // i=inertial frame, and e=ecef frame) as:
1028 : : //
1029 : : // C_b/i = C_b/e * C_e/i
1030 : : //
1031 : : // Using quaternions (note reverse ordering compared to matrix representation):
1032 : : //
1033 : : // Q_b/i = Q_e/i * Q_b/e
1034 : : //
1035 : : // Use the specific matrices as needed. The above example of course is for the whole
1036 : : // body to inertial orientation.
1037 : : // The new orientation angles can be extracted from the matrix or the quaternion.
1038 : : // ToDo: Do we need to deal with normalization of the quaternions here?
1039 : :
1040 : 0 : Element* orientation_el = document->FindElement("orientation");
1041 [ # # ]: 0 : if (orientation_el) {
1042 : 0 : string frame = orientation_el->GetAttributeValue("frame");
1043 : 0 : frame = to_lower(frame);
1044 : 0 : vOrient = orientation_el->FindElementTripletConvertTo("RAD");
1045 : : FGQuaternion QuatI2Body;
1046 [ # # ]: 0 : if (frame == "eci") {
1047 : :
1048 : 0 : QuatI2Body = FGQuaternion(vOrient);
1049 : :
1050 [ # # ]: 0 : } else if (frame == "ecef") {
1051 : :
1052 : : // In this case we are given the Euler angles representing the orientation of
1053 : : // the body with respect to the ECEF system, represented by the C_b/e Matrix.
1054 : : // We want the body orientation with respect to the inertial system:
1055 : : //
1056 : : // C_b/i = C_b/e * C_e/i
1057 : : //
1058 : : // Using quaternions (note reverse ordering compared to matrix representation):
1059 : : //
1060 : : // Q_b/i = Q_e/i * Q_b/e
1061 : :
1062 : 0 : FGQuaternion QuatEC2Body(vOrient); // Store relationship of Body frame wrt ECEF frame, Q_b/e
1063 : 0 : FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix
1064 : 0 : QuatI2Body = QuatI2EC * QuatEC2Body; // Q_b/i = Q_e/i * Q_b/e
1065 : :
1066 [ # # ]: 0 : } else if (frame == "local") {
1067 : :
1068 : : // In this case, we are supplying the Euler angles for the vehicle with
1069 : : // respect to the local (NED frame), also called the navigation frame.
1070 : : // This is the most common way of initializing the orientation of
1071 : : // aircraft. The matrix representation is:
1072 : : //
1073 : : // C_b/i = C_b/n * C_n/e * C_e/i
1074 : : //
1075 : : // Or, using quaternions (note reverse ordering compared to matrix representation):
1076 : : //
1077 : : // Q_b/i = Q_e/i * Q_n/e * Q_b/n
1078 : :
1079 : 0 : FGQuaternion QuatLocal2Body = FGQuaternion(vOrient); // Store relationship of Body frame wrt local (NED) frame, Q_b/n
1080 : 0 : FGQuaternion QuatEC2Local = Propagate->GetTec2l(); // Get Q_n/e from matrix
1081 : 0 : FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix
1082 : 0 : QuatI2Body = QuatI2EC * QuatEC2Local * QuatLocal2Body; // Q_b/i = Q_e/i * Q_n/e * Q_b/n
1083 : :
1084 : : } else {
1085 : :
1086 : : cerr << endl << fgred << " Orientation frame type: \"" << frame
1087 : 0 : << "\" is not supported!" << reset << endl << endl;
1088 : 0 : result = false;
1089 : :
1090 : : }
1091 : :
1092 : 0 : Propagate->SetInertialOrientation(QuatI2Body);
1093 : : }
1094 : :
1095 : : // Initialize vehicle velocity
1096 : : // Allowable frames
1097 : : // - ECI (Earth Centered Inertial)
1098 : : // - ECEF (Earth Centered, Earth Fixed)
1099 : : // - Local
1100 : : // - Body
1101 : : // The vehicle will be defaulted to (0,0,0) in the Body frame if nothing is provided.
1102 : :
1103 : 0 : Element* velocity_el = document->FindElement("velocity");
1104 : 0 : FGColumnVector3 vInertialVelocity;
1105 : : FGColumnVector3 vInitVelocity = FGColumnVector3(0.0, 0.0, 0.0);
1106 [ # # ]: 0 : if (velocity_el) {
1107 : :
1108 : 0 : string frame = velocity_el->GetAttributeValue("frame");
1109 : 0 : frame = to_lower(frame);
1110 : 0 : FGColumnVector3 vInitVelocity = velocity_el->FindElementTripletConvertTo("FT/SEC");
1111 : : FGColumnVector3 omega_cross_r = Inertial->omega() * Propagate->GetInertialPosition();
1112 : :
1113 [ # # ]: 0 : if (frame == "eci") {
1114 : : vInertialVelocity = vInitVelocity;
1115 [ # # ]: 0 : } else if (frame == "ecef") {
1116 : 0 : FGMatrix33 mTec2i = Propagate->GetTec2i(); // Get C_i/e
1117 : 0 : vInertialVelocity = mTec2i * vInitVelocity + omega_cross_r;
1118 [ # # ]: 0 : } else if (frame == "local") {
1119 : 0 : FGMatrix33 mTl2i = Propagate->GetTl2i();
1120 : 0 : vInertialVelocity = mTl2i * vInitVelocity + omega_cross_r;
1121 [ # # ]: 0 : } else if (frame == "body") {
1122 : 0 : FGMatrix33 mTb2i = Propagate->GetTb2i();
1123 : 0 : vInertialVelocity = mTb2i * vInitVelocity + omega_cross_r;
1124 : : } else {
1125 : :
1126 : : cerr << endl << fgred << " Velocity frame type: \"" << frame
1127 : 0 : << "\" is not supported!" << reset << endl << endl;
1128 : 0 : result = false;
1129 : :
1130 : 0 : }
1131 : :
1132 : : } else {
1133 : :
1134 : 0 : FGMatrix33 mTb2i = Propagate->GetTb2i();
1135 : 0 : vInertialVelocity = mTb2i * vInitVelocity + (Inertial->omega() * Propagate->GetInertialPosition());
1136 : :
1137 : : }
1138 : :
1139 : 0 : Propagate->SetInertialVelocity(vInertialVelocity);
1140 : :
1141 : : // Allowable frames
1142 : : // - ECI (Earth Centered Inertial)
1143 : : // - ECEF (Earth Centered, Earth Fixed)
1144 : : // - Body
1145 : :
1146 : 0 : FGColumnVector3 vInertialRate;
1147 : 0 : Element* attrate_el = document->FindElement("attitude_rate");
1148 [ # # ]: 0 : if (attrate_el) {
1149 : :
1150 : 0 : string frame = attrate_el->GetAttributeValue("frame");
1151 : 0 : frame = to_lower(frame);
1152 : 0 : FGColumnVector3 vAttRate = attrate_el->FindElementTripletConvertTo("RAD/SEC");
1153 : :
1154 [ # # ]: 0 : if (frame == "eci") {
1155 : : vInertialRate = vAttRate;
1156 [ # # ]: 0 : } else if (frame == "ecef") {
1157 : : // vInertialRate = vAttRate + Inertial->omega();
1158 [ # # ]: 0 : } else if (frame == "body") {
1159 : : //Todo: determine local frame rate
1160 : 0 : FGMatrix33 mTb2l = Propagate->GetTb2l();
1161 : : // vInertialRate = mTb2l*vAttRate + Inertial->omega();
1162 [ # # ]: 0 : } else if (!frame.empty()) { // misspelling of frame
1163 : :
1164 : : cerr << endl << fgred << " Attitude rate frame type: \"" << frame
1165 : 0 : << "\" is not supported!" << reset << endl << endl;
1166 : 0 : result = false;
1167 : :
1168 : 0 : } else if (frame.empty()) {
1169 : :
1170 : 0 : }
1171 : :
1172 : : } else { // Body frame attitude rate assumed 0 relative to local.
1173 : : /*
1174 : : //Todo: determine local frame rate
1175 : :
1176 : : FGMatrix33 mTi2l = Propagate->GetTi2l();
1177 : : vVel = mTi2l * vInertialVelocity;
1178 : :
1179 : : // Compute the local frame ECEF velocity
1180 : : vVel = Tb2l * VState.vUVW;
1181 : :
1182 : : FGColumnVector3 vOmegaLocal = FGColumnVector3(
1183 : : radInv*vVel(eEast),
1184 : : -radInv*vVel(eNorth),
1185 : : -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
1186 : : */
1187 : : }
1188 : :
1189 : : // Check to see if any engines are specified to be initialized in a running state
1190 : 0 : FGPropulsion* propulsion = fdmex->GetPropulsion();
1191 : 0 : Element* running_elements = document->FindElement("running");
1192 [ # # ]: 0 : while (running_elements) {
1193 : 0 : n = int(running_elements->GetDataAsNumber());
1194 : 0 : propulsion->InitRunning(n);
1195 : 0 : running_elements = document->FindNextElement("running");
1196 : : }
1197 : :
1198 : : // fdmex->RunIC();
1199 : :
1200 : 0 : return result;
1201 : : }
1202 : :
1203 : : //******************************************************************************
1204 : :
1205 : 1 : void FGInitialCondition::bind(void){
1206 : : PropertyManager->Tie("ic/vc-kts", this,
1207 : : &FGInitialCondition::GetVcalibratedKtsIC,
1208 : : &FGInitialCondition::SetVcalibratedKtsIC,
1209 : 1 : true);
1210 : : PropertyManager->Tie("ic/ve-kts", this,
1211 : : &FGInitialCondition::GetVequivalentKtsIC,
1212 : : &FGInitialCondition::SetVequivalentKtsIC,
1213 : 1 : true);
1214 : : PropertyManager->Tie("ic/vg-kts", this,
1215 : : &FGInitialCondition::GetVgroundKtsIC,
1216 : : &FGInitialCondition::SetVgroundKtsIC,
1217 : 1 : true);
1218 : : PropertyManager->Tie("ic/vt-kts", this,
1219 : : &FGInitialCondition::GetVtrueKtsIC,
1220 : : &FGInitialCondition::SetVtrueKtsIC,
1221 : 1 : true);
1222 : : PropertyManager->Tie("ic/mach", this,
1223 : : &FGInitialCondition::GetMachIC,
1224 : : &FGInitialCondition::SetMachIC,
1225 : 1 : true);
1226 : : PropertyManager->Tie("ic/roc-fpm", this,
1227 : : &FGInitialCondition::GetClimbRateFpmIC,
1228 : : &FGInitialCondition::SetClimbRateFpmIC,
1229 : 1 : true);
1230 : : PropertyManager->Tie("ic/gamma-deg", this,
1231 : : &FGInitialCondition::GetFlightPathAngleDegIC,
1232 : : &FGInitialCondition::SetFlightPathAngleDegIC,
1233 : 1 : true);
1234 : : PropertyManager->Tie("ic/alpha-deg", this,
1235 : : &FGInitialCondition::GetAlphaDegIC,
1236 : : &FGInitialCondition::SetAlphaDegIC,
1237 : 1 : true);
1238 : : PropertyManager->Tie("ic/beta-deg", this,
1239 : : &FGInitialCondition::GetBetaDegIC,
1240 : : &FGInitialCondition::SetBetaDegIC,
1241 : 1 : true);
1242 : : PropertyManager->Tie("ic/theta-deg", this,
1243 : : &FGInitialCondition::GetThetaDegIC,
1244 : : &FGInitialCondition::SetThetaDegIC,
1245 : 1 : true);
1246 : : PropertyManager->Tie("ic/phi-deg", this,
1247 : : &FGInitialCondition::GetPhiDegIC,
1248 : : &FGInitialCondition::SetPhiDegIC,
1249 : 1 : true);
1250 : : PropertyManager->Tie("ic/psi-true-deg", this,
1251 : 1 : &FGInitialCondition::GetPsiDegIC );
1252 : : PropertyManager->Tie("ic/lat-gc-deg", this,
1253 : : &FGInitialCondition::GetLatitudeDegIC,
1254 : : &FGInitialCondition::SetLatitudeDegIC,
1255 : 1 : true);
1256 : : PropertyManager->Tie("ic/long-gc-deg", this,
1257 : : &FGInitialCondition::GetLongitudeDegIC,
1258 : : &FGInitialCondition::SetLongitudeDegIC,
1259 : 1 : true);
1260 : : PropertyManager->Tie("ic/h-sl-ft", this,
1261 : : &FGInitialCondition::GetAltitudeASLFtIC,
1262 : : &FGInitialCondition::SetAltitudeASLFtIC,
1263 : 1 : true);
1264 : : PropertyManager->Tie("ic/h-agl-ft", this,
1265 : : &FGInitialCondition::GetAltitudeAGLFtIC,
1266 : : &FGInitialCondition::SetAltitudeAGLFtIC,
1267 : 1 : true);
1268 : : PropertyManager->Tie("ic/sea-level-radius-ft", this,
1269 : : &FGInitialCondition::GetSeaLevelRadiusFtIC,
1270 : : &FGInitialCondition::SetSeaLevelRadiusFtIC,
1271 : 1 : true);
1272 : : PropertyManager->Tie("ic/terrain-elevation-ft", this,
1273 : : &FGInitialCondition::GetTerrainElevationFtIC,
1274 : : &FGInitialCondition::SetTerrainElevationFtIC,
1275 : 1 : true);
1276 : : PropertyManager->Tie("ic/vg-fps", this,
1277 : : &FGInitialCondition::GetVgroundFpsIC,
1278 : : &FGInitialCondition::SetVgroundFpsIC,
1279 : 1 : true);
1280 : : PropertyManager->Tie("ic/vt-fps", this,
1281 : : &FGInitialCondition::GetVtrueFpsIC,
1282 : : &FGInitialCondition::SetVtrueFpsIC,
1283 : 1 : true);
1284 : : PropertyManager->Tie("ic/vw-bx-fps", this,
1285 : 1 : &FGInitialCondition::GetWindUFpsIC);
1286 : : PropertyManager->Tie("ic/vw-by-fps", this,
1287 : 1 : &FGInitialCondition::GetWindVFpsIC);
1288 : : PropertyManager->Tie("ic/vw-bz-fps", this,
1289 : 1 : &FGInitialCondition::GetWindWFpsIC);
1290 : : PropertyManager->Tie("ic/vw-north-fps", this,
1291 : 1 : &FGInitialCondition::GetWindNFpsIC);
1292 : : PropertyManager->Tie("ic/vw-east-fps", this,
1293 : 1 : &FGInitialCondition::GetWindEFpsIC);
1294 : : PropertyManager->Tie("ic/vw-down-fps", this,
1295 : 1 : &FGInitialCondition::GetWindDFpsIC);
1296 : : PropertyManager->Tie("ic/vw-mag-fps", this,
1297 : 1 : &FGInitialCondition::GetWindFpsIC);
1298 : : PropertyManager->Tie("ic/vw-dir-deg", this,
1299 : : &FGInitialCondition::GetWindDirDegIC,
1300 : : &FGInitialCondition::SetWindDirDegIC,
1301 : 1 : true);
1302 : :
1303 : : PropertyManager->Tie("ic/roc-fps", this,
1304 : : &FGInitialCondition::GetClimbRateFpsIC,
1305 : : &FGInitialCondition::SetClimbRateFpsIC,
1306 : 1 : true);
1307 : : PropertyManager->Tie("ic/u-fps", this,
1308 : : &FGInitialCondition::GetUBodyFpsIC,
1309 : : &FGInitialCondition::SetUBodyFpsIC,
1310 : 1 : true);
1311 : : PropertyManager->Tie("ic/v-fps", this,
1312 : : &FGInitialCondition::GetVBodyFpsIC,
1313 : : &FGInitialCondition::SetVBodyFpsIC,
1314 : 1 : true);
1315 : : PropertyManager->Tie("ic/w-fps", this,
1316 : : &FGInitialCondition::GetWBodyFpsIC,
1317 : : &FGInitialCondition::SetWBodyFpsIC,
1318 : 1 : true);
1319 : : PropertyManager->Tie("ic/vn-fps", this,
1320 : : &FGInitialCondition::GetVNorthFpsIC,
1321 : : &FGInitialCondition::SetVNorthFpsIC,
1322 : 1 : true);
1323 : : PropertyManager->Tie("ic/ve-fps", this,
1324 : : &FGInitialCondition::GetVEastFpsIC,
1325 : : &FGInitialCondition::SetVEastFpsIC,
1326 : 1 : true);
1327 : : PropertyManager->Tie("ic/vd-fps", this,
1328 : : &FGInitialCondition::GetVDownFpsIC,
1329 : : &FGInitialCondition::SetVDownFpsIC,
1330 : 1 : true);
1331 : : PropertyManager->Tie("ic/gamma-rad", this,
1332 : : &FGInitialCondition::GetFlightPathAngleRadIC,
1333 : : &FGInitialCondition::SetFlightPathAngleRadIC,
1334 : 1 : true);
1335 : : PropertyManager->Tie("ic/alpha-rad", this,
1336 : : &FGInitialCondition::GetAlphaRadIC,
1337 : : &FGInitialCondition::SetAlphaRadIC,
1338 : 1 : true);
1339 : : PropertyManager->Tie("ic/theta-rad", this,
1340 : : &FGInitialCondition::GetThetaRadIC,
1341 : : &FGInitialCondition::SetThetaRadIC,
1342 : 1 : true);
1343 : : PropertyManager->Tie("ic/beta-rad", this,
1344 : : &FGInitialCondition::GetBetaRadIC,
1345 : : &FGInitialCondition::SetBetaRadIC,
1346 : 1 : true);
1347 : : PropertyManager->Tie("ic/phi-rad", this,
1348 : : &FGInitialCondition::GetPhiRadIC,
1349 : : &FGInitialCondition::SetPhiRadIC,
1350 : 1 : true);
1351 : : PropertyManager->Tie("ic/psi-true-rad", this,
1352 : 1 : &FGInitialCondition::GetPsiRadIC);
1353 : : PropertyManager->Tie("ic/lat-gc-rad", this,
1354 : : &FGInitialCondition::GetLatitudeRadIC,
1355 : : &FGInitialCondition::SetLatitudeRadIC,
1356 : 1 : true);
1357 : : PropertyManager->Tie("ic/long-gc-rad", this,
1358 : : &FGInitialCondition::GetLongitudeRadIC,
1359 : : &FGInitialCondition::SetLongitudeRadIC,
1360 : 1 : true);
1361 : : PropertyManager->Tie("ic/p-rad_sec", this,
1362 : : &FGInitialCondition::GetPRadpsIC,
1363 : : &FGInitialCondition::SetPRadpsIC,
1364 : 1 : true);
1365 : : PropertyManager->Tie("ic/q-rad_sec", this,
1366 : : &FGInitialCondition::GetQRadpsIC,
1367 : : &FGInitialCondition::SetQRadpsIC,
1368 : 1 : true);
1369 : : PropertyManager->Tie("ic/r-rad_sec", this,
1370 : : &FGInitialCondition::GetRRadpsIC,
1371 : : &FGInitialCondition::SetRRadpsIC,
1372 : 1 : true);
1373 : :
1374 : : typedef int (FGInitialCondition::*iPMF)(void) const;
1375 : : PropertyManager->Tie("simulation/write-state-file",
1376 : : this,
1377 : : (iPMF)0,
1378 : 2 : &FGInitialCondition::WriteStateFile);
1379 : :
1380 : 1 : }
1381 : :
1382 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
1383 : : // The bitmasked value choices are as follows:
1384 : : // unset: In this case (the default) JSBSim would only print
1385 : : // out the normally expected messages, essentially echoing
1386 : : // the config files as they are read. If the environment
1387 : : // variable is not set, debug_lvl is set to 1 internally
1388 : : // 0: This requests JSBSim not to output any messages
1389 : : // whatsoever.
1390 : : // 1: This value explicity requests the normal JSBSim
1391 : : // startup messages
1392 : : // 2: This value asks for a message to be printed out when
1393 : : // a class is instantiated
1394 : : // 4: When this value is set, a message is displayed when a
1395 : : // FGModel object executes its Run() method
1396 : : // 8: When this value is set, various runtime state variables
1397 : : // are printed out periodically
1398 : : // 16: When set various parameters are sanity checked and
1399 : : // a message is printed out when they go out of bounds
1400 : :
1401 : 2 : void FGInitialCondition::Debug(int from)
1402 : : {
1403 [ + - ]: 2 : if (debug_lvl <= 0) return;
1404 : :
1405 : 2 : if (debug_lvl & 1) { // Standard console startup message output
1406 : : }
1407 [ - + ]: 2 : if (debug_lvl & 2 ) { // Instantiation/Destruction notification
1408 [ # # ]: 0 : if (from == 0) cout << "Instantiated: FGInitialCondition" << endl;
1409 [ # # ]: 0 : if (from == 1) cout << "Destroyed: FGInitialCondition" << endl;
1410 : : }
1411 : 2 : if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
1412 : : }
1413 : 2 : if (debug_lvl & 8 ) { // Runtime state variables
1414 : : }
1415 : 2 : if (debug_lvl & 16) { // Sanity checking
1416 : : }
1417 [ - + ]: 2 : if (debug_lvl & 64) {
1418 [ # # ]: 0 : if (from == 0) { // Constructor
1419 : 0 : cout << IdSrc << endl;
1420 : 0 : cout << IdHdr << endl;
1421 : : }
1422 : : }
1423 : : }
1424 [ + + ][ + - ]: 12 : }
|
