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