src/FDM/JSBSim/initialization/FGInitialCondition.cpp

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