1 /*******************************************************************************
2
3 Header: FGInitialCondition.cpp
4 Author: Tony Peden, Bertrand Coconnier
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
11 Software Foundation; either version 2 of the License, or (at your option) any
12 later 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
20 with this program; if not, write to the Free Software Foundation, Inc., 59
21 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
22
23 Further information about the GNU Lesser General Public License can also be
24 found on the world wide web at http://www.gnu.org.
25
26
27 HISTORY
28 --------------------------------------------------------------------------------
29 7/1/99 TP Created
30 11/25/10 BC Complete revision - Use minimal set of variables to prevent
31 inconsistent states. Wind is correctly handled.
32
33
34 FUNCTIONAL DESCRIPTION
35 --------------------------------------------------------------------------------
36
37 The purpose of this class is to take a set of initial conditions and provide a
38 kinematically consistent set of body axis velocity components, euler angles, and
39 altitude. This class does not attempt to trim the model i.e. the sim will most
40 likely start in a very dynamic state (unless, of course, you have chosen your
41 IC's wisely) even after setting it up with this class.
42
43 ********************************************************************************
44 INCLUDES
45 *******************************************************************************/
46
47 #include "FGInitialCondition.h"
48 #include "models/FGInertial.h"
49 #include "models/FGAtmosphere.h"
50 #include "models/FGAccelerations.h"
51 #include "input_output/FGXMLFileRead.h"
52 #include "FGTrim.h"
53
54 using namespace std;
55
56 namespace JSBSim {
57
58 //******************************************************************************
59
FGInitialCondition(FGFDMExec * FDMExec)60 FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) : fdmex(FDMExec)
61 {
62 InitializeIC();
63
64 if(FDMExec) {
65 Atmosphere=fdmex->GetAtmosphere();
66 Aircraft=fdmex->GetAircraft();
67 } else {
68 cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
69 }
70
71 Debug(0);
72 }
73
74 //******************************************************************************
75
~FGInitialCondition()76 FGInitialCondition::~FGInitialCondition()
77 {
78 Debug(1);
79 }
80
81 //******************************************************************************
82
ResetIC(double u0,double v0,double w0,double p0,double q0,double r0,double alpha0,double beta0,double phi0,double theta0,double psi0,double latRad0,double lonRad0,double altAGLFt0,double gamma0)83 void FGInitialCondition::ResetIC(double u0, double v0, double w0,
84 double p0, double q0, double r0,
85 double alpha0, double beta0,
86 double phi0, double theta0, double psi0,
87 double latRad0, double lonRad0, double altAGLFt0,
88 double gamma0)
89 {
90 double calpha = cos(alpha0), cbeta = cos(beta0);
91 double salpha = sin(alpha0), sbeta = sin(beta0);
92
93 InitializeIC();
94
95 vPQR_body = FGColumnVector3(p0, q0, r0);
96 alpha = alpha0; beta = beta0;
97
98 position.SetLongitude(lonRad0);
99 position.SetLatitude(latRad0);
100 position.SetAltitudeAGL(altAGLFt0);
101 lastLatitudeSet = setgeoc;
102 lastAltitudeSet = setagl;
103
104 orientation = FGQuaternion(phi0, theta0, psi0);
105 const FGMatrix33& Tb2l = orientation.GetTInv();
106
107 vUVW_NED = Tb2l * FGColumnVector3(u0, v0, w0);
108 vt = vUVW_NED.Magnitude();
109 lastSpeedSet = setuvw;
110
111 Tw2b = FGMatrix33(calpha*cbeta, -calpha*sbeta, -salpha,
112 sbeta, cbeta, 0.0,
113 salpha*cbeta, -salpha*sbeta, calpha);
114 Tb2w = Tw2b.Transposed();
115
116 SetFlightPathAngleRadIC(gamma0);
117 }
118
119 //******************************************************************************
120
InitializeIC(void)121 void FGInitialCondition::InitializeIC(void)
122 {
123 alpha = beta = 0.0;
124 epa = 0.0;
125 a = fdmex->GetInertial()->GetSemimajor();
126 double b = fdmex->GetInertial()->GetSemiminor();
127 double ec = b/a;
128 e2 = 1.0 - ec*ec;
129
130 position.SetEllipse(a, b);
131
132 position.SetPositionGeodetic(0.0, 0.0, 0.0);
133
134 orientation = FGQuaternion(0.0, 0.0, 0.0);
135 vUVW_NED.InitMatrix();
136 vPQR_body.InitMatrix();
137 vt=0;
138
139 targetNlfIC = 1.0;
140
141 Tw2b.InitMatrix(1., 0., 0., 0., 1., 0., 0., 0., 1.);
142 Tb2w.InitMatrix(1., 0., 0., 0., 1., 0., 0., 0., 1.);
143
144 lastSpeedSet = setvt;
145 lastAltitudeSet = setasl;
146 lastLatitudeSet = setgeoc;
147 enginesRunning = 0;
148 trimRequested = TrimMode::tNone;
149 }
150
151 //******************************************************************************
152
SetVequivalentKtsIC(double ve)153 void FGInitialCondition::SetVequivalentKtsIC(double ve)
154 {
155 double altitudeASL = position.GetAltitudeASL();
156 double rho = Atmosphere->GetDensity(altitudeASL);
157 double rhoSL = Atmosphere->GetDensitySL();
158 SetVtrueFpsIC(ve*ktstofps*sqrt(rhoSL/rho));
159 lastSpeedSet = setve;
160 }
161
162 //******************************************************************************
163
SetMachIC(double mach)164 void FGInitialCondition::SetMachIC(double mach)
165 {
166 double altitudeASL = position.GetAltitudeASL();
167 double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
168 SetVtrueFpsIC(mach*soundSpeed);
169 lastSpeedSet = setmach;
170 }
171
172 //******************************************************************************
173
SetVcalibratedKtsIC(double vcas)174 void FGInitialCondition::SetVcalibratedKtsIC(double vcas)
175 {
176 double altitudeASL = position.GetAltitudeASL();
177 double pressure = Atmosphere->GetPressure(altitudeASL);
178 double mach = MachFromVcalibrated(fabs(vcas)*ktstofps, pressure);
179 double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
180
181 SetVtrueFpsIC(mach * soundSpeed);
182 lastSpeedSet = setvc;
183 }
184
185 //******************************************************************************
186 // Updates alpha and beta according to the aircraft true airspeed in the local
187 // NED frame.
188
calcAeroAngles(const FGColumnVector3 & _vt_NED)189 void FGInitialCondition::calcAeroAngles(const FGColumnVector3& _vt_NED)
190 {
191 const FGMatrix33& Tl2b = orientation.GetT();
192 FGColumnVector3 _vt_BODY = Tl2b * _vt_NED;
193 double ua = _vt_BODY(eX);
194 double va = _vt_BODY(eY);
195 double wa = _vt_BODY(eZ);
196 double uwa = sqrt(ua*ua + wa*wa);
197 double calpha, cbeta;
198 double salpha, sbeta;
199
200 alpha = beta = 0.0;
201 calpha = cbeta = 1.0;
202 salpha = sbeta = 0.0;
203
204 if( wa != 0 )
205 alpha = atan2( wa, ua );
206
207 // alpha cannot be constrained without updating other informations like the
208 // true speed or the Euler angles. Otherwise we might end up with an
209 // inconsistent state of the aircraft.
210 /*alpha = Constrain(fdmex->GetAerodynamics()->GetAlphaCLMin(), alpha,
211 fdmex->GetAerodynamics()->GetAlphaCLMax());*/
212
213 if( va != 0 )
214 beta = atan2( va, uwa );
215
216 if (uwa != 0) {
217 calpha = ua / uwa;
218 salpha = wa / uwa;
219 }
220
221 if (vt != 0) {
222 cbeta = uwa / vt;
223 sbeta = va / vt;
224 }
225
226 Tw2b = FGMatrix33(calpha*cbeta, -calpha*sbeta, -salpha,
227 sbeta, cbeta, 0.0,
228 salpha*cbeta, -salpha*sbeta, calpha);
229 Tb2w = Tw2b.Transposed();
230 }
231
232 //******************************************************************************
233 // Set the ground velocity. Caution it sets the vertical velocity to zero to
234 // keep backward compatibility.
235
SetVgroundFpsIC(double vg)236 void FGInitialCondition::SetVgroundFpsIC(double vg)
237 {
238 const FGMatrix33& Tb2l = orientation.GetTInv();
239 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
240 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
241
242 vUVW_NED(eU) = vg * orientation.GetCosEuler(ePsi);
243 vUVW_NED(eV) = vg * orientation.GetSinEuler(ePsi);
244 vUVW_NED(eW) = 0.;
245 _vt_NED = vUVW_NED + _vWIND_NED;
246 vt = _vt_NED.Magnitude();
247
248 calcAeroAngles(_vt_NED);
249
250 lastSpeedSet = setvg;
251 }
252
253 //******************************************************************************
254 // Sets the true airspeed. The amplitude of the airspeed is modified but its
255 // direction is kept unchanged. If there is no wind, the same is true for the
256 // ground velocity. If there is some wind, the airspeed direction is unchanged
257 // but this may result in the ground velocity direction being altered. This is
258 // for backward compatibility.
259
SetVtrueFpsIC(double vtrue)260 void FGInitialCondition::SetVtrueFpsIC(double vtrue)
261 {
262 const FGMatrix33& Tb2l = orientation.GetTInv();
263 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
264 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
265
266 if (vt > 0.1)
267 _vt_NED *= vtrue / vt;
268 else
269 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vtrue, 0., 0.);
270
271 vt = vtrue;
272 vUVW_NED = _vt_NED - _vWIND_NED;
273
274 calcAeroAngles(_vt_NED);
275
276 lastSpeedSet = setvt;
277 }
278
279 //******************************************************************************
280 // When the climb rate is modified, we need to update the angles theta and beta
281 // to keep the true airspeed amplitude, the AoA and the heading unchanged.
282 // Beta will be modified if the aircraft roll angle is not null.
283
SetClimbRateFpsIC(double hdot)284 void FGInitialCondition::SetClimbRateFpsIC(double hdot)
285 {
286 if (fabs(hdot) > vt) {
287 cerr << "The climb rate cannot be higher than the true speed." << endl;
288 return;
289 }
290
291 const FGMatrix33& Tb2l = orientation.GetTInv();
292 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
293 FGColumnVector3 _WIND_NED = _vt_NED - vUVW_NED;
294 double hdot0 = -_vt_NED(eW);
295
296 if (fabs(hdot0) < vt) { // Is this check really needed ?
297 double scale = sqrt((vt*vt-hdot*hdot)/(vt*vt-hdot0*hdot0));
298 _vt_NED(eU) *= scale;
299 _vt_NED(eV) *= scale;
300 }
301 _vt_NED(eW) = -hdot;
302 vUVW_NED = _vt_NED - _WIND_NED;
303
304 // Updating the angles theta and beta to keep the true airspeed amplitude
305 calcThetaBeta(alpha, _vt_NED);
306 }
307
308 //******************************************************************************
309 // When the AoA is modified, we need to update the angles theta and beta to
310 // keep the true airspeed amplitude, the climb rate and the heading unchanged.
311 // Beta will be modified if the aircraft roll angle is not null.
312
SetAlphaRadIC(double alfa)313 void FGInitialCondition::SetAlphaRadIC(double alfa)
314 {
315 const FGMatrix33& Tb2l = orientation.GetTInv();
316 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
317 calcThetaBeta(alfa, _vt_NED);
318 }
319
320 //******************************************************************************
321 // When the AoA is modified, we need to update the angles theta and beta to
322 // keep the true airspeed amplitude, the climb rate and the heading unchanged.
323 // Beta will be modified if the aircraft roll angle is not null.
324
calcThetaBeta(double alfa,const FGColumnVector3 & _vt_NED)325 void FGInitialCondition::calcThetaBeta(double alfa, const FGColumnVector3& _vt_NED)
326 {
327 FGColumnVector3 vOrient = orientation.GetEuler();
328 double calpha = cos(alfa), salpha = sin(alfa);
329 double cpsi = orientation.GetCosEuler(ePsi), spsi = orientation.GetSinEuler(ePsi);
330 double cphi = orientation.GetCosEuler(ePhi), sphi = orientation.GetSinEuler(ePhi);
331 FGMatrix33 Tpsi( cpsi, spsi, 0.,
332 -spsi, cpsi, 0.,
333 0., 0., 1.);
334 FGMatrix33 Tphi(1., 0., 0.,
335 0., cphi, sphi,
336 0.,-sphi, cphi);
337 FGMatrix33 Talpha( calpha, 0., salpha,
338 0., 1., 0.,
339 -salpha, 0., calpha);
340
341 FGColumnVector3 v0 = Tpsi * _vt_NED;
342 FGColumnVector3 n = (Talpha * Tphi).Transposed() * FGColumnVector3(0., 0., 1.);
343 FGColumnVector3 y = FGColumnVector3(0., 1., 0.);
344 FGColumnVector3 u = y - DotProduct(y, n) * n;
345 FGColumnVector3 p = y * n;
346
347 if (DotProduct(p, v0) < 0) p *= -1.0;
348 p.Normalize();
349
350 u *= DotProduct(v0, y) / DotProduct(u, y);
351
352 // There are situations where the desired alpha angle cannot be obtained. This
353 // is not a limitation of the algorithm but is due to the mathematical problem
354 // not having a solution. This can only be cured by limiting the alpha angle
355 // or by modifying an additional angle (psi ?). Since this is anticipated to
356 // be a pathological case (mainly when a high roll angle is required) this
357 // situation is not addressed below. However if there are complaints about the
358 // following error being raised too often, we might need to reconsider this
359 // position.
360 if (DotProduct(v0, v0) < DotProduct(u, u)) {
361 cerr << "Cannot modify angle 'alpha' from " << alpha << " to " << alfa << endl;
362 return;
363 }
364
365 FGColumnVector3 v1 = u + sqrt(DotProduct(v0, v0) - DotProduct(u, u))*p;
366
367 FGColumnVector3 v0xz(v0(eU), 0., v0(eW));
368 FGColumnVector3 v1xz(v1(eU), 0., v1(eW));
369 v0xz.Normalize();
370 v1xz.Normalize();
371 double sinTheta = (v1xz * v0xz)(eY);
372 vOrient(eTht) = asin(sinTheta);
373
374 orientation = FGQuaternion(vOrient);
375
376 const FGMatrix33& Tl2b = orientation.GetT();
377 FGColumnVector3 v2 = Talpha * Tl2b * _vt_NED;
378
379 alpha = alfa;
380 beta = atan2(v2(eV), v2(eU));
381 double cbeta=1.0, sbeta=0.0;
382 if (vt != 0.0) {
383 cbeta = v2(eU) / vt;
384 sbeta = v2(eV) / vt;
385 }
386 Tw2b = FGMatrix33(calpha*cbeta, -calpha*sbeta, -salpha,
387 sbeta, cbeta, 0.0,
388 salpha*cbeta, -salpha*sbeta, calpha);
389 Tb2w = Tw2b.Transposed();
390 }
391
392 //******************************************************************************
393 // When the beta angle is modified, we need to update the angles theta and psi
394 // to keep the true airspeed (amplitude and direction - including the climb rate)
395 // and the alpha angle unchanged. This may result in the aircraft heading (psi)
396 // being altered especially if there is cross wind.
397
SetBetaRadIC(double bta)398 void FGInitialCondition::SetBetaRadIC(double bta)
399 {
400 const FGMatrix33& Tb2l = orientation.GetTInv();
401 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
402 FGColumnVector3 vOrient = orientation.GetEuler();
403
404 beta = bta;
405 double calpha = cos(alpha), salpha = sin(alpha);
406 double cbeta = cos(beta), sbeta = sin(beta);
407 double cphi = orientation.GetCosEuler(ePhi), sphi = orientation.GetSinEuler(ePhi);
408 FGMatrix33 TphiInv(1., 0., 0.,
409 0., cphi,-sphi,
410 0., sphi, cphi);
411
412 Tw2b = FGMatrix33(calpha*cbeta, -calpha*sbeta, -salpha,
413 sbeta, cbeta, 0.0,
414 salpha*cbeta, -salpha*sbeta, calpha);
415 Tb2w = Tw2b.Transposed();
416
417 FGColumnVector3 vf = TphiInv * Tw2b * FGColumnVector3(vt, 0., 0.);
418 FGColumnVector3 v0xy(_vt_NED(eX), _vt_NED(eY), 0.);
419 FGColumnVector3 v1xy(sqrt(v0xy(eX)*v0xy(eX)+v0xy(eY)*v0xy(eY)-vf(eY)*vf(eY)),vf(eY),0.);
420 v0xy.Normalize();
421 v1xy.Normalize();
422
423 if (vf(eX) < 0.) v0xy(eX) *= -1.0;
424
425 double sinPsi = (v1xy * v0xy)(eZ);
426 double cosPsi = DotProduct(v0xy, v1xy);
427 vOrient(ePsi) = atan2(sinPsi, cosPsi);
428 FGMatrix33 Tpsi( cosPsi, sinPsi, 0.,
429 -sinPsi, cosPsi, 0.,
430 0., 0., 1.);
431
432 FGColumnVector3 v2xz = Tpsi * _vt_NED;
433 FGColumnVector3 vfxz = vf;
434 v2xz(eV) = vfxz(eV) = 0.0;
435 v2xz.Normalize();
436 vfxz.Normalize();
437 double sinTheta = (v2xz * vfxz)(eY);
438 vOrient(eTht) = -asin(sinTheta);
439
440 orientation = FGQuaternion(vOrient);
441 }
442
443 //******************************************************************************
444 // Modifies the body frame orientation.
445
SetEulerAngleRadIC(int idx,double angle)446 void FGInitialCondition::SetEulerAngleRadIC(int idx, double angle)
447 {
448 const FGMatrix33& Tb2l = orientation.GetTInv();
449 const FGMatrix33& Tl2b = orientation.GetT();
450 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
451 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
452 FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
453 FGColumnVector3 vOrient = orientation.GetEuler();
454
455 vOrient(idx) = angle;
456 orientation = FGQuaternion(vOrient);
457
458 if ((lastSpeedSet != setned) && (lastSpeedSet != setvg)) {
459 const FGMatrix33& newTb2l = orientation.GetTInv();
460 vUVW_NED = newTb2l * _vUVW_BODY;
461 _vt_NED = vUVW_NED + _vWIND_NED;
462 vt = _vt_NED.Magnitude();
463 }
464
465 calcAeroAngles(_vt_NED);
466 }
467
468 //******************************************************************************
469 // Modifies an aircraft velocity component (eU, eV or eW) in the body frame. The
470 // true airspeed is modified accordingly. If there is some wind, the airspeed
471 // direction modification may differ from the body velocity modification.
472
SetBodyVelFpsIC(int idx,double vel)473 void FGInitialCondition::SetBodyVelFpsIC(int idx, double vel)
474 {
475 const FGMatrix33& Tb2l = orientation.GetTInv();
476 const FGMatrix33& Tl2b = orientation.GetT();
477 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
478 FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
479 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
480
481 _vUVW_BODY(idx) = vel;
482 vUVW_NED = Tb2l * _vUVW_BODY;
483 _vt_NED = vUVW_NED + _vWIND_NED;
484 vt = _vt_NED.Magnitude();
485
486 calcAeroAngles(_vt_NED);
487
488 lastSpeedSet = setuvw;
489 }
490
491 //******************************************************************************
492 // Modifies an aircraft velocity component (eX, eY or eZ) in the local NED frame.
493 // The true airspeed is modified accordingly. If there is some wind, the airspeed
494 // direction modification may differ from the local velocity modification.
495
SetNEDVelFpsIC(int idx,double vel)496 void FGInitialCondition::SetNEDVelFpsIC(int idx, double vel)
497 {
498 const FGMatrix33& Tb2l = orientation.GetTInv();
499 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
500 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
501
502 vUVW_NED(idx) = vel;
503 _vt_NED = vUVW_NED + _vWIND_NED;
504 vt = _vt_NED.Magnitude();
505
506 calcAeroAngles(_vt_NED);
507
508 lastSpeedSet = setned;
509 }
510
511 //******************************************************************************
512 // Set wind amplitude and direction in the local NED frame. The aircraft velocity
513 // with respect to the ground is not changed but the true airspeed is.
514
SetWindNEDFpsIC(double wN,double wE,double wD)515 void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD )
516 {
517 FGColumnVector3 _vt_NED = vUVW_NED + FGColumnVector3(wN, wE, wD);
518 vt = _vt_NED.Magnitude();
519
520 calcAeroAngles(_vt_NED);
521 }
522
523 //******************************************************************************
524 // Set the cross wind velocity (in knots). Here, 'cross wind' means perpendicular
525 // to the aircraft heading and parallel to the ground. The aircraft velocity
526 // with respect to the ground is not changed but the true airspeed is.
527
SetCrossWindKtsIC(double cross)528 void FGInitialCondition::SetCrossWindKtsIC(double cross)
529 {
530 const FGMatrix33& Tb2l = orientation.GetTInv();
531 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
532 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
533 FGColumnVector3 _vCROSS(-orientation.GetSinEuler(ePsi), orientation.GetCosEuler(ePsi), 0.);
534
535 // Gram-Schmidt process is used to remove the existing cross wind component
536 _vWIND_NED -= DotProduct(_vWIND_NED, _vCROSS) * _vCROSS;
537 // Which is now replaced by the new value. The input cross wind is expected
538 // in knots, so first convert to fps, which is the internal unit used.
539 _vWIND_NED += (cross * ktstofps) * _vCROSS;
540 _vt_NED = vUVW_NED + _vWIND_NED;
541 vt = _vt_NED.Magnitude();
542
543 calcAeroAngles(_vt_NED);
544 }
545
546 //******************************************************************************
547 // Set the head wind velocity (in knots). Here, 'head wind' means parallel
548 // to the aircraft heading and to the ground. The aircraft velocity
549 // with respect to the ground is not changed but the true airspeed is.
550
SetHeadWindKtsIC(double head)551 void FGInitialCondition::SetHeadWindKtsIC(double head)
552 {
553 const FGMatrix33& Tb2l = orientation.GetTInv();
554 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
555 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
556 // This is a head wind, so the direction vector for the wind
557 // needs to be set opposite to the heading the aircraft
558 // is taking. So, the cos and sin of the heading (psi)
559 // are negated in the line below.
560 FGColumnVector3 _vHEAD(-orientation.GetCosEuler(ePsi), -orientation.GetSinEuler(ePsi), 0.);
561
562 // Gram-Schmidt process is used to remove the existing head wind component
563 _vWIND_NED -= DotProduct(_vWIND_NED, _vHEAD) * _vHEAD;
564 // Which is now replaced by the new value. The input head wind is expected
565 // in knots, so first convert to fps, which is the internal unit used.
566 _vWIND_NED += (head * ktstofps) * _vHEAD;
567 _vt_NED = vUVW_NED + _vWIND_NED;
568
569 vt = _vt_NED.Magnitude();
570
571 calcAeroAngles(_vt_NED);
572 }
573
574 //******************************************************************************
575 // Set the vertical wind velocity (in knots). The 'vertical' is taken in the
576 // local NED frame. The aircraft velocity with respect to the ground is not
577 // changed but the true airspeed is.
578
SetWindDownKtsIC(double wD)579 void FGInitialCondition::SetWindDownKtsIC(double wD)
580 {
581 const FGMatrix33& Tb2l = orientation.GetTInv();
582 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
583
584 _vt_NED(eW) = vUVW_NED(eW) + wD;
585 vt = _vt_NED.Magnitude();
586
587 calcAeroAngles(_vt_NED);
588 }
589
590 //******************************************************************************
591 // Modifies the wind velocity (in knots) while keeping its direction unchanged.
592 // The vertical component (in local NED frame) is unmodified. The aircraft
593 // velocity with respect to the ground is not changed but the true airspeed is.
594
SetWindMagKtsIC(double mag)595 void FGInitialCondition::SetWindMagKtsIC(double mag)
596 {
597 const FGMatrix33& Tb2l = orientation.GetTInv();
598 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
599 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
600 FGColumnVector3 _vHEAD(_vWIND_NED(eU), _vWIND_NED(eV), 0.);
601 double windMag = _vHEAD.Magnitude();
602
603 if (windMag > 0.001)
604 _vHEAD *= (mag*ktstofps) / windMag;
605 else
606 _vHEAD = FGColumnVector3((mag*ktstofps), 0., 0.);
607
608 _vWIND_NED(eU) = _vHEAD(eU);
609 _vWIND_NED(eV) = _vHEAD(eV);
610 _vt_NED = vUVW_NED + _vWIND_NED;
611 vt = _vt_NED.Magnitude();
612
613 calcAeroAngles(_vt_NED);
614 }
615
616 //******************************************************************************
617 // Modifies the wind direction while keeping its velocity unchanged. The vertical
618 // component (in local NED frame) is unmodified. The aircraft velocity with
619 // respect to the ground is not changed but the true airspeed is.
620
SetWindDirDegIC(double dir)621 void FGInitialCondition::SetWindDirDegIC(double dir)
622 {
623 const FGMatrix33& Tb2l = orientation.GetTInv();
624 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
625 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
626 double mag = _vWIND_NED.Magnitude(eU, eV);
627 FGColumnVector3 _vHEAD(mag*cos(dir*degtorad), mag*sin(dir*degtorad), 0.);
628
629 _vWIND_NED(eU) = _vHEAD(eU);
630 _vWIND_NED(eV) = _vHEAD(eV);
631 _vt_NED = vUVW_NED + _vWIND_NED;
632 vt = _vt_NED.Magnitude();
633
634 calcAeroAngles(_vt_NED);
635 }
636
637 //******************************************************************************
638
SetTerrainElevationFtIC(double elev)639 void FGInitialCondition::SetTerrainElevationFtIC(double elev)
640 {
641 double agl = GetAltitudeAGLFtIC();
642
643 fdmex->GetGroundCallback()->SetTerrainGeoCentRadius(elev + position.GetSeaLevelRadius());
644
645 if (lastAltitudeSet == setagl)
646 SetAltitudeAGLFtIC(agl);
647 }
648
649 //******************************************************************************
650
GetAltitudeAGLFtIC(void) const651 double FGInitialCondition::GetAltitudeAGLFtIC(void) const
652 {
653 return position.GetAltitudeAGL();
654 }
655
656 //******************************************************************************
657
GetTerrainElevationFtIC(void) const658 double FGInitialCondition::GetTerrainElevationFtIC(void) const
659 {
660 return position.GetTerrainRadius() - position.GetSeaLevelRadius();
661 }
662
663 //******************************************************************************
664
SetAltitudeAGLFtIC(double agl)665 void FGInitialCondition::SetAltitudeAGLFtIC(double agl)
666 {
667 double terrainElevation = position.GetTerrainRadius()
668 - position.GetSeaLevelRadius();
669 SetAltitudeASLFtIC(agl + terrainElevation);
670 lastAltitudeSet = setagl;
671 }
672
673 //******************************************************************************
674 // Set the altitude SL. If the airspeed has been previously set with parameters
675 // that are atmosphere dependent (Mach, VCAS, VEAS) then the true airspeed is
676 // modified to keep the last set speed to its previous value.
677
SetAltitudeASLFtIC(double alt)678 void FGInitialCondition::SetAltitudeASLFtIC(double alt)
679 {
680 double altitudeASL = position.GetAltitudeASL();
681 double pressure = Atmosphere->GetPressure(altitudeASL);
682 double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
683 double rho = Atmosphere->GetDensity(altitudeASL);
684 double rhoSL = Atmosphere->GetDensitySL();
685
686 double mach0 = vt / soundSpeed;
687 double vc0 = VcalibratedFromMach(mach0, pressure);
688 double ve0 = vt * sqrt(rho/rhoSL);
689
690 double geodLatitude = position.GetGeodLatitudeRad();
691 altitudeASL=alt;
692 position.SetAltitudeASL(alt);
693
694 // The call to SetAltitudeASL has most likely modified the geodetic latitude
695 // so we need to restore it to its initial value.
696 if (lastLatitudeSet == setgeod)
697 SetGeodLatitudeRadIC(geodLatitude);
698
699 soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
700 rho = Atmosphere->GetDensity(altitudeASL);
701 pressure = Atmosphere->GetPressure(altitudeASL);
702
703 switch(lastSpeedSet) {
704 case setvc:
705 mach0 = MachFromVcalibrated(vc0, pressure);
706 SetVtrueFpsIC(mach0 * soundSpeed);
707 break;
708 case setmach:
709 SetVtrueFpsIC(mach0 * soundSpeed);
710 break;
711 case setve:
712 SetVtrueFpsIC(ve0 * sqrt(rhoSL/rho));
713 break;
714 default: // Make the compiler stop complaining about missing enums
715 break;
716 }
717
718 lastAltitudeSet = setasl;
719 }
720
721 //******************************************************************************
722
SetGeodLatitudeRadIC(double geodLatitude)723 void FGInitialCondition::SetGeodLatitudeRadIC(double geodLatitude)
724 {
725 double h = ComputeGeodAltitude(geodLatitude);
726 double lon = position.GetLongitude();
727
728 position.SetPositionGeodetic(lon, geodLatitude, h);
729 lastLatitudeSet = setgeod;
730 }
731
732 //******************************************************************************
733
SetLatitudeRadIC(double lat)734 void FGInitialCondition::SetLatitudeRadIC(double lat)
735 {
736 double altitude;
737
738 lastLatitudeSet = setgeoc;
739
740 switch(lastAltitudeSet) {
741 case setagl:
742 altitude = GetAltitudeAGLFtIC();
743 position.SetLatitude(lat);
744 SetAltitudeAGLFtIC(altitude);
745 break;
746 default:
747 position.SetLatitude(lat);
748 break;
749 }
750 }
751
752 //******************************************************************************
753
SetLongitudeRadIC(double lon)754 void FGInitialCondition::SetLongitudeRadIC(double lon)
755 {
756 double altitude;
757
758 switch(lastAltitudeSet) {
759 case setagl:
760 altitude = GetAltitudeAGLFtIC();
761 position.SetLongitude(lon);
762 SetAltitudeAGLFtIC(altitude);
763 break;
764 default:
765 altitude = position.GetAltitudeASL();
766 position.SetLongitude(lon);
767 position.SetAltitudeASL(altitude);
768 break;
769 }
770 }
771
772 //******************************************************************************
773
GetWindDirDegIC(void) const774 double FGInitialCondition::GetWindDirDegIC(void) const
775 {
776 const FGMatrix33& Tb2l = orientation.GetTInv();
777 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
778 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
779
780 return _vWIND_NED(eV) == 0.0 ? 0.0
781 : atan2(_vWIND_NED(eV), _vWIND_NED(eU))*radtodeg;
782 }
783
784 //******************************************************************************
785
GetNEDWindFpsIC(int idx) const786 double FGInitialCondition::GetNEDWindFpsIC(int idx) const
787 {
788 const FGMatrix33& Tb2l = orientation.GetTInv();
789 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
790 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
791
792 return _vWIND_NED(idx);
793 }
794
795 //******************************************************************************
796
GetWindFpsIC(void) const797 double FGInitialCondition::GetWindFpsIC(void) const
798 {
799 const FGMatrix33& Tb2l = orientation.GetTInv();
800 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
801 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
802
803 return _vWIND_NED.Magnitude(eU, eV);
804 }
805
806 //******************************************************************************
807
GetBodyWindFpsIC(int idx) const808 double FGInitialCondition::GetBodyWindFpsIC(int idx) const
809 {
810 const FGMatrix33& Tl2b = orientation.GetT();
811 FGColumnVector3 _vt_BODY = Tw2b * FGColumnVector3(vt, 0., 0.);
812 FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
813 FGColumnVector3 _vWIND_BODY = _vt_BODY - _vUVW_BODY;
814
815 return _vWIND_BODY(idx);
816 }
817
818 //******************************************************************************
819
GetVcalibratedKtsIC(void) const820 double FGInitialCondition::GetVcalibratedKtsIC(void) const
821 {
822 double altitudeASL = position.GetAltitudeASL();
823 double pressure = Atmosphere->GetPressure(altitudeASL);
824 double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
825 double mach = vt / soundSpeed;
826
827 return fpstokts * VcalibratedFromMach(mach, pressure);
828 }
829
830 //******************************************************************************
831
GetVequivalentKtsIC(void) const832 double FGInitialCondition::GetVequivalentKtsIC(void) const
833 {
834 double altitudeASL = position.GetAltitudeASL();
835 double rho = Atmosphere->GetDensity(altitudeASL);
836 double rhoSL = Atmosphere->GetDensitySL();
837 return fpstokts * vt * sqrt(rho/rhoSL);
838 }
839
840 //******************************************************************************
841
GetMachIC(void) const842 double FGInitialCondition::GetMachIC(void) const
843 {
844 double altitudeASL = position.GetAltitudeASL();
845 double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
846 return vt / soundSpeed;
847 }
848
849 //******************************************************************************
850
GetBodyVelFpsIC(int idx) const851 double FGInitialCondition::GetBodyVelFpsIC(int idx) const
852 {
853 const FGMatrix33& Tl2b = orientation.GetT();
854 FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
855
856 return _vUVW_BODY(idx);
857 }
858
859 //******************************************************************************
860
Load(const SGPath & rstfile,bool useStoredPath)861 bool FGInitialCondition::Load(const SGPath& rstfile, bool useStoredPath)
862 {
863 SGPath init_file_name;
864 if(useStoredPath && rstfile.isRelative()) {
865 init_file_name = fdmex->GetFullAircraftPath()/rstfile.utf8Str();
866 } else {
867 init_file_name = rstfile;
868 }
869
870 FGXMLFileRead XMLFileRead;
871 Element* document = XMLFileRead.LoadXMLDocument(init_file_name);
872
873 // Make sure that the document is valid
874 if (!document) {
875 cerr << "File: " << init_file_name << " could not be read." << endl;
876 exit(-1);
877 }
878
879 if (document->GetName() != string("initialize")) {
880 cerr << "File: " << init_file_name << " is not a reset file." << endl;
881 exit(-1);
882 }
883
884 double version = HUGE_VAL;
885 bool result = false;
886
887 if (document->HasAttribute("version"))
888 version = document->GetAttributeValueAsNumber("version");
889
890 if (version == HUGE_VAL) {
891 result = Load_v1(document); // Default to the old version
892 } else if (version >= 3.0) {
893 cerr << "Only initialization file formats 1 and 2 are currently supported" << endl;
894 exit (-1);
895 } else if (version >= 2.0) {
896 result = Load_v2(document);
897 } else if (version >= 1.0) {
898 result = Load_v1(document);
899 }
900
901 // Check to see if any engines are specified to be initialized in a running state
902 Element* running_elements = document->FindElement("running");
903 while (running_elements) {
904 int engineNumber = int(running_elements->GetDataAsNumber());
905 enginesRunning |= engineNumber == -1 ? engineNumber : 1 << engineNumber;
906 running_elements = document->FindNextElement("running");
907 }
908
909 return result;
910 }
911
912 //******************************************************************************
913 // Given an altitude above the mean sea level (or a position radius which is the
914 // same) and a geodetic latitude, compute the geodetic altitude.
915 //
916 // TODO: extend the routine to the case where lastAltitudeSet is equal to
917 // setagl.
918
ComputeGeodAltitude(double geodLatitude)919 double FGInitialCondition::ComputeGeodAltitude(double geodLatitude)
920 {
921 double R = position.GetRadius();
922 double slat = sin(geodLatitude);
923 double RN = a / sqrt(1.0 - e2*slat*slat);
924 double p1 = e2*RN*slat*slat;
925 double p2 = e2*e2*RN*RN*slat*slat-R*R;
926 return p1 + sqrt(p1*p1-p2) - RN;
927 }
928
929 //******************************************************************************
930
LoadLatitude(Element * position_el)931 bool FGInitialCondition::LoadLatitude(Element* position_el)
932 {
933 Element* latitude_el = position_el->FindElement("latitude");
934
935 if (latitude_el) {
936 double latitude = position_el->FindElementValueAsNumberConvertTo("latitude", "RAD");
937
938 if (fabs(latitude) > 0.5*M_PI) {
939 string unit_type = latitude_el->GetAttributeValue("unit");
940 if (unit_type.empty()) unit_type="RAD";
941
942 cerr << latitude_el->ReadFrom() << "The latitude value "
943 << latitude_el->GetDataAsNumber() << " " << unit_type
944 << " is outside the range [";
945 if (unit_type == "DEG")
946 cerr << "-90 DEG ; +90 DEG]" << endl;
947 else
948 cerr << "-PI/2 RAD; +PI/2 RAD]" << endl;
949
950 return false;
951 }
952
953 string lat_type = latitude_el->GetAttributeValue("type");
954
955 if (lat_type == "geod" || lat_type == "geodetic")
956 SetGeodLatitudeRadIC(latitude);
957 else {
958 position.SetLatitude(latitude);
959 lastLatitudeSet = setgeoc;
960 }
961 }
962
963 return true;
964 }
965
966 //******************************************************************************
967
SetTrimRequest(std::string trim)968 void FGInitialCondition::SetTrimRequest(std::string trim)
969 {
970 std::string& trimOption = to_lower(trim);
971 if (trimOption == "1")
972 trimRequested = TrimMode::tGround; // For backwards compatabiity
973 else if (trimOption == "longitudinal")
974 trimRequested = TrimMode::tLongitudinal;
975 else if (trimOption == "full")
976 trimRequested = TrimMode::tFull;
977 else if (trimOption == "ground")
978 trimRequested = TrimMode::tGround;
979 else if (trimOption == "pullup")
980 trimRequested = TrimMode::tPullup;
981 else if (trimOption == "custom")
982 trimRequested = TrimMode::tCustom;
983 else if (trimOption == "turn")
984 trimRequested = TrimMode::tTurn;
985 }
986
987 //******************************************************************************
988
Load_v1(Element * document)989 bool FGInitialCondition::Load_v1(Element* document)
990 {
991 bool result = true;
992
993 if (document->FindElement("longitude"))
994 SetLongitudeRadIC(document->FindElementValueAsNumberConvertTo("longitude", "RAD"));
995 if (document->FindElement("elevation"))
996 SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT"));
997
998 if (document->FindElement("altitude")) // This is feet above ground level
999 SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT"));
1000 else if (document->FindElement("altitudeAGL")) // This is feet above ground level
1001 SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
1002 else if (document->FindElement("altitudeMSL")) // This is feet above sea level
1003 SetAltitudeASLFtIC(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
1004
1005 result = LoadLatitude(document);
1006
1007 FGColumnVector3 vOrient = orientation.GetEuler();
1008
1009 if (document->FindElement("phi"))
1010 vOrient(ePhi) = document->FindElementValueAsNumberConvertTo("phi", "RAD");
1011 if (document->FindElement("theta"))
1012 vOrient(eTht) = document->FindElementValueAsNumberConvertTo("theta", "RAD");
1013 if (document->FindElement("psi"))
1014 vOrient(ePsi) = document->FindElementValueAsNumberConvertTo("psi", "RAD");
1015
1016 orientation = FGQuaternion(vOrient);
1017
1018 if (document->FindElement("ubody"))
1019 SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo("ubody", "FT/SEC"));
1020 if (document->FindElement("vbody"))
1021 SetVBodyFpsIC(document->FindElementValueAsNumberConvertTo("vbody", "FT/SEC"));
1022 if (document->FindElement("wbody"))
1023 SetWBodyFpsIC(document->FindElementValueAsNumberConvertTo("wbody", "FT/SEC"));
1024 if (document->FindElement("vnorth"))
1025 SetVNorthFpsIC(document->FindElementValueAsNumberConvertTo("vnorth", "FT/SEC"));
1026 if (document->FindElement("veast"))
1027 SetVEastFpsIC(document->FindElementValueAsNumberConvertTo("veast", "FT/SEC"));
1028 if (document->FindElement("vdown"))
1029 SetVDownFpsIC(document->FindElementValueAsNumberConvertTo("vdown", "FT/SEC"));
1030 if (document->FindElement("vc"))
1031 SetVcalibratedKtsIC(document->FindElementValueAsNumberConvertTo("vc", "KTS"));
1032 if (document->FindElement("vt"))
1033 SetVtrueKtsIC(document->FindElementValueAsNumberConvertTo("vt", "KTS"));
1034 if (document->FindElement("mach"))
1035 SetMachIC(document->FindElementValueAsNumber("mach"));
1036 if (document->FindElement("gamma"))
1037 SetFlightPathAngleDegIC(document->FindElementValueAsNumberConvertTo("gamma", "DEG"));
1038 if (document->FindElement("roc"))
1039 SetClimbRateFpsIC(document->FindElementValueAsNumberConvertTo("roc", "FT/SEC"));
1040 if (document->FindElement("vground"))
1041 SetVgroundKtsIC(document->FindElementValueAsNumberConvertTo("vground", "KTS"));
1042 if (document->FindElement("alpha"))
1043 SetAlphaDegIC(document->FindElementValueAsNumberConvertTo("alpha", "DEG"));
1044 if (document->FindElement("beta"))
1045 SetBetaDegIC(document->FindElementValueAsNumberConvertTo("beta", "DEG"));
1046 if (document->FindElement("vwind"))
1047 SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo("vwind", "KTS"));
1048 if (document->FindElement("winddir"))
1049 SetWindDirDegIC(document->FindElementValueAsNumberConvertTo("winddir", "DEG"));
1050 if (document->FindElement("hwind"))
1051 SetHeadWindKtsIC(document->FindElementValueAsNumberConvertTo("hwind", "KTS"));
1052 if (document->FindElement("xwind"))
1053 SetCrossWindKtsIC(document->FindElementValueAsNumberConvertTo("xwind", "KTS"));
1054 if (document->FindElement("targetNlf"))
1055 SetTargetNlfIC(document->FindElementValueAsNumber("targetNlf"));
1056 if (document->FindElement("trim"))
1057 SetTrimRequest(document->FindElementValue("trim"));
1058
1059 // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
1060 // This is the rotation rate of the "Local" frame, expressed in the local frame.
1061 const FGMatrix33& Tl2b = orientation.GetT();
1062 double radInv = 1.0 / position.GetRadius();
1063 FGColumnVector3 vOmegaLocal = FGColumnVector3(
1064 radInv*vUVW_NED(eEast),
1065 -radInv*vUVW_NED(eNorth),
1066 -radInv*vUVW_NED(eEast)*position.GetTanLatitude() );
1067
1068 vPQR_body = Tl2b * vOmegaLocal;
1069
1070 return result;
1071 }
1072
1073 //******************************************************************************
1074
Load_v2(Element * document)1075 bool FGInitialCondition::Load_v2(Element* document)
1076 {
1077 FGColumnVector3 vOrient;
1078 bool result = true;
1079
1080 // support both earth_position_angle and planet_position_angle, for now.
1081 if (document->FindElement("earth_position_angle"))
1082 epa = document->FindElementValueAsNumberConvertTo("earth_position_angle", "RAD");
1083 if (document->FindElement("planet_position_angle"))
1084 epa = document->FindElementValueAsNumberConvertTo("planet_position_angle", "RAD");
1085
1086 // Calculate the inertial to ECEF matrices
1087 FGMatrix33 Ti2ec(cos(epa), sin(epa), 0.0,
1088 -sin(epa), cos(epa), 0.0,
1089 0.0, 0.0, 1.0);
1090 FGMatrix33 Tec2i = Ti2ec.Transposed();
1091
1092 if (document->FindElement("planet_rotation_rate")) {
1093 fdmex->GetInertial()->SetOmegaPlanet(document->FindElementValueAsNumberConvertTo("planet_rotation_rate", "RAD"));
1094 fdmex->GetPropagate()->in.vOmegaPlanet = fdmex->GetInertial()->GetOmegaPlanet();
1095 fdmex->GetAccelerations()->in.vOmegaPlanet = fdmex->GetInertial()->GetOmegaPlanet();
1096 }
1097 FGColumnVector3 vOmegaEarth = fdmex->GetInertial()->GetOmegaPlanet();
1098
1099 if (document->FindElement("elevation"))
1100 fdmex->GetGroundCallback()->SetTerrainGeoCentRadius(document->FindElementValueAsNumberConvertTo("elevation", "FT")+position.GetSeaLevelRadius());
1101
1102 // Initialize vehicle position
1103 //
1104 // Allowable frames:
1105 // - ECI (Earth Centered Inertial)
1106 // - ECEF (Earth Centered, Earth Fixed)
1107
1108 Element* position_el = document->FindElement("position");
1109 if (position_el) {
1110 string frame = position_el->GetAttributeValue("frame");
1111 frame = to_lower(frame);
1112 if (frame == "eci") { // Need to transform vLoc to ECEF for storage and use in FGLocation.
1113 position = Ti2ec * position_el->FindElementTripletConvertTo("FT");
1114 } else if (frame == "ecef") {
1115 if (!position_el->FindElement("x") && !position_el->FindElement("y") && !position_el->FindElement("z")) {
1116 if (position_el->FindElement("longitude"))
1117 position.SetLongitude(position_el->FindElementValueAsNumberConvertTo("longitude", "RAD"));
1118
1119 if (position_el->FindElement("radius")) {
1120 position.SetRadius(position_el->FindElementValueAsNumberConvertTo("radius", "FT"));
1121 } else if (position_el->FindElement("altitudeAGL")) {
1122 position.SetAltitudeAGL(position_el->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
1123 } else if (position_el->FindElement("altitudeMSL")) {
1124 position.SetAltitudeASL(position_el->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
1125 } else {
1126 cerr << endl << " No altitude or radius initial condition is given." << endl;
1127 result = false;
1128 }
1129
1130 result = LoadLatitude(position_el);
1131
1132 } else {
1133 position = position_el->FindElementTripletConvertTo("FT");
1134 }
1135 } else {
1136 cerr << endl << " Neither ECI nor ECEF frame is specified for initial position." << endl;
1137 result = false;
1138 }
1139 } else {
1140 cerr << endl << " Initial position not specified in this initialization file." << endl;
1141 result = false;
1142 }
1143
1144 // End of position initialization
1145
1146 // Initialize vehicle orientation
1147 // Allowable frames
1148 // - ECI (Earth Centered Inertial)
1149 // - ECEF (Earth Centered, Earth Fixed)
1150 // - Local
1151 //
1152 // Need to convert the provided orientation to a local orientation, using
1153 // the given orientation and knowledge of the Earth position angle.
1154 // This could be done using matrices (where in the subscript "b/a",
1155 // it is meant "b with respect to a", and where b=body frame,
1156 // i=inertial frame, l=local NED frame and e=ecef frame) as:
1157 //
1158 // C_b/l = C_b/e * C_e/l
1159 //
1160 // Using quaternions (note reverse ordering compared to matrix representation):
1161 //
1162 // Q_b/l = Q_e/l * Q_b/e
1163 //
1164 // Use the specific matrices as needed. The above example of course is for the whole
1165 // body to local orientation.
1166 // The new orientation angles can be extracted from the matrix or the quaternion.
1167 // ToDo: Do we need to deal with normalization of the quaternions here?
1168
1169 Element* orientation_el = document->FindElement("orientation");
1170 if (orientation_el) {
1171 string frame = orientation_el->GetAttributeValue("frame");
1172 frame = to_lower(frame);
1173 vOrient = orientation_el->FindElementTripletConvertTo("RAD");
1174 if (frame == "eci") {
1175
1176 // In this case, we are supplying the Euler angles for the vehicle with
1177 // respect to the inertial system, represented by the C_b/i Matrix.
1178 // We want the body orientation with respect to the local (NED frame):
1179 //
1180 // C_b/l = C_b/i * C_i/l
1181 //
1182 // Or, using quaternions (note reverse ordering compared to matrix representation):
1183 //
1184 // Q_b/l = Q_i/l * Q_b/i
1185
1186 FGQuaternion QuatI2Body = FGQuaternion(vOrient);
1187 QuatI2Body.Normalize();
1188 FGQuaternion QuatLocal2I = Tec2i * position.GetTl2ec();
1189 QuatLocal2I.Normalize();
1190 orientation = QuatLocal2I * QuatI2Body;
1191
1192 } else if (frame == "ecef") {
1193
1194 // In this case we are given the Euler angles representing the orientation of
1195 // the body with respect to the ECEF system, represented by the C_b/e Matrix.
1196 // We want the body orientation with respect to the local (NED frame):
1197 //
1198 // C_b/l = C_b/e * C_e/l
1199 //
1200 // Using quaternions (note reverse ordering compared to matrix representation):
1201 //
1202 // Q_b/l = Q_e/l * Q_b/e
1203
1204 FGQuaternion QuatEC2Body(vOrient); // Store relationship of Body frame wrt ECEF frame, Q_b/e
1205 QuatEC2Body.Normalize();
1206 FGQuaternion QuatLocal2EC = position.GetTl2ec(); // Get Q_e/l from matrix
1207 QuatLocal2EC.Normalize();
1208 orientation = QuatLocal2EC * QuatEC2Body; // Q_b/l = Q_e/l * Q_b/e
1209
1210 } else if (frame == "local") {
1211
1212 orientation = FGQuaternion(vOrient);
1213
1214 } else {
1215
1216 cerr << endl << fgred << " Orientation frame type: \"" << frame
1217 << "\" is not supported!" << reset << endl << endl;
1218 result = false;
1219
1220 }
1221 }
1222
1223 // Initialize vehicle velocity
1224 // Allowable frames
1225 // - ECI (Earth Centered Inertial)
1226 // - ECEF (Earth Centered, Earth Fixed)
1227 // - Local
1228 // - Body
1229 // The vehicle will be defaulted to (0,0,0) in the Body frame if nothing is provided.
1230
1231 Element* velocity_el = document->FindElement("velocity");
1232 FGColumnVector3 vInitVelocity = FGColumnVector3(0.0, 0.0, 0.0);
1233 FGMatrix33 mTec2l = position.GetTec2l();
1234 const FGMatrix33& Tb2l = orientation.GetTInv();
1235
1236 if (velocity_el) {
1237
1238 string frame = velocity_el->GetAttributeValue("frame");
1239 frame = to_lower(frame);
1240 FGColumnVector3 vInitVelocity = velocity_el->FindElementTripletConvertTo("FT/SEC");
1241
1242 if (frame == "eci") {
1243 FGColumnVector3 omega_cross_r = vOmegaEarth * (Tec2i * position);
1244 vUVW_NED = mTec2l * (vInitVelocity - omega_cross_r);
1245 lastSpeedSet = setned;
1246 } else if (frame == "ecef") {
1247 vUVW_NED = mTec2l * vInitVelocity;
1248 lastSpeedSet = setned;
1249 } else if (frame == "local") {
1250 vUVW_NED = vInitVelocity;
1251 lastSpeedSet = setned;
1252 } else if (frame == "body") {
1253 vUVW_NED = Tb2l * vInitVelocity;
1254 lastSpeedSet = setuvw;
1255 } else {
1256
1257 cerr << endl << fgred << " Velocity frame type: \"" << frame
1258 << "\" is not supported!" << reset << endl << endl;
1259 result = false;
1260
1261 }
1262
1263 } else {
1264
1265 vUVW_NED = Tb2l * vInitVelocity;
1266
1267 }
1268
1269 vt = vUVW_NED.Magnitude();
1270
1271 calcAeroAngles(vUVW_NED);
1272
1273 // Initialize vehicle body rates
1274 // Allowable frames
1275 // - ECI (Earth Centered Inertial)
1276 // - ECEF (Earth Centered, Earth Fixed)
1277 // - Body
1278
1279 Element* attrate_el = document->FindElement("attitude_rate");
1280 const FGMatrix33& Tl2b = orientation.GetT();
1281
1282 // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
1283 // This is the rotation rate of the "Local" frame, expressed in the local frame.
1284 double radInv = 1.0 / position.GetRadius();
1285 FGColumnVector3 vOmegaLocal = FGColumnVector3(
1286 radInv*vUVW_NED(eEast),
1287 -radInv*vUVW_NED(eNorth),
1288 -radInv*vUVW_NED(eEast)*position.GetTanLatitude() );
1289
1290 if (attrate_el) {
1291
1292 string frame = attrate_el->GetAttributeValue("frame");
1293 frame = to_lower(frame);
1294 FGColumnVector3 vAttRate = attrate_el->FindElementTripletConvertTo("RAD/SEC");
1295
1296 if (frame == "eci") {
1297 FGMatrix33 Ti2l = position.GetTec2l() * Ti2ec;
1298 vPQR_body = Tl2b * Ti2l * (vAttRate - vOmegaEarth);
1299 } else if (frame == "ecef") {
1300 vPQR_body = Tl2b * position.GetTec2l() * vAttRate;
1301 } else if (frame == "local") {
1302 vPQR_body = Tl2b * (vAttRate + vOmegaLocal);
1303 } else if (frame == "body") {
1304 vPQR_body = vAttRate;
1305 } else if (!frame.empty()) { // misspelling of frame
1306
1307 cerr << endl << fgred << " Attitude rate frame type: \"" << frame
1308 << "\" is not supported!" << reset << endl << endl;
1309 result = false;
1310
1311 } else if (frame.empty()) {
1312 vPQR_body = Tl2b * vOmegaLocal;
1313 }
1314
1315 } else { // Body frame attitude rate assumed 0 relative to local.
1316 vPQR_body = Tl2b * vOmegaLocal;
1317 }
1318
1319 return result;
1320 }
1321
1322 //******************************************************************************
1323
bind(FGPropertyManager * PropertyManager)1324 void FGInitialCondition::bind(FGPropertyManager* PropertyManager)
1325 {
1326 PropertyManager->Tie("ic/vc-kts", this,
1327 &FGInitialCondition::GetVcalibratedKtsIC,
1328 &FGInitialCondition::SetVcalibratedKtsIC,
1329 true);
1330 PropertyManager->Tie("ic/ve-kts", this,
1331 &FGInitialCondition::GetVequivalentKtsIC,
1332 &FGInitialCondition::SetVequivalentKtsIC,
1333 true);
1334 PropertyManager->Tie("ic/vg-kts", this,
1335 &FGInitialCondition::GetVgroundKtsIC,
1336 &FGInitialCondition::SetVgroundKtsIC,
1337 true);
1338 PropertyManager->Tie("ic/vt-kts", this,
1339 &FGInitialCondition::GetVtrueKtsIC,
1340 &FGInitialCondition::SetVtrueKtsIC,
1341 true);
1342 PropertyManager->Tie("ic/mach", this,
1343 &FGInitialCondition::GetMachIC,
1344 &FGInitialCondition::SetMachIC,
1345 true);
1346 PropertyManager->Tie("ic/roc-fpm", this,
1347 &FGInitialCondition::GetClimbRateFpmIC,
1348 &FGInitialCondition::SetClimbRateFpmIC,
1349 true);
1350 PropertyManager->Tie("ic/gamma-deg", this,
1351 &FGInitialCondition::GetFlightPathAngleDegIC,
1352 &FGInitialCondition::SetFlightPathAngleDegIC,
1353 true);
1354 PropertyManager->Tie("ic/alpha-deg", this,
1355 &FGInitialCondition::GetAlphaDegIC,
1356 &FGInitialCondition::SetAlphaDegIC,
1357 true);
1358 PropertyManager->Tie("ic/beta-deg", this,
1359 &FGInitialCondition::GetBetaDegIC,
1360 &FGInitialCondition::SetBetaDegIC,
1361 true);
1362 PropertyManager->Tie("ic/theta-deg", this,
1363 &FGInitialCondition::GetThetaDegIC,
1364 &FGInitialCondition::SetThetaDegIC,
1365 true);
1366 PropertyManager->Tie("ic/phi-deg", this,
1367 &FGInitialCondition::GetPhiDegIC,
1368 &FGInitialCondition::SetPhiDegIC,
1369 true);
1370 PropertyManager->Tie("ic/psi-true-deg", this,
1371 &FGInitialCondition::GetPsiDegIC,
1372 &FGInitialCondition::SetPsiDegIC,
1373 true);
1374 PropertyManager->Tie("ic/lat-gc-deg", this,
1375 &FGInitialCondition::GetLatitudeDegIC,
1376 &FGInitialCondition::SetLatitudeDegIC,
1377 true);
1378 PropertyManager->Tie("ic/long-gc-deg", this,
1379 &FGInitialCondition::GetLongitudeDegIC,
1380 &FGInitialCondition::SetLongitudeDegIC,
1381 true);
1382 PropertyManager->Tie("ic/h-sl-ft", this,
1383 &FGInitialCondition::GetAltitudeASLFtIC,
1384 &FGInitialCondition::SetAltitudeASLFtIC,
1385 true);
1386 PropertyManager->Tie("ic/h-agl-ft", this,
1387 &FGInitialCondition::GetAltitudeAGLFtIC,
1388 &FGInitialCondition::SetAltitudeAGLFtIC,
1389 true);
1390 PropertyManager->Tie("ic/terrain-elevation-ft", this,
1391 &FGInitialCondition::GetTerrainElevationFtIC,
1392 &FGInitialCondition::SetTerrainElevationFtIC,
1393 true);
1394 PropertyManager->Tie("ic/vg-fps", this,
1395 &FGInitialCondition::GetVgroundFpsIC,
1396 &FGInitialCondition::SetVgroundFpsIC,
1397 true);
1398 PropertyManager->Tie("ic/vt-fps", this,
1399 &FGInitialCondition::GetVtrueFpsIC,
1400 &FGInitialCondition::SetVtrueFpsIC,
1401 true);
1402 PropertyManager->Tie("ic/vw-bx-fps", this,
1403 &FGInitialCondition::GetWindUFpsIC);
1404 PropertyManager->Tie("ic/vw-by-fps", this,
1405 &FGInitialCondition::GetWindVFpsIC);
1406 PropertyManager->Tie("ic/vw-bz-fps", this,
1407 &FGInitialCondition::GetWindWFpsIC);
1408 PropertyManager->Tie("ic/vw-north-fps", this,
1409 &FGInitialCondition::GetWindNFpsIC);
1410 PropertyManager->Tie("ic/vw-east-fps", this,
1411 &FGInitialCondition::GetWindEFpsIC);
1412 PropertyManager->Tie("ic/vw-down-fps", this,
1413 &FGInitialCondition::GetWindDFpsIC);
1414 PropertyManager->Tie("ic/vw-mag-fps", this,
1415 &FGInitialCondition::GetWindFpsIC);
1416 PropertyManager->Tie("ic/vw-dir-deg", this,
1417 &FGInitialCondition::GetWindDirDegIC,
1418 &FGInitialCondition::SetWindDirDegIC,
1419 true);
1420
1421 PropertyManager->Tie("ic/roc-fps", this,
1422 &FGInitialCondition::GetClimbRateFpsIC,
1423 &FGInitialCondition::SetClimbRateFpsIC,
1424 true);
1425 PropertyManager->Tie("ic/u-fps", this,
1426 &FGInitialCondition::GetUBodyFpsIC,
1427 &FGInitialCondition::SetUBodyFpsIC,
1428 true);
1429 PropertyManager->Tie("ic/v-fps", this,
1430 &FGInitialCondition::GetVBodyFpsIC,
1431 &FGInitialCondition::SetVBodyFpsIC,
1432 true);
1433 PropertyManager->Tie("ic/w-fps", this,
1434 &FGInitialCondition::GetWBodyFpsIC,
1435 &FGInitialCondition::SetWBodyFpsIC,
1436 true);
1437 PropertyManager->Tie("ic/vn-fps", this,
1438 &FGInitialCondition::GetVNorthFpsIC,
1439 &FGInitialCondition::SetVNorthFpsIC,
1440 true);
1441 PropertyManager->Tie("ic/ve-fps", this,
1442 &FGInitialCondition::GetVEastFpsIC,
1443 &FGInitialCondition::SetVEastFpsIC,
1444 true);
1445 PropertyManager->Tie("ic/vd-fps", this,
1446 &FGInitialCondition::GetVDownFpsIC,
1447 &FGInitialCondition::SetVDownFpsIC,
1448 true);
1449 PropertyManager->Tie("ic/gamma-rad", this,
1450 &FGInitialCondition::GetFlightPathAngleRadIC,
1451 &FGInitialCondition::SetFlightPathAngleRadIC,
1452 true);
1453 PropertyManager->Tie("ic/alpha-rad", this,
1454 &FGInitialCondition::GetAlphaRadIC,
1455 &FGInitialCondition::SetAlphaRadIC,
1456 true);
1457 PropertyManager->Tie("ic/theta-rad", this,
1458 &FGInitialCondition::GetThetaRadIC,
1459 &FGInitialCondition::SetThetaRadIC,
1460 true);
1461 PropertyManager->Tie("ic/beta-rad", this,
1462 &FGInitialCondition::GetBetaRadIC,
1463 &FGInitialCondition::SetBetaRadIC,
1464 true);
1465 PropertyManager->Tie("ic/phi-rad", this,
1466 &FGInitialCondition::GetPhiRadIC,
1467 &FGInitialCondition::SetPhiRadIC,
1468 true);
1469 PropertyManager->Tie("ic/psi-true-rad", this,
1470 &FGInitialCondition::GetPsiRadIC,
1471 &FGInitialCondition::SetPsiRadIC,
1472 true);
1473 PropertyManager->Tie("ic/lat-gc-rad", this,
1474 &FGInitialCondition::GetLatitudeRadIC,
1475 &FGInitialCondition::SetLatitudeRadIC,
1476 true);
1477 PropertyManager->Tie("ic/long-gc-rad", this,
1478 &FGInitialCondition::GetLongitudeRadIC,
1479 &FGInitialCondition::SetLongitudeRadIC,
1480 true);
1481 PropertyManager->Tie("ic/p-rad_sec", this,
1482 &FGInitialCondition::GetPRadpsIC,
1483 &FGInitialCondition::SetPRadpsIC,
1484 true);
1485 PropertyManager->Tie("ic/q-rad_sec", this,
1486 &FGInitialCondition::GetQRadpsIC,
1487 &FGInitialCondition::SetQRadpsIC,
1488 true);
1489 PropertyManager->Tie("ic/r-rad_sec", this,
1490 &FGInitialCondition::GetRRadpsIC,
1491 &FGInitialCondition::SetRRadpsIC,
1492 true);
1493 PropertyManager->Tie("ic/lat-geod-rad", this,
1494 &FGInitialCondition::GetGeodLatitudeRadIC,
1495 &FGInitialCondition::SetGeodLatitudeRadIC,
1496 true);
1497 PropertyManager->Tie("ic/lat-geod-deg", this,
1498 &FGInitialCondition::GetGeodLatitudeDegIC,
1499 &FGInitialCondition::SetGeodLatitudeDegIC,
1500 true);
1501 PropertyManager->Tie("ic/geod-alt-ft", &position,
1502 &FGLocation::GetGeodAltitude);
1503
1504 PropertyManager->Tie("ic/targetNlf", this,
1505 &FGInitialCondition::GetTargetNlfIC,
1506 &FGInitialCondition::SetTargetNlfIC,
1507 true);
1508 }
1509
1510 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
1511 // The bitmasked value choices are as follows:
1512 // unset: In this case (the default) JSBSim would only print
1513 // out the normally expected messages, essentially echoing
1514 // the config files as they are read. If the environment
1515 // variable is not set, debug_lvl is set to 1 internally
1516 // 0: This requests JSBSim not to output any messages
1517 // whatsoever.
1518 // 1: This value explicity requests the normal JSBSim
1519 // startup messages
1520 // 2: This value asks for a message to be printed out when
1521 // a class is instantiated
1522 // 4: When this value is set, a message is displayed when a
1523 // FGModel object executes its Run() method
1524 // 8: When this value is set, various runtime state variables
1525 // are printed out periodically
1526 // 16: When set various parameters are sanity checked and
1527 // a message is printed out when they go out of bounds
1528
Debug(int from)1529 void FGInitialCondition::Debug(int from)
1530 {
1531 if (debug_lvl <= 0) return;
1532
1533 if (debug_lvl & 1) { // Standard console startup message output
1534 }
1535 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
1536 if (from == 0) cout << "Instantiated: FGInitialCondition" << endl;
1537 if (from == 1) cout << "Destroyed: FGInitialCondition" << endl;
1538 }
1539 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
1540 }
1541 if (debug_lvl & 8 ) { // Runtime state variables
1542 }
1543 if (debug_lvl & 16) { // Sanity checking
1544 }
1545 if (debug_lvl & 64) {
1546 if (from == 0) { // Constructor
1547 }
1548 }
1549 }
1550 }
1551