src/FDM/JSBSim/initialization/FGInitialCondition.cpp

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