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