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.56 2011/01/23 12:13:44 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 = Quat.GetTInv();
 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(rho/rhoSL));
 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) {
 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   // The AoA is not modified here but the function SetAlphaRadIC is updating the
 336   // same angles than SetClimbRateFpsIC needs to update.
 337   // TODO : create a subroutine that only shares the relevant code.
 338   SetAlphaRadIC(alpha);
 339 }
 340
 341 //******************************************************************************
 342 // When the AoA is modified, we need to update the angles theta and beta to
 343 // keep the true airspeed amplitude, the climb rate and the heading unchanged.
 344 // Beta will be modified if the aircraft roll angle is not null.
 345
 346 void FGInitialCondition::SetAlphaRadIC(double alfa)
 347 {
 348   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 349
 350   double calpha = cos(alfa), salpha = sin(alfa);
 351   double cpsi = cos(psi), spsi = sin(psi);
 352   double cphi = cos(phi), sphi = sin(phi);
 353   FGMatrix33 Tpsi( cpsi, spsi, 0.,
 354                     -spsi, cpsi, 0.,
 355                        0.,   0., 1.);
 356   FGMatrix33 Tphi(1.,   0.,   0.,
 357                   0., cphi, sphi,
 358                   0.,-sphi, cphi);
 359   FGMatrix33 Talpha( calpha, 0., salpha,
 360                          0., 1.,    0.,
 361                     -salpha, 0., calpha);
 362
 363   FGColumnVector3 v0 = Tpsi * _vt_NED;
 364   FGColumnVector3 n = (Talpha * Tphi).Transposed() * FGColumnVector3(0., 0., 1.);
 365   FGColumnVector3 y = FGColumnVector3(0., 1., 0.);
 366   FGColumnVector3 u = y - DotProduct(y, n) * n;
 367   FGColumnVector3 p = y * n;
 368
 369   if (DotProduct(p, v0) < 0) p *= -1.0;
 370   p.Normalize();
 371
 372   u *= DotProduct(v0, y) / DotProduct(u, y);
 373
 374   // There are situations where the desired alpha angle cannot be obtained. This
 375   // is not a limitation of the algorithm but is due to the mathematical problem
 376   // not having a solution. This can only be cured by limiting the alpha angle
 377   // or by modifying an additional angle (psi ?). Since this is anticipated to
 378   // be a pathological case (mainly when a high roll angle is required) this
 379   // situation is not addressed below. However if there are complaints about the
 380   // following error being raised too often, we might need to reconsider this
 381   // position.
 382   if (DotProduct(v0, v0) < DotProduct(u, u)) {
 383     cerr << "Cannot modify angle 'alpha' from " << alpha << " to " << alfa << endl;
 384     return;
 385   }
 386
 387   FGColumnVector3 v1 = u + sqrt(DotProduct(v0, v0) - DotProduct(u, u))*p;
 388
 389   FGColumnVector3 v0xz(v0(eU), 0., v0(eW));
 390   FGColumnVector3 v1xz(v1(eU), 0., v1(eW));
 391   v0xz.Normalize();
 392   v1xz.Normalize();
 393   double sinTheta = (v1xz * v0xz)(eY);
 394   theta = asin(sinTheta);
 395
 396   FGQuaternion Quat(phi, theta, psi);
 397   Quat.Normalize();
 398   Tl2b = Quat.GetT();
 399   Tb2l = Quat.GetTInv();
 400
 401   FGColumnVector3 v2 = Talpha * Quat.GetT() * _vt_NED;
 402
 403   alpha = alfa;
 404   beta = atan2(v2(eV), v2(eU));
 405   double cbeta=0.0, sbeta=0.0;
 406   if (vt != 0.0) {
 407     cbeta = v2(eU) / vt;
 408     sbeta = v2(eV) / vt;
 409   }
 410   Tw2b = FGMatrix33(calpha*cbeta, -calpha*sbeta,  -salpha,
 411                            sbeta,         cbeta,      0.0,
 412                     salpha*cbeta, -salpha*sbeta,   calpha);
 413   Tb2w = Tw2b.Transposed();
 414 }
 415
 416 //******************************************************************************
 417 // When the beta angle is modified, we need to update the angles theta and psi
 418 // to keep the true airspeed (amplitude and direction - including the climb rate)
 419 // and the alpha angle unchanged. This may result in the aircraft heading (psi)
 420 // being altered especially if there is cross wind.
 421
 422 void FGInitialCondition::SetBetaRadIC(double bta)
 423 {
 424   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 425
 426   beta = bta;
 427   double calpha = cos(alpha), salpha = sin(alpha);
 428   double cbeta = cos(beta), sbeta = sin(beta);
 429
 430   Tw2b = FGMatrix33(calpha*cbeta, -calpha*sbeta,  -salpha,
 431                            sbeta,         cbeta,      0.0,
 432                     salpha*cbeta, -salpha*sbeta,   calpha);
 433   Tb2w = Tw2b.Transposed();
 434
 435   FGColumnVector3 vf = FGQuaternion(eX, phi).GetTInv() * Tw2b * FGColumnVector3(vt, 0., 0.);
 436   FGColumnVector3 v0xy(_vt_NED(eX), _vt_NED(eY), 0.);
 437   FGColumnVector3 v1xy(sqrt(v0xy(eX)*v0xy(eX)+v0xy(eY)*v0xy(eY)-vf(eY)*vf(eY)),vf(eY),0.);
 438   v0xy.Normalize();
 439   v1xy.Normalize();
 440
 441   if (vf(eX) < 0.) v0xy(eX) *= -1.0;
 442
 443   double sinPsi = (v1xy * v0xy)(eZ);
 444   double cosPsi = DotProduct(v0xy, v1xy);
 445   psi = atan2(sinPsi, cosPsi);
 446   FGMatrix33 Tpsi( cosPsi, sinPsi, 0.,
 447                   -sinPsi, cosPsi, 0.,
 448                       0.,     0., 1.);
 449
 450   FGColumnVector3 v2xz = Tpsi * _vt_NED;
 451   FGColumnVector3 vfxz = vf;
 452   v2xz(eV) = vfxz(eV) = 0.0;
 453   v2xz.Normalize();
 454   vfxz.Normalize();
 455   double sinTheta = (v2xz * vfxz)(eY);
 456   theta = -asin(sinTheta);
 457
 458   FGQuaternion Quat(phi, theta, psi);
 459   Quat.Normalize();
 460   Tl2b = Quat.GetT();
 461   Tb2l = Quat.GetTInv();
 462 }
 463
 464 //******************************************************************************
 465 // Modifies the body frame orientation (roll angle phi). The true airspeed in
 466 // the local NED frame is kept unchanged. Hence the true airspeed in the body
 467 // frame is modified.
 468
 469 void FGInitialCondition::SetPhiRadIC(double fi)
 470 {
 471   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 472
 473   phi = fi;
 474   FGQuaternion Quat = FGQuaternion(phi, theta, psi);
 475   Quat.Normalize();
 476   Tl2b = Quat.GetT();
 477   Tb2l = Quat.GetTInv();
 478
 479   calcAeroAngles(_vt_NED);
 480 }
 481
 482 //******************************************************************************
 483 // Modifies the body frame orientation (pitch angle theta). The true airspeed in
 484 // the local NED frame is kept unchanged. Hence the true airspeed in the body
 485 // frame is modified.
 486
 487 void FGInitialCondition::SetThetaRadIC(double teta)
 488 {
 489   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 490
 491   theta = teta;
 492   FGQuaternion Quat = FGQuaternion(phi, theta, psi);
 493   Quat.Normalize();
 494   Tl2b = Quat.GetT();
 495   Tb2l = Quat.GetTInv();
 496
 497   calcAeroAngles(_vt_NED);
 498 }
 499
 500 //******************************************************************************
 501 // Modifies the body frame orientation (yaw angle psi). The true airspeed in
 502 // the local NED frame is kept unchanged. Hence the true airspeed in the body
 503 // frame is modified.
 504
 505 void FGInitialCondition::SetPsiRadIC(double psy)
 506 {
 507   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 508
 509   psi = psy;
 510   FGQuaternion Quat = FGQuaternion(phi, theta, psi);
 511   Quat.Normalize();
 512   Tl2b = Quat.GetT();
 513   Tb2l = Quat.GetTInv();
 514
 515   calcAeroAngles(_vt_NED);
 516 }
 517
 518 //******************************************************************************
 519 // Modifies an aircraft velocity component (eU, eV or eW) in the body frame. The
 520 // true airspeed is modified accordingly. If there is some wind, the airspeed
 521 // direction modification may differ from the body velocity modification.
 522
 523 void FGInitialCondition::SetBodyVelFpsIC(int idx, double vel)
 524 {
 525   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 526   FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
 527   FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
 528
 529   _vUVW_BODY(idx) = vel;
 530   vUVW_NED = Tb2l * _vUVW_BODY;
 531   _vt_NED = vUVW_NED + _vWIND_NED;
 532   vt = _vt_NED.Magnitude();
 533
 534   calcAeroAngles(_vt_NED);
 535
 536   lastSpeedSet = setuvw;
 537 }
 538
 539 //******************************************************************************
 540 // Modifies an aircraft velocity component (eX, eY or eZ) in the local NED frame.
 541 // The true airspeed is modified accordingly. If there is some wind, the airspeed
 542 // direction modification may differ from the local velocity modification.
 543
 544 void FGInitialCondition::SetNEDVelFpsIC(int idx, double vel)
 545 {
 546   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 547   FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
 548
 549   vUVW_NED(idx) = vel;
 550   _vt_NED = vUVW_NED + _vWIND_NED;
 551   vt = _vt_NED.Magnitude();
 552
 553   calcAeroAngles(_vt_NED);
 554
 555   lastSpeedSet = setned;
 556 }
 557
 558 //******************************************************************************
 559 // Set wind amplitude and direction in the local NED frame. The aircraft velocity
 560 // with respect to the ground is not changed but the true airspeed is.
 561
 562 void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD )
 563 {
 564   FGColumnVector3 _vt_NED = vUVW_NED + FGColumnVector3(wN, wE, wD);
 565   vt = _vt_NED.Magnitude();
 566
 567   calcAeroAngles(_vt_NED);
 568 }
 569
 570 //******************************************************************************
 571 // Set the cross wind velocity (in knots). Here, 'cross wind' means perpendicular
 572 // to the aircraft heading and parallel to the ground. The aircraft velocity
 573 // with respect to the ground is not changed but the true airspeed is.
 574
 575 void FGInitialCondition::SetCrossWindKtsIC(double cross)
 576 {
 577   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 578   FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
 579   FGColumnVector3 _vCROSS(-sin(psi), cos(psi), 0.);
 580
 581   // Gram-Schmidt process is used to remove the existing cross wind component
 582   _vWIND_NED -= DotProduct(_vWIND_NED, _vCROSS) * _vCROSS;
 583   // which is now replaced by the new value.
 584   _vWIND_NED += cross * _vCROSS;
 585   _vt_NED = vUVW_NED + _vWIND_NED;
 586   vt = _vt_NED.Magnitude();
 587
 588   calcAeroAngles(_vt_NED);
 589 }
 590
 591 //******************************************************************************
 592 // Set the head wind velocity (in knots). Here, 'head wind' means parallel
 593 // to the aircraft heading and to the ground. The aircraft velocity
 594 // with respect to the ground is not changed but the true airspeed is.
 595
 596 void FGInitialCondition::SetHeadWindKtsIC(double head)
 597 {
 598   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 599   FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
 600   FGColumnVector3 _vHEAD(cos(psi), sin(psi), 0.);
 601
 602   // Gram-Schmidt process is used to remove the existing cross wind component
 603   _vWIND_NED -= DotProduct(_vWIND_NED, _vHEAD) * _vHEAD;
 604   // which is now replaced by the new value.
 605   _vWIND_NED += head * _vHEAD;
 606   _vt_NED = vUVW_NED + _vWIND_NED;
 607   vt = _vt_NED.Magnitude();
 608
 609   calcAeroAngles(_vt_NED);
 610 }
 611
 612 //******************************************************************************
 613 // Set the vertical wind velocity (in knots). The 'vertical' is taken in the
 614 // local NED frame. The aircraft velocity with respect to the ground is not
 615 // changed but the true airspeed is.
 616
 617 void FGInitialCondition::SetWindDownKtsIC(double wD)
 618 {
 619   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 620
 621   _vt_NED(eW) = vUVW_NED(eW) + wD;
 622   vt = _vt_NED.Magnitude();
 623
 624   calcAeroAngles(_vt_NED);
 625 }
 626
 627 //******************************************************************************
 628 // Modifies the wind velocity (in knots) while keeping its direction unchanged.
 629 // The vertical component (in local NED frame) is unmodified. The aircraft
 630 // velocity with respect to the ground is not changed but the true airspeed is.
 631
 632 void FGInitialCondition::SetWindMagKtsIC(double mag)
 633 {
 634   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 635   FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
 636   FGColumnVector3 _vHEAD(_vWIND_NED(eU), _vWIND_NED(eV), 0.);
 637   double windMag = _vHEAD.Magnitude();
 638
 639   if (windMag > 0.001)
 640     _vHEAD *= mag / windMag;
 641   else
 642     _vHEAD = FGColumnVector3(mag, 0., 0.);
 643
 644   _vWIND_NED(eU) = _vHEAD(eU);
 645   _vWIND_NED(eV) = _vHEAD(eV);
 646   _vt_NED = vUVW_NED + _vWIND_NED;
 647   vt = _vt_NED.Magnitude();
 648
 649   calcAeroAngles(_vt_NED);
 650 }
 651
 652 //******************************************************************************
 653 // Modifies the wind direction while keeping its velocity unchanged. The vertical
 654 // component (in local NED frame) is unmodified. The aircraft velocity with
 655 // respect to the ground is not changed but the true airspeed is.
 656
 657 void FGInitialCondition::SetWindDirDegIC(double dir)
 658 {
 659   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 660   FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
 661   double mag = _vWIND_NED.Magnitude(eU, eV);
 662   FGColumnVector3 _vHEAD(mag*cos(dir*degtorad), mag*sin(dir*degtorad), 0.);
 663
 664   _vWIND_NED(eU) = _vHEAD(eU);
 665   _vWIND_NED(eV) = _vHEAD(eV);
 666   _vt_NED = vUVW_NED + _vWIND_NED;
 667   vt = _vt_NED.Magnitude();
 668
 669   calcAeroAngles(_vt_NED);
 670 }
 671
 672 //******************************************************************************
 673 // Set the altitude SL. If the airspeed has been previously set with parameters
 674 // that are atmosphere dependent (Mach, VCAS, VEAS) then the true airspeed is
 675 // modified to keep the last set speed to its previous value.
 676
 677 void FGInitialCondition::SetAltitudeASLFtIC(double alt)
 678 {
 679   double altitudeASL = position.GetRadius() - sea_level_radius;
 680   double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL);
 681   double soundSpeed = sqrt(SHRatio*Reng*temperature);
 682   double rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL);
 683   double rhoSL = fdmex->GetAtmosphere()->GetDensitySL();
 684
 685   double mach0 = vt / soundSpeed;
 686   double vc0 = calcVcas(mach0);
 687   double ve0 = vt * sqrt(rho/rhoSL);
 688
 689   altitudeASL=alt;
 690   temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL);
 691   soundSpeed = sqrt(SHRatio*Reng*temperature);
 692   rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL);
 693
 694   switch(lastSpeedSet) {
 695     case setvc:
 696       mach0 = getMachFromVcas(vc0);
 697     case setmach:
 698       SetVtrueFpsIC(mach0 * soundSpeed);
 699       break;
 700     case setve:
 701       SetVtrueFpsIC(ve0 * sqrt(rho/rhoSL));
 702       break;
 703   }
 704
 705   position.SetRadius(alt + sea_level_radius);
 706 }
 707
 708 //******************************************************************************
 709 // Calculate the VCAS. Uses the Rayleigh formula for supersonic speeds
 710 // (See "Introduction to Aerodynamics of a Compressible Fluid - H.W. Liepmann,
 711 // A.E. Puckett - Wiley & sons (1947)" §5.4 pp 75-80)
 712
 713 double FGInitialCondition::calcVcas(double Mach) const
 714 {
 715   double altitudeASL = position.GetRadius() - sea_level_radius;
 716   double p=fdmex->GetAtmosphere()->GetPressure(altitudeASL);
 717   double psl=fdmex->GetAtmosphere()->GetPressureSL();
 718   double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
 719   double pt,A,vcas;
 720
 721   if (Mach < 0) Mach=0;
 722   if (Mach < 1)    //calculate total pressure assuming isentropic flow
 723     pt=p*pow((1 + 0.2*Mach*Mach),3.5);
 724   else {
 725     // shock in front of pitot tube, we'll assume its normal and use
 726     // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
 727     // pressure behind the shock to the static pressure in front of
 728     // the normal shock assumption should not be a bad one -- most supersonic
 729     // aircraft place the pitot probe out front so that it is the forward
 730     // most point on the aircraft.  The real shock would, of course, take
 731     // on something like the shape of a rounded-off cone but, here again,
 732     // the assumption should be good since the opening of the pitot probe
 733     // is very small and, therefore, the effects of the shock curvature
 734     // should be small as well. AFAIK, this approach is fairly well accepted
 735     // within the aerospace community
 736
 737     // The denominator below is zero for Mach ~ 0.38, for which
 738     // we'll never be here, so we're safe
 739
 740     pt = p*166.92158*pow(Mach,7.0)/pow(7*Mach*Mach-1,2.5);
 741   }
 742
 743   A = pow(((pt-p)/psl+1),0.28571);
 744   vcas = sqrt(7*psl/rhosl*(A-1));
 745   //cout << "calcVcas: vcas= " << vcas*fpstokts << " mach= " << Mach << " pressure: " << pt << endl;
 746   return vcas;
 747 }
 748
 749 //******************************************************************************
 750 // Reverse the VCAS formula to obtain the corresponding Mach number. For subsonic
 751 // speeds, the reversed formula has a closed form. For supersonic speeds, the
 752 // formula is reversed by the Newton-Raphson algorithm.
 753
 754 double FGInitialCondition::getMachFromVcas(double vcas)
 755 {
 756   double altitudeASL = position.GetRadius() - sea_level_radius;
 757   double p=fdmex->GetAtmosphere()->GetPressure(altitudeASL);
 758   double psl=fdmex->GetAtmosphere()->GetPressureSL();
 759   double rhosl=fdmex->GetAtmosphere()->GetDensitySL();
 760
 761   double pt = p + psl*(pow(1+vcas*vcas*rhosl/(7.0*psl),3.5)-1);
 762
 763   if (pt/p < 1.89293)
 764     return sqrt(5.0*(pow(pt/p, 0.2857143) -1)); // Mach < 1
 765   else {
 766     // Mach >= 1
 767     double mach = sqrt(0.77666*pt/p); // Initial guess is based on a quadratic approximation of the Rayleigh formula
 768     double delta = 1.;
 769     double target = pt/(166.92158*p);
 770     int iter = 0;
 771
 772     // Find the root with Newton-Raphson. Since the differential is never zero,
 773     // the function is monotonic and has only one root with a multiplicity of one.
 774     // Convergence is certain.
 775     while (delta > 1E-5 && iter < 10) {
 776       double m2 = mach*mach; // Mach^2
 777       double m6 = m2*m2*m2;  // Mach^6
 778       delta = mach*m6/pow(7.0*m2-1.0,2.5) - target;
 779       double diff = 7.0*m6*(2.0*m2-1)/pow(7.0*m2-1.0,3.5); // Never zero when Mach >= 1
 780       mach -= delta/diff;
 781       iter++;
 782     }
 783
 784     return mach;
 785   }
 786 }
 787
 788 //******************************************************************************
 789
 790 double FGInitialCondition::GetWindDirDegIC(void) const
 791 {
 792   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 793   FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
 794
 795   return _vWIND_NED(eV) == 0.0 ? 0.0
 796                                : atan2(_vWIND_NED(eV), _vWIND_NED(eU))*radtodeg;
 797 }
 798
 799 //******************************************************************************
 800
 801 double FGInitialCondition::GetNEDWindFpsIC(int idx) const
 802 {
 803   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 804   FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
 805
 806   return _vWIND_NED(idx);
 807 }
 808
 809 //******************************************************************************
 810
 811 double FGInitialCondition::GetWindFpsIC(void) const
 812 {
 813   FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
 814   FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
 815
 816   return _vWIND_NED.Magnitude(eU, eV);
 817 }
 818
 819 //******************************************************************************
 820
 821 double FGInitialCondition::GetBodyWindFpsIC(int idx) const
 822 {
 823   FGColumnVector3 _vt_BODY = Tw2b * FGColumnVector3(vt, 0., 0.);
 824   FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
 825   FGColumnVector3 _vWIND_BODY = _vt_BODY - _vUVW_BODY;
 826
 827   return _vWIND_BODY(idx);
 828 }
 829
 830 //******************************************************************************
 831
 832 double FGInitialCondition::GetVcalibratedKtsIC(void) const
 833 {
 834   double altitudeASL = position.GetRadius() - sea_level_radius;
 835   double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL);
 836   double soundSpeed = sqrt(SHRatio*Reng*temperature);
 837   double mach = vt / soundSpeed;
 838   return fpstokts * calcVcas(mach);
 839 }
 840
 841 //******************************************************************************
 842
 843 double FGInitialCondition::GetVequivalentKtsIC(void) const
 844 {
 845   double altitudeASL = position.GetRadius() - sea_level_radius;
 846   double rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL);
 847   double rhoSL = fdmex->GetAtmosphere()->GetDensitySL();
 848   return fpstokts * vt * sqrt(rho/rhoSL);
 849 }
 850
 851 //******************************************************************************
 852
 853 double FGInitialCondition::GetMachIC(void) const
 854 {
 855   double altitudeASL = position.GetRadius() - sea_level_radius;
 856   double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL);
 857   double soundSpeed = sqrt(SHRatio*Reng*temperature);
 858   return vt / soundSpeed;
 859 }
 860
 861 //******************************************************************************
 862
 863 double FGInitialCondition::GetBodyVelFpsIC(int idx) const
 864 {
 865   FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
 866
 867   return _vUVW_BODY(idx);
 868 }
 869
 870 //******************************************************************************
 871
 872 bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
 873 {
 874   string sep = "/";
 875   if( useStoredPath ) {
 876     init_file_name = fdmex->GetFullAircraftPath() + sep + rstfile + ".xml";
 877   } else {
 878     init_file_name = rstfile;
 879   }
 880
 881   document = LoadXMLDocument(init_file_name);
 882
 883   // Make sure that the document is valid
 884   if (!document) {
 885     cerr << "File: " << init_file_name << " could not be read." << endl;
 886     exit(-1);
 887   }
 888
 889   if (document->GetName() != string("initialize")) {
 890     cerr << "File: " << init_file_name << " is not a reset file." << endl;
 891     exit(-1);
 892   }
 893
 894   double version = document->GetAttributeValueAsNumber("version");
 895   bool result = false;
 896
 897   if (version == HUGE_VAL) {
 898     result = Load_v1(); // Default to the old version
 899   } else if (version >= 3.0) {
 900     cerr << "Only initialization file formats 1 and 2 are currently supported" << endl;
 901     exit (-1);
 902   } else if (version >= 2.0) {
 903     result = Load_v2();
 904   } else if (version >= 1.0) {
 905     result = Load_v1();
 906   }
 907
 908   // Check to see if any engines are specified to be initialized in a running state
 909   FGPropulsion* propulsion = fdmex->GetPropulsion();
 910   Element* running_elements = document->FindElement("running");
 911   while (running_elements) {
 912     int n = int(running_elements->GetDataAsNumber());
 913     try {
 914       propulsion->InitRunning(n);
 915     } catch (string str) {
 916       cerr << str << endl;
 917       result = false;
 918     }
 919     running_elements = document->FindNextElement("running");
 920   }
 921
 922   fdmex->RunIC();
 923   fdmex->GetPropagate()->DumpState();
 924
 925   return result;
 926 }
 927
 928 //******************************************************************************
 929
 930 bool FGInitialCondition::Load_v1(void)
 931 {
 932   bool result = true;
 933
 934   if (document->FindElement("latitude"))
 935     position.SetLatitude(document->FindElementValueAsNumberConvertTo("latitude", "RAD"));
 936   if (document->FindElement("longitude"))
 937     position.SetLongitude(document->FindElementValueAsNumberConvertTo("longitude", "RAD"));
 938   if (document->FindElement("elevation"))
 939     terrain_elevation = document->FindElementValueAsNumberConvertTo("elevation", "FT");
 940
 941   if (document->FindElement("altitude")) // This is feet above ground level
 942     position.SetRadius(document->FindElementValueAsNumberConvertTo("altitude", "FT") + terrain_elevation + sea_level_radius);
 943   else if (document->FindElement("altitudeAGL")) // This is feet above ground level
 944     position.SetRadius(document->FindElementValueAsNumberConvertTo("altitudeAGL", "FT") + terrain_elevation + sea_level_radius);
 945   else if (document->FindElement("altitudeMSL")) // This is feet above sea level
 946     position.SetRadius(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT") + sea_level_radius);
 947
 948   if (document->FindElement("phi"))
 949     phi = document->FindElementValueAsNumberConvertTo("phi", "RAD");
 950   if (document->FindElement("theta"))
 951     theta = document->FindElementValueAsNumberConvertTo("theta", "RAD");
 952   if (document->FindElement("psi"))
 953     psi = document->FindElementValueAsNumberConvertTo("psi", "RAD");
 954
 955   FGQuaternion Quat(phi, theta, psi);
 956   Quat.Normalize();
 957   Tl2b = Quat.GetT();
 958   Tb2l = Quat.GetTInv();
 959
 960   if (document->FindElement("ubody"))
 961     SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo("ubody", "FT/SEC"));
 962   if (document->FindElement("vbody"))
 963     SetVBodyFpsIC(document->FindElementValueAsNumberConvertTo("vbody", "FT/SEC"));
 964   if (document->FindElement("wbody"))
 965     SetWBodyFpsIC(document->FindElementValueAsNumberConvertTo("wbody", "FT/SEC"));
 966   if (document->FindElement("vnorth"))
 967     SetVNorthFpsIC(document->FindElementValueAsNumberConvertTo("vnorth", "FT/SEC"));
 968   if (document->FindElement("veast"))
 969     SetVEastFpsIC(document->FindElementValueAsNumberConvertTo("veast", "FT/SEC"));
 970   if (document->FindElement("vdown"))
 971     SetVDownFpsIC(document->FindElementValueAsNumberConvertTo("vdown", "FT/SEC"));
 972   if (document->FindElement("vc"))
 973     SetVcalibratedKtsIC(document->FindElementValueAsNumberConvertTo("vc", "KTS"));
 974   if (document->FindElement("vt"))
 975     SetVtrueKtsIC(document->FindElementValueAsNumberConvertTo("vt", "KTS"));
 976   if (document->FindElement("mach"))
 977     SetMachIC(document->FindElementValueAsNumber("mach"));
 978   if (document->FindElement("gamma"))
 979     SetFlightPathAngleDegIC(document->FindElementValueAsNumberConvertTo("gamma", "DEG"));
 980   if (document->FindElement("roc"))
 981     SetClimbRateFpsIC(document->FindElementValueAsNumberConvertTo("roc", "FT/SEC"));
 982   if (document->FindElement("vground"))
 983     SetVgroundKtsIC(document->FindElementValueAsNumberConvertTo("vground", "KTS"));
 984   if (document->FindElement("alpha"))
 985     SetAlphaDegIC(document->FindElementValueAsNumberConvertTo("alpha", "DEG"));
 986   if (document->FindElement("beta"))
 987     SetBetaDegIC(document->FindElementValueAsNumberConvertTo("beta", "DEG"));
 988   if (document->FindElement("vwind"))
 989     SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo("vwind", "KTS"));
 990   if (document->FindElement("winddir"))
 991     SetWindDirDegIC(document->FindElementValueAsNumberConvertTo("winddir", "DEG"));
 992   if (document->FindElement("hwind"))
 993     SetHeadWindKtsIC(document->FindElementValueAsNumberConvertTo("hwind", "KTS"));
 994   if (document->FindElement("xwind"))
 995     SetCrossWindKtsIC(document->FindElementValueAsNumberConvertTo("xwind", "KTS"));
 996   if (document->FindElement("targetNlf"))
 997   {
 998     SetTargetNlfIC(document->FindElementValueAsNumber("targetNlf"));
 999   }
1000
1001   return result;
1002 }
1003
1004 //******************************************************************************
1005
1006 bool FGInitialCondition::Load_v2(void)
1007 {
1008   FGColumnVector3 vOrient;
1009   bool result = true;
1010   FGColumnVector3 vOmegaEarth = FGColumnVector3(0.0, 0.0, fdmex->GetInertial()->omega());
1011
1012   if (document->FindElement("earth_position_angle"))
1013     position.SetEarthPositionAngle(document->FindElementValueAsNumberConvertTo("earth_position_angle", "RAD"));
1014
1015   if (document->FindElement("elevation"))
1016     terrain_elevation = document->FindElementValueAsNumberConvertTo("elevation", "FT");
1017
1018   // Initialize vehicle position
1019   //
1020   // Allowable frames:
1021   // - ECI (Earth Centered Inertial)
1022   // - ECEF (Earth Centered, Earth Fixed)
1023
1024   Element* position_el = document->FindElement("position");
1025   if (position_el) {
1026     string frame = position_el->GetAttributeValue("frame");
1027     frame = to_lower(frame);
1028     if (frame == "eci") { // Need to transform vLoc to ECEF for storage and use in FGLocation.
1029       position = position.GetTi2ec() * position_el->FindElementTripletConvertTo("FT");
1030     } else if (frame == "ecef") {
1031       if (!position_el->FindElement("x") && !position_el->FindElement("y") && !position_el->FindElement("z")) {
1032         if (position_el->FindElement("radius")) {
1033           position.SetRadius(position_el->FindElementValueAsNumberConvertTo("radius", "FT"));
1034         } else if (position_el->FindElement("altitudeAGL")) {
1035           position.SetRadius(sea_level_radius + terrain_elevation + position_el->FindElementValueAsNumberConvertTo("altitude", "FT"));
1036         } else if (position_el->FindElement("altitudeMSL")) {
1037           position.SetRadius(sea_level_radius + position_el->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
1038         } else {
1039           cerr << endl << "  No altitude or radius initial condition is given." << endl;
1040           result = false;
1041         }
1042         if (position_el->FindElement("longitude"))
1043           position.SetLongitude(position_el->FindElementValueAsNumberConvertTo("longitude", "RAD"));
1044         if (position_el->FindElement("latitude"))
1045           position.SetLatitude(position_el->FindElementValueAsNumberConvertTo("latitude", "RAD"));
1046       } else {
1047         position = position_el->FindElementTripletConvertTo("FT");
1048       }
1049     } else {
1050       cerr << endl << "  Neither ECI nor ECEF frame is specified for initial position." << endl;
1051       result = false;
1052     }
1053   } else {
1054     cerr << endl << "  Initial position not specified in this initialization file." << endl;
1055     result = false;
1056   }
1057
1058   // End of position initialization
1059
1060   // Initialize vehicle orientation
1061   // Allowable frames
1062   // - ECI (Earth Centered Inertial)
1063   // - ECEF (Earth Centered, Earth Fixed)
1064   // - Local
1065   //
1066   // Need to convert the provided orientation to a local orientation, using
1067   // the given orientation and knowledge of the Earth position angle.
1068   // This could be done using matrices (where in the subscript "b/a",
1069   // it is meant "b with respect to a", and where b=body frame,
1070   // i=inertial frame, and e=ecef frame) as:
1071   //
1072   // C_b/l =  C_b/e * C_e/l
1073   //
1074   // Using quaternions (note reverse ordering compared to matrix representation):
1075   //
1076   // Q_b/l = Q_e/l * Q_b/e
1077   //
1078   // Use the specific matrices as needed. The above example of course is for the whole
1079   // body to local orientation.
1080   // The new orientation angles can be extracted from the matrix or the quaternion.
1081   // ToDo: Do we need to deal with normalization of the quaternions here?
1082
1083   Element* orientation_el = document->FindElement("orientation");
1084   FGQuaternion QuatLocal2Body;
1085   if (orientation_el) {
1086     string frame = orientation_el->GetAttributeValue("frame");
1087     frame = to_lower(frame);
1088     vOrient = orientation_el->FindElementTripletConvertTo("RAD");
1089     if (frame == "eci") {
1090
1091       // In this case, we are supplying the Euler angles for the vehicle with
1092       // respect to the inertial system, represented by the C_b/i Matrix.
1093       // We want the body orientation with respect to the local (NED frame):
1094       //
1095       // C_b/l = C_b/i * C_i/l
1096       //
1097       // Or, using quaternions (note reverse ordering compared to matrix representation):
1098       //
1099       // Q_b/l = Q_e/l * Q_b/i
1100
1101       FGQuaternion QuatI2Body = FGQuaternion(vOrient);
1102       QuatI2Body.Normalize();
1103       FGQuaternion QuatLocal2I = position.GetTl2i();
1104       QuatLocal2I.Normalize();
1105       QuatLocal2Body = QuatLocal2I * QuatI2Body;
1106
1107     } else if (frame == "ecef") {
1108
1109       // In this case we are given the Euler angles representing the orientation of
1110       // the body with respect to the ECEF system, represented by the C_b/e Matrix.
1111       // We want the body orientation with respect to the local (NED frame):
1112       //
1113       // C_b/l =  C_b/e * C_e/l
1114       //
1115       // Using quaternions (note reverse ordering compared to matrix representation):
1116       //
1117       // Q_b/l = Q_e/l * Q_b/e
1118
1119       FGQuaternion QuatEC2Body(vOrient); // Store relationship of Body frame wrt ECEF frame, Q_b/e
1120       QuatEC2Body.Normalize();
1121       FGQuaternion QuatLocal2EC = position.GetTl2ec(); // Get Q_e/l from matrix
1122       QuatLocal2EC.Normalize();
1123       QuatLocal2Body = QuatLocal2EC * QuatEC2Body; // Q_b/l = Q_e/l * Q_b/e
1124
1125     } else if (frame == "local") {
1126
1127       QuatLocal2Body = FGQuaternion(vOrient);
1128
1129     } else {
1130
1131       cerr << endl << fgred << "  Orientation frame type: \"" << frame
1132            << "\" is not supported!" << reset << endl << endl;
1133       result = false;
1134
1135     }
1136   }
1137
1138   QuatLocal2Body.Normalize();
1139   phi = QuatLocal2Body.GetEuler(ePhi);
1140   theta = QuatLocal2Body.GetEuler(eTht);
1141   psi = QuatLocal2Body.GetEuler(ePsi);
1142   Tl2b = QuatLocal2Body.GetT();
1143   Tb2l = QuatLocal2Body.GetTInv();
1144
1145   // Initialize vehicle velocity
1146   // Allowable frames
1147   // - ECI (Earth Centered Inertial)
1148   // - ECEF (Earth Centered, Earth Fixed)
1149   // - Local
1150   // - Body
1151   // The vehicle will be defaulted to (0,0,0) in the Body frame if nothing is provided.
1152
1153   Element* velocity_el = document->FindElement("velocity");
1154   FGColumnVector3 vInitVelocity = FGColumnVector3(0.0, 0.0, 0.0);
1155   FGMatrix33 mTec2l = position.GetTec2l();
1156   if (velocity_el) {
1157
1158     string frame = velocity_el->GetAttributeValue("frame");
1159     frame = to_lower(frame);
1160     FGColumnVector3 vInitVelocity = velocity_el->FindElementTripletConvertTo("FT/SEC");
1161
1162     if (frame == "eci") {
1163       FGColumnVector3 omega_cross_r = vOmegaEarth * (position.GetTec2i() * position);
1164       vUVW_NED = mTec2l * (vInitVelocity - omega_cross_r);
1165     } else if (frame == "ecef") {
1166       vUVW_NED = mTec2l * vInitVelocity;
1167     } else if (frame == "local") {
1168       vUVW_NED = vInitVelocity;
1169       lastSpeedSet = setned;
1170     } else if (frame == "body") {
1171       vUVW_NED = Tb2l * vInitVelocity;
1172       lastSpeedSet = setuvw;
1173     } else {
1174
1175       cerr << endl << fgred << "  Velocity frame type: \"" << frame
1176            << "\" is not supported!" << reset << endl << endl;
1177       result = false;
1178
1179     }
1180
1181   } else {
1182
1183     vUVW_NED = Tb2l * vInitVelocity;
1184
1185   }
1186
1187   vt = vUVW_NED.Magnitude();
1188
1189   calcAeroAngles(vUVW_NED);
1190
1191   // Initialize vehicle body rates
1192   // Allowable frames
1193   // - ECI (Earth Centered Inertial)
1194   // - ECEF (Earth Centered, Earth Fixed)
1195   // - Body
1196
1197   FGColumnVector3 vLocalRate;
1198   Element* attrate_el = document->FindElement("attitude_rate");
1199   double radInv = 1.0 / position.GetRadius();
1200   FGColumnVector3 vOmegaLocal = FGColumnVector3(
1201    radInv*vUVW_NED(eEast),
1202   -radInv*vUVW_NED(eNorth),
1203   -radInv*vUVW_NED(eEast)*position.GetTanLatitude() );
1204
1205   if (attrate_el) {
1206
1207     string frame = attrate_el->GetAttributeValue("frame");
1208     frame = to_lower(frame);
1209     FGColumnVector3 vAttRate = attrate_el->FindElementTripletConvertTo("RAD/SEC");
1210
1211     if (frame == "eci") {
1212       vLocalRate = Tl2b * (position.GetTi2l() * (vAttRate - vOmegaEarth) - vOmegaLocal);
1213     } else if (frame == "ecef") {
1214       vLocalRate = Tl2b * (position.GetTec2l() * vAttRate - vOmegaLocal);
1215     } else if (frame == "local") {
1216       vLocalRate = vAttRate;
1217     } else if (!frame.empty()) { // misspelling of frame
1218
1219       cerr << endl << fgred << "  Attitude rate frame type: \"" << frame
1220            << "\" is not supported!" << reset << endl << endl;
1221       result = false;
1222
1223     } else if (frame.empty()) {
1224
1225     }
1226
1227   } else { // Body frame attitude rate assumed 0 relative to local.
1228       vLocalRate.InitMatrix();
1229   }
1230
1231   p = vLocalRate(eP);
1232   q = vLocalRate(eQ);
1233   r = vLocalRate(eR);
1234
1235   return result;
1236 }
1237
1238 //******************************************************************************
1239
1240 void FGInitialCondition::bind(void)
1241 {
1242   PropertyManager->Tie("ic/vc-kts", this,
1243                        &FGInitialCondition::GetVcalibratedKtsIC,
1244                        &FGInitialCondition::SetVcalibratedKtsIC,
1245                        true);
1246   PropertyManager->Tie("ic/ve-kts", this,
1247                        &FGInitialCondition::GetVequivalentKtsIC,
1248                        &FGInitialCondition::SetVequivalentKtsIC,
1249                        true);
1250   PropertyManager->Tie("ic/vg-kts", this,
1251                        &FGInitialCondition::GetVgroundKtsIC,
1252                        &FGInitialCondition::SetVgroundKtsIC,
1253                        true);
1254   PropertyManager->Tie("ic/vt-kts", this,
1255                        &FGInitialCondition::GetVtrueKtsIC,
1256                        &FGInitialCondition::SetVtrueKtsIC,
1257                        true);
1258   PropertyManager->Tie("ic/mach", this,
1259                        &FGInitialCondition::GetMachIC,
1260                        &FGInitialCondition::SetMachIC,
1261                        true);
1262   PropertyManager->Tie("ic/roc-fpm", this,
1263                        &FGInitialCondition::GetClimbRateFpmIC,
1264                        &FGInitialCondition::SetClimbRateFpmIC,
1265                        true);
1266   PropertyManager->Tie("ic/gamma-deg", this,
1267                        &FGInitialCondition::GetFlightPathAngleDegIC,
1268                        &FGInitialCondition::SetFlightPathAngleDegIC,
1269                        true);
1270   PropertyManager->Tie("ic/alpha-deg", this,
1271                        &FGInitialCondition::GetAlphaDegIC,
1272                        &FGInitialCondition::SetAlphaDegIC,
1273                        true);
1274   PropertyManager->Tie("ic/beta-deg", this,
1275                        &FGInitialCondition::GetBetaDegIC,
1276                        &FGInitialCondition::SetBetaDegIC,
1277                        true);
1278   PropertyManager->Tie("ic/theta-deg", this,
1279                        &FGInitialCondition::GetThetaDegIC,
1280                        &FGInitialCondition::SetThetaDegIC,
1281                        true);
1282   PropertyManager->Tie("ic/phi-deg", this,
1283                        &FGInitialCondition::GetPhiDegIC,
1284                        &FGInitialCondition::SetPhiDegIC,
1285                        true);
1286   PropertyManager->Tie("ic/psi-true-deg", this,
1287                        &FGInitialCondition::GetPsiDegIC );
1288   PropertyManager->Tie("ic/lat-gc-deg", this,
1289                        &FGInitialCondition::GetLatitudeDegIC,
1290                        &FGInitialCondition::SetLatitudeDegIC,
1291                        true);
1292   PropertyManager->Tie("ic/long-gc-deg", this,
1293                        &FGInitialCondition::GetLongitudeDegIC,
1294                        &FGInitialCondition::SetLongitudeDegIC,
1295                        true);
1296   PropertyManager->Tie("ic/h-sl-ft", this,
1297                        &FGInitialCondition::GetAltitudeASLFtIC,
1298                        &FGInitialCondition::SetAltitudeASLFtIC,
1299                        true);
1300   PropertyManager->Tie("ic/h-agl-ft", this,
1301                        &FGInitialCondition::GetAltitudeAGLFtIC,
1302                        &FGInitialCondition::SetAltitudeAGLFtIC,
1303                        true);
1304   PropertyManager->Tie("ic/sea-level-radius-ft", this,
1305                        &FGInitialCondition::GetSeaLevelRadiusFtIC,
1306                        &FGInitialCondition::SetSeaLevelRadiusFtIC,
1307                        true);
1308   PropertyManager->Tie("ic/terrain-elevation-ft", this,
1309                        &FGInitialCondition::GetTerrainElevationFtIC,
1310                        &FGInitialCondition::SetTerrainElevationFtIC,
1311                        true);
1312   PropertyManager->Tie("ic/vg-fps", this,
1313                        &FGInitialCondition::GetVgroundFpsIC,
1314                        &FGInitialCondition::SetVgroundFpsIC,
1315                        true);
1316   PropertyManager->Tie("ic/vt-fps", this,
1317                        &FGInitialCondition::GetVtrueFpsIC,
1318                        &FGInitialCondition::SetVtrueFpsIC,
1319                        true);
1320   PropertyManager->Tie("ic/vw-bx-fps", this,
1321                        &FGInitialCondition::GetWindUFpsIC);
1322   PropertyManager->Tie("ic/vw-by-fps", this,
1323                        &FGInitialCondition::GetWindVFpsIC);
1324   PropertyManager->Tie("ic/vw-bz-fps", this,
1325                        &FGInitialCondition::GetWindWFpsIC);
1326   PropertyManager->Tie("ic/vw-north-fps", this,
1327                        &FGInitialCondition::GetWindNFpsIC);
1328   PropertyManager->Tie("ic/vw-east-fps", this,
1329                        &FGInitialCondition::GetWindEFpsIC);
1330   PropertyManager->Tie("ic/vw-down-fps", this,
1331                        &FGInitialCondition::GetWindDFpsIC);
1332   PropertyManager->Tie("ic/vw-mag-fps", this,
1333                        &FGInitialCondition::GetWindFpsIC);
1334   PropertyManager->Tie("ic/vw-dir-deg", this,
1335                        &FGInitialCondition::GetWindDirDegIC,
1336                        &FGInitialCondition::SetWindDirDegIC,
1337                        true);
1338
1339   PropertyManager->Tie("ic/roc-fps", this,
1340                        &FGInitialCondition::GetClimbRateFpsIC,
1341                        &FGInitialCondition::SetClimbRateFpsIC,
1342                        true);
1343   PropertyManager->Tie("ic/u-fps", this,
1344                        &FGInitialCondition::GetUBodyFpsIC,
1345                        &FGInitialCondition::SetUBodyFpsIC,
1346                        true);
1347   PropertyManager->Tie("ic/v-fps", this,
1348                        &FGInitialCondition::GetVBodyFpsIC,
1349                        &FGInitialCondition::SetVBodyFpsIC,
1350                        true);
1351   PropertyManager->Tie("ic/w-fps", this,
1352                        &FGInitialCondition::GetWBodyFpsIC,
1353                        &FGInitialCondition::SetWBodyFpsIC,
1354                        true);
1355   PropertyManager->Tie("ic/vn-fps", this,
1356                        &FGInitialCondition::GetVNorthFpsIC,
1357                        &FGInitialCondition::SetVNorthFpsIC,
1358                        true);
1359   PropertyManager->Tie("ic/ve-fps", this,
1360                        &FGInitialCondition::GetVEastFpsIC,
1361                        &FGInitialCondition::SetVEastFpsIC,
1362                        true);
1363   PropertyManager->Tie("ic/vd-fps", this,
1364                        &FGInitialCondition::GetVDownFpsIC,
1365                        &FGInitialCondition::SetVDownFpsIC,
1366                        true);
1367   PropertyManager->Tie("ic/gamma-rad", this,
1368                        &FGInitialCondition::GetFlightPathAngleRadIC,
1369                        &FGInitialCondition::SetFlightPathAngleRadIC,
1370                        true);
1371   PropertyManager->Tie("ic/alpha-rad", this,
1372                        &FGInitialCondition::GetAlphaRadIC,
1373                        &FGInitialCondition::SetAlphaRadIC,
1374                        true);
1375   PropertyManager->Tie("ic/theta-rad", this,
1376                        &FGInitialCondition::GetThetaRadIC,
1377                        &FGInitialCondition::SetThetaRadIC,
1378                        true);
1379   PropertyManager->Tie("ic/beta-rad", this,
1380                        &FGInitialCondition::GetBetaRadIC,
1381                        &FGInitialCondition::SetBetaRadIC,
1382                        true);
1383   PropertyManager->Tie("ic/phi-rad", this,
1384                        &FGInitialCondition::GetPhiRadIC,
1385                        &FGInitialCondition::SetPhiRadIC,
1386                        true);
1387   PropertyManager->Tie("ic/psi-true-rad", this,
1388                        &FGInitialCondition::GetPsiRadIC);
1389   PropertyManager->Tie("ic/lat-gc-rad", this,
1390                        &FGInitialCondition::GetLatitudeRadIC,
1391                        &FGInitialCondition::SetLatitudeRadIC,
1392                        true);
1393   PropertyManager->Tie("ic/long-gc-rad", this,
1394                        &FGInitialCondition::GetLongitudeRadIC,
1395                        &FGInitialCondition::SetLongitudeRadIC,
1396                        true);
1397   PropertyManager->Tie("ic/p-rad_sec", this,
1398                        &FGInitialCondition::GetPRadpsIC,
1399                        &FGInitialCondition::SetPRadpsIC,
1400                        true);
1401   PropertyManager->Tie("ic/q-rad_sec", this,
1402                        &FGInitialCondition::GetQRadpsIC,
1403                        &FGInitialCondition::SetQRadpsIC,
1404                        true);
1405   PropertyManager->Tie("ic/r-rad_sec", this,
1406                        &FGInitialCondition::GetRRadpsIC,
1407                        &FGInitialCondition::SetRRadpsIC,
1408                        true);
1409
1410   typedef int (FGInitialCondition::*iPMF)(void) const;
1411   PropertyManager->Tie("simulation/write-state-file",
1412                        this,
1413                        (iPMF)0,
1414                        &FGInitialCondition::WriteStateFile);
1415
1416 }
1417
1418 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
1419 //    The bitmasked value choices are as follows:
1420 //    unset: In this case (the default) JSBSim would only print
1421 //       out the normally expected messages, essentially echoing
1422 //       the config files as they are read. If the environment
1423 //       variable is not set, debug_lvl is set to 1 internally
1424 //    0: This requests JSBSim not to output any messages
1425 //       whatsoever.
1426 //    1: This value explicity requests the normal JSBSim
1427 //       startup messages
1428 //    2: This value asks for a message to be printed out when
1429 //       a class is instantiated
1430 //    4: When this value is set, a message is displayed when a
1431 //       FGModel object executes its Run() method
1432 //    8: When this value is set, various runtime state variables
1433 //       are printed out periodically
1434 //    16: When set various parameters are sanity checked and
1435 //       a message is printed out when they go out of bounds
1436
1437 void FGInitialCondition::Debug(int from)
1438 {
1439   if (debug_lvl <= 0) return;
1440
1441   if (debug_lvl & 1) { // Standard console startup message output
1442   }
1443   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
1444     if (from == 0) cout << "Instantiated: FGInitialCondition" << endl;
1445     if (from == 1) cout << "Destroyed:    FGInitialCondition" << endl;
1446   }
1447   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
1448   }
1449   if (debug_lvl & 8 ) { // Runtime state variables
1450   }
1451   if (debug_lvl & 16) { // Sanity checking
1452   }
1453   if (debug_lvl & 64) {
1454     if (from == 0) { // Constructor
1455       cout << IdSrc << endl;
1456       cout << IdHdr << endl;
1457     }
1458   }
1459 }
1460 }