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