src/FDM/JSBSim/FGState.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGState.cpp
   4  Author:       Jon Berndt
   5  Date started: 11/17/98
   6  Called by:    FGFDMExec and accessed by all models.
   7
   8  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
   9
  10  This program is free software; you can redistribute it and/or modify it under
  11  the terms of the GNU General Public License as published by the Free Software
  12  Foundation; either version 2 of the License, or (at your option) any later
  13  version.
  14
  15  This program is distributed in the hope that it will be useful, but WITHOUT
  16  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  17  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  18  details.
  19
  20  You should have received a copy of the GNU General Public License along with
  21  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  22  Place - Suite 330, Boston, MA  02111-1307, USA.
  23
  24  Further information about the GNU General Public License can also be found on
  25  the world wide web at http://www.gnu.org.
  26
  27 FUNCTIONAL DESCRIPTION
  28 --------------------------------------------------------------------------------
  29 See header file.
  30
  31 HISTORY
  32 --------------------------------------------------------------------------------
  33 11/17/98   JSB   Created
  34
  35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  36 INCLUDES
  37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  38
  39 #ifdef FGFS
  40 #  include <simgear/compiler.h>
  41 #  include <math.h>
  42 #else
  43 #  include <cmath>
  44 #endif
  45
  46 #ifndef M_PI
  47 #  include <simgear/constants.h>
  48 #  define M_PI SG_PI
  49 #endif
  50
  51 #include "FGState.h"
  52
  53 static const char *IdSrc = "$Id$";
  54 static const char *IdHdr = ID_STATE;
  55
  56 extern short debug_lvl;
  57
  58 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  59 MACROS
  60 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  61
  62 #define RegisterVariable(ID,DEF) coeffdef[#ID] = ID; paramdef[ID] = DEF
  63
  64 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  65 CLASS IMPLEMENTATION
  66 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  67
  68 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  69 //
  70 // For every term registered here there must be a corresponding handler in
  71 // GetParameter() below that retrieves that parameter. Also, there must be an
  72 // entry in the enum eParam definition in FGDefs.h. The ID is what must be used
  73 // in any config file entry which references that item.
  74
  75 FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
  76     mTl2b(3,3),
  77     mTs2b(3,3),
  78     vQtrn(4),
  79     vlastQdot(4),
  80     vQdot(4),
  81     vTmp(4),
  82     vEuler(3)
  83 {
  84   FDMExec = fdex;
  85
  86   a = 1000.0;
  87   sim_time = 0.0;
  88   dt = 1.0/120.0;
  89   ActiveEngine = -1;
  90
  91   Aircraft     = FDMExec->GetAircraft();
  92   Translation  = FDMExec->GetTranslation();
  93   Rotation     = FDMExec->GetRotation();
  94   Position     = FDMExec->GetPosition();
  95   FCS          = FDMExec->GetFCS();
  96   Output       = FDMExec->GetOutput();
  97   Atmosphere   = FDMExec->GetAtmosphere();
  98   Aerodynamics = FDMExec->GetAerodynamics();
  99
 100   RegisterVariable(FG_TIME,           " time "           );
 101   RegisterVariable(FG_QBAR,           " qbar "           );
 102   RegisterVariable(FG_WINGAREA,       " wing_area "      );
 103   RegisterVariable(FG_WINGSPAN,       " wingspan "       );
 104   RegisterVariable(FG_CBAR,           " cbar "           );
 105   RegisterVariable(FG_ALPHA,          " alpha "          );
 106   RegisterVariable(FG_ALPHADOT,       " alphadot "       );
 107   RegisterVariable(FG_BETA,           " beta "           );
 108   RegisterVariable(FG_BETADOT,        " betadot "        );
 109   RegisterVariable(FG_PHI,            " roll_angle "     );
 110   RegisterVariable(FG_THT,            " pitch_angle "    );
 111   RegisterVariable(FG_PSI,            " heading_angle "  );
 112   RegisterVariable(FG_PITCHRATE,      " pitch_rate "     );
 113   RegisterVariable(FG_ROLLRATE,       " roll_rate "      );
 114   RegisterVariable(FG_YAWRATE,        " yaw_rate "       );
 115   RegisterVariable(FG_CL_SQRD,        " Clift_sqrd "     );
 116   RegisterVariable(FG_MACH,           " mach "           );
 117   RegisterVariable(FG_ALTITUDE,       " altitude "       );
 118   RegisterVariable(FG_BI2VEL,         " BI2Vel "         );
 119   RegisterVariable(FG_CI2VEL,         " CI2Vel "         );
 120   RegisterVariable(FG_ELEVATOR_POS,   " elevator_pos "   );
 121   RegisterVariable(FG_AILERON_POS,    " aileron_pos "    );
 122   RegisterVariable(FG_RUDDER_POS,     " rudder_pos "     );
 123   RegisterVariable(FG_SPDBRAKE_POS,   " speedbrake_pos " );
 124   RegisterVariable(FG_SPOILERS_POS,   " spoiler_pos "    );
 125   RegisterVariable(FG_FLAPS_POS,      " flaps_pos "      );
 126   RegisterVariable(FG_ELEVATOR_CMD,   " elevator_cmd "   );
 127   RegisterVariable(FG_AILERON_CMD,    " aileron_cmd "    );
 128   RegisterVariable(FG_RUDDER_CMD,     " rudder_cmd "     );
 129   RegisterVariable(FG_SPDBRAKE_CMD,   " speedbrake_cmd " );
 130   RegisterVariable(FG_SPOILERS_CMD,   " spoiler_cmd "    );
 131   RegisterVariable(FG_FLAPS_CMD,      " flaps_cmd "      );
 132   RegisterVariable(FG_THROTTLE_CMD,   " throttle_cmd "   );
 133   RegisterVariable(FG_THROTTLE_POS,   " throttle_pos "   );
 134   RegisterVariable(FG_ACTIVE_ENGINE,  " active_engine "  );
 135   RegisterVariable(FG_HOVERB,         " height/span "    );
 136   RegisterVariable(FG_PITCH_TRIM_CMD, " pitch_trim_cmd " );
 137   RegisterVariable(FG_LEFT_BRAKE_CMD, " left_brake_cmd " );
 138   RegisterVariable(FG_RIGHT_BRAKE_CMD," right_brake_cmd ");
 139   RegisterVariable(FG_CENTER_BRAKE_CMD," center_brake_cmd ");
 140   RegisterVariable(FG_SET_LOGGING,    " data_logging "   );
 141
 142   if (debug_lvl & 2) cout << "Instantiated: FGState" << endl;
 143 }
 144
 145 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 146
 147 FGState::~FGState()
 148 {
 149   if (debug_lvl & 2) cout << "Destroyed:    FGState" << endl;
 150 }
 151
 152 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 153
 154 float FGState::GetParameter(eParam val_idx) {
 155   float scratch;
 156
 157   switch(val_idx) {
 158   case FG_TIME:
 159     return sim_time;
 160   case FG_QBAR:
 161     return Translation->Getqbar();
 162   case FG_WINGAREA:
 163     return Aircraft->GetWingArea();
 164   case FG_WINGSPAN:
 165     return Aircraft->GetWingSpan();
 166   case FG_CBAR:
 167     return Aircraft->Getcbar();
 168   case FG_ALPHA:
 169     return Translation->Getalpha();
 170   case FG_ALPHADOT:
 171     return Translation->Getadot();
 172   case FG_BETA:
 173     return Translation->Getbeta();
 174   case FG_BETADOT:
 175     return Translation->Getbdot();
 176   case FG_PHI:
 177     return Rotation->Getphi();
 178   case FG_THT:
 179     return Rotation->Gettht();
 180   case FG_PSI:
 181     return Rotation->Getpsi();
 182   case FG_PITCHRATE:
 183     return Rotation->GetPQR(eQ);
 184   case FG_ROLLRATE:
 185     return Rotation->GetPQR(eP);
 186   case FG_YAWRATE:
 187     return Rotation->GetPQR(eR);
 188   case FG_CL_SQRD:
 189     if (Translation->Getqbar() > 0.00)
 190       scratch = Aerodynamics->GetvLastFs(eLift)/(Aircraft->GetWingArea()*Translation->Getqbar());
 191     else
 192       scratch = 0.0;
 193     return scratch*scratch;
 194   case FG_ELEVATOR_POS:
 195     return FCS->GetDePos();
 196   case FG_AILERON_POS:
 197     return FCS->GetDaPos();
 198   case FG_RUDDER_POS:
 199     return FCS->GetDrPos();
 200   case FG_SPDBRAKE_POS:
 201     return FCS->GetDsbPos();
 202   case FG_SPOILERS_POS:
 203     return FCS->GetDspPos();
 204   case FG_FLAPS_POS:
 205     return FCS->GetDfPos();
 206   case FG_ELEVATOR_CMD:
 207     return FCS->GetDeCmd();
 208   case FG_AILERON_CMD:
 209     return FCS->GetDaCmd();
 210   case FG_RUDDER_CMD:
 211     return FCS->GetDrCmd();
 212   case FG_SPDBRAKE_CMD:
 213     return FCS->GetDsbCmd();
 214   case FG_SPOILERS_CMD:
 215     return FCS->GetDspCmd();
 216   case FG_FLAPS_CMD:
 217     return FCS->GetDfCmd();
 218   case FG_MACH:
 219     return Translation->GetMach();
 220   case FG_ALTITUDE:
 221     return Position->Geth();
 222   case FG_BI2VEL:
 223     if(Translation->GetVt() > 0)
 224         return Aircraft->GetWingSpan()/(2.0 * Translation->GetVt());
 225     else
 226         return 0;
 227   case FG_CI2VEL:
 228     if(Translation->GetVt() > 0)
 229         return Aircraft->Getcbar()/(2.0 * Translation->GetVt());
 230     else
 231         return 0;
 232   case FG_THROTTLE_CMD:
 233     if (ActiveEngine < 0) return FCS->GetThrottleCmd(0);
 234     else return FCS->GetThrottleCmd(ActiveEngine);
 235   case FG_THROTTLE_POS:
 236     if (ActiveEngine < 0) return FCS->GetThrottlePos(0);
 237     else return FCS->GetThrottlePos(ActiveEngine);
 238   case FG_HOVERB:
 239     return Position->GetHOverB();
 240   case FG_PITCH_TRIM_CMD:
 241     return FCS->GetPitchTrimCmd();
 242   default:
 243     cerr << "FGState::GetParameter() - No handler for parameter " << val_idx << endl;
 244     return 0.0;
 245   }
 246   return 0;
 247 }
 248
 249 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 250
 251 float FGState::GetParameter(string val_string) {
 252   return GetParameter(coeffdef[val_string]);
 253 }
 254
 255 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 256
 257 eParam FGState::GetParameterIndex(string val_string) {
 258   return coeffdef[val_string];
 259 }
 260
 261 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 262
 263 void FGState::SetParameter(eParam val_idx, float val) {
 264   switch(val_idx) {
 265   case FG_ELEVATOR_POS:
 266     FCS->SetDePos(val);
 267     break;
 268   case FG_AILERON_POS:
 269     FCS->SetDaPos(val);
 270     break;
 271   case FG_RUDDER_POS:
 272     FCS->SetDrPos(val);
 273     break;
 274   case FG_SPDBRAKE_POS:
 275     FCS->SetDsbPos(val);
 276     break;
 277   case FG_SPOILERS_POS:
 278     FCS->SetDspPos(val);
 279     break;
 280   case FG_FLAPS_POS:
 281     FCS->SetDfPos(val);
 282     break;
 283   case FG_THROTTLE_POS:
 284     FCS->SetThrottlePos(ActiveEngine,val);
 285     break;
 286
 287   case FG_ELEVATOR_CMD:
 288     FCS->SetDeCmd(val);
 289     break;
 290   case FG_AILERON_CMD:
 291     FCS->SetDaCmd(val);
 292     break;
 293   case FG_RUDDER_CMD:
 294     FCS->SetDrCmd(val);
 295     break;
 296   case FG_SPDBRAKE_CMD:
 297     FCS->SetDsbCmd(val);
 298     break;
 299   case FG_SPOILERS_CMD:
 300     FCS->SetDspCmd(val);
 301     break;
 302   case FG_FLAPS_CMD:
 303     FCS->SetDfCmd(val);
 304     break;
 305   case FG_THROTTLE_CMD:
 306     FCS->SetThrottleCmd(ActiveEngine,val);
 307     break;
 308
 309   case FG_ACTIVE_ENGINE:
 310     ActiveEngine = (int)val;
 311     break;
 312
 313   case FG_LEFT_BRAKE_CMD:
 314     FCS->SetLBrake(val);
 315     break;
 316   case FG_CENTER_BRAKE_CMD:
 317     FCS->SetCBrake(val);
 318     break;
 319   case FG_RIGHT_BRAKE_CMD:
 320     FCS->SetRBrake(val);
 321     break;
 322
 323   case FG_SET_LOGGING:
 324     if      (val < -0.01) Output->Disable();
 325     else if (val >  0.01) Output->Enable();
 326     else                  Output->Toggle();
 327     break;
 328
 329   default:
 330     cerr << "Parameter '" << val_idx << "' (" << paramdef[val_idx] << ") not handled" << endl;
 331   }
 332 }
 333
 334 //***************************************************************************
 335 //
 336 // Reset: Assume all angles READ FROM FILE IN DEGREES !!
 337 //
 338
 339 bool FGState::Reset(string path, string acname, string fname) {
 340   string resetDef;
 341   float U, V, W;
 342   float phi, tht, psi;
 343   float latitude, longitude, h;
 344
 345   resetDef = path + "/" + acname + "/" + fname + ".xml";
 346
 347 #if defined ( sgi ) && !defined( __GNUC__ )
 348   ifstream resetfile(resetDef.c_str(), ios::in );
 349 #else
 350   ifstream resetfile(resetDef.c_str(), ios::in | ios::binary );
 351 #endif
 352
 353   if (resetfile) {
 354     resetfile >> U;
 355     resetfile >> V;
 356     resetfile >> W;
 357     resetfile >> latitude;
 358     resetfile >> longitude;
 359     resetfile >> phi;
 360     resetfile >> tht;
 361     resetfile >> psi;
 362     resetfile >> h;
 363     resetfile.close();
 364
 365     Position->SetLatitude(latitude*DEGTORAD);
 366     Position->SetLongitude(longitude*DEGTORAD);
 367     Position->Seth(h);
 368
 369     Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD,
 370                latitude*DEGTORAD, longitude*DEGTORAD, h);
 371
 372     return true;
 373   } else {
 374     cerr << "Unable to load reset file " << fname << endl;
 375     return false;
 376   }
 377 }
 378
 379 //***************************************************************************
 380 //
 381 // Initialize: Assume all angles GIVEN IN RADIANS !!
 382 //
 383
 384 void FGState::Initialize(float U, float V, float W,
 385                          float phi, float tht, float psi,
 386                          float Latitude, float Longitude, float H) {
 387   FGColumnVector vUVW(3);
 388   FGColumnVector vLocalVelNED(3);
 389   FGColumnVector vLocalEuler(3);
 390   float alpha, beta;
 391   float qbar, Vt;
 392
 393   Position->SetLatitude(Latitude);
 394   Position->SetLongitude(Longitude);
 395   Position->Seth(H);
 396
 397   Atmosphere->Run();
 398
 399   if (W != 0.0)
 400     alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
 401   else
 402     alpha = 0.0;
 403   if (V != 0.0)
 404     beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
 405   else
 406     beta = 0.0;
 407
 408   vUVW << U << V << W;
 409   Translation->SetUVW(vUVW);
 410
 411   vLocalEuler << phi << tht << psi;
 412   Rotation->SetEuler(vLocalEuler);
 413
 414   Translation->SetAB(alpha, beta);
 415
 416   Vt = sqrt(U*U + V*V + W*W);
 417   Translation->SetVt(Vt);
 418
 419   Translation->SetMach(Vt/Atmosphere->GetSoundSpeed());
 420
 421   qbar = 0.5*(U*U + V*V + W*W)*Atmosphere->GetDensity();
 422   Translation->Setqbar(qbar);
 423
 424   InitMatrices(phi, tht, psi);
 425
 426   vLocalVelNED = mTb2l*vUVW;
 427   Position->SetvVel(vLocalVelNED);
 428 }
 429
 430 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 431
 432 void FGState::Initialize(FGInitialCondition *FGIC) {
 433
 434   float tht,psi,phi;
 435   float U, V, W, h;
 436   float latitude, longitude;
 437
 438   latitude = FGIC->GetLatitudeRadIC();
 439   longitude = FGIC->GetLongitudeRadIC();
 440   h = FGIC->GetAltitudeFtIC();
 441   U = FGIC->GetUBodyFpsIC();
 442   V = FGIC->GetVBodyFpsIC();
 443   W = FGIC->GetWBodyFpsIC();
 444   tht = FGIC->GetThetaRadIC();
 445   phi = FGIC->GetPhiRadIC();
 446   psi = FGIC->GetPsiRadIC();
 447
 448   Initialize(U, V, W, phi, tht, psi, latitude, longitude, h);
 449
 450   Position->SetSeaLevelRadius( FGIC->GetSeaLevelRadiusFtIC() );
 451   Position->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() +
 452                                              FGIC->GetTerrainAltitudeFtIC() );
 453
 454 }
 455
 456 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 457
 458 bool FGState::StoreData(string fname) {
 459   ofstream datafile(fname.c_str());
 460
 461   if (datafile) {
 462     datafile << Translation->GetUVW(eU);
 463     datafile << Translation->GetUVW(eV);
 464     datafile << Translation->GetUVW(eW);
 465     datafile << Position->GetLatitude();
 466     datafile << Position->GetLongitude();
 467     datafile << Rotation->GetEuler(ePhi);
 468     datafile << Rotation->GetEuler(eTht);
 469     datafile << Rotation->GetEuler(ePsi);
 470     datafile << Position->Geth();
 471     datafile.close();
 472     return true;
 473   } else {
 474     cerr << "Could not open dump file " << fname << endl;
 475     return false;
 476   }
 477 }
 478
 479 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 480
 481 void FGState::InitMatrices(float phi, float tht, float psi) {
 482   float thtd2, psid2, phid2;
 483   float Sthtd2, Spsid2, Sphid2;
 484   float Cthtd2, Cpsid2, Cphid2;
 485   float Cphid2Cthtd2;
 486   float Cphid2Sthtd2;
 487   float Sphid2Sthtd2;
 488   float Sphid2Cthtd2;
 489
 490   thtd2 = tht/2.0;
 491   psid2 = psi/2.0;
 492   phid2 = phi/2.0;
 493
 494   Sthtd2 = sin(thtd2);
 495   Spsid2 = sin(psid2);
 496   Sphid2 = sin(phid2);
 497
 498   Cthtd2 = cos(thtd2);
 499   Cpsid2 = cos(psid2);
 500   Cphid2 = cos(phid2);
 501
 502   Cphid2Cthtd2 = Cphid2*Cthtd2;
 503   Cphid2Sthtd2 = Cphid2*Sthtd2;
 504   Sphid2Sthtd2 = Sphid2*Sthtd2;
 505   Sphid2Cthtd2 = Sphid2*Cthtd2;
 506
 507   vQtrn(1) = Cphid2Cthtd2*Cpsid2 + Sphid2Sthtd2*Spsid2;
 508   vQtrn(2) = Sphid2Cthtd2*Cpsid2 - Cphid2Sthtd2*Spsid2;
 509   vQtrn(3) = Cphid2Sthtd2*Cpsid2 + Sphid2Cthtd2*Spsid2;
 510   vQtrn(4) = Cphid2Cthtd2*Spsid2 - Sphid2Sthtd2*Cpsid2;
 511
 512   CalcMatrices();
 513 }
 514
 515 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 516
 517 void FGState::CalcMatrices(void) {
 518   float Q0Q0, Q1Q1, Q2Q2, Q3Q3;
 519   float Q0Q1, Q0Q2, Q0Q3, Q1Q2;
 520   float Q1Q3, Q2Q3;
 521
 522   Q0Q0 = vQtrn(1)*vQtrn(1);
 523   Q1Q1 = vQtrn(2)*vQtrn(2);
 524   Q2Q2 = vQtrn(3)*vQtrn(3);
 525   Q3Q3 = vQtrn(4)*vQtrn(4);
 526   Q0Q1 = vQtrn(1)*vQtrn(2);
 527   Q0Q2 = vQtrn(1)*vQtrn(3);
 528   Q0Q3 = vQtrn(1)*vQtrn(4);
 529   Q1Q2 = vQtrn(2)*vQtrn(3);
 530   Q1Q3 = vQtrn(2)*vQtrn(4);
 531   Q2Q3 = vQtrn(3)*vQtrn(4);
 532
 533   mTl2b(1,1) = Q0Q0 + Q1Q1 - Q2Q2 - Q3Q3;
 534   mTl2b(1,2) = 2*(Q1Q2 + Q0Q3);
 535   mTl2b(1,3) = 2*(Q1Q3 - Q0Q2);
 536   mTl2b(2,1) = 2*(Q1Q2 - Q0Q3);
 537   mTl2b(2,2) = Q0Q0 - Q1Q1 + Q2Q2 - Q3Q3;
 538   mTl2b(2,3) = 2*(Q2Q3 + Q0Q1);
 539   mTl2b(3,1) = 2*(Q1Q3 + Q0Q2);
 540   mTl2b(3,2) = 2*(Q2Q3 - Q0Q1);
 541   mTl2b(3,3) = Q0Q0 - Q1Q1 - Q2Q2 + Q3Q3;
 542
 543   mTb2l = mTl2b;
 544   mTb2l.T();
 545 }
 546
 547 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 548
 549 void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) {
 550   vQdot(1) = -0.5*(vQtrn(2)*vPQR(eP) + vQtrn(3)*vPQR(eQ) + vQtrn(4)*vPQR(eR));
 551   vQdot(2) =  0.5*(vQtrn(1)*vPQR(eP) + vQtrn(3)*vPQR(eR) - vQtrn(4)*vPQR(eQ));
 552   vQdot(3) =  0.5*(vQtrn(1)*vPQR(eQ) + vQtrn(4)*vPQR(eP) - vQtrn(2)*vPQR(eR));
 553   vQdot(4) =  0.5*(vQtrn(1)*vPQR(eR) + vQtrn(2)*vPQR(eQ) - vQtrn(3)*vPQR(eP));
 554   vQtrn += 0.5*dt*rate*(vlastQdot + vQdot);
 555
 556   vQtrn.Normalize();
 557
 558   vlastQdot = vQdot;
 559 }
 560
 561 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 562
 563 FGColumnVector FGState::CalcEuler(void) {
 564   if (mTl2b(3,3) == 0.0) mTl2b(3,3) = 0.0000001;
 565   if (mTl2b(1,1) == 0.0) mTl2b(1,1) = 0.0000001;
 566
 567   vEuler(ePhi) = atan2(mTl2b(2,3), mTl2b(3,3));
 568   vEuler(eTht) = asin(-mTl2b(1,3));
 569   vEuler(ePsi) = atan2(mTl2b(1,2), mTl2b(1,1));
 570
 571   if (vEuler(ePsi) < 0.0) vEuler(ePsi) += 2*M_PI;
 572
 573   return vEuler;
 574 }
 575
 576 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 577
 578 FGMatrix FGState::GetTs2b(float alpha, float beta) {
 579   float ca, cb, sa, sb;
 580
 581   ca = cos(alpha);
 582   sa = sin(alpha);
 583   cb = cos(beta);
 584   sb = sin(beta);
 585
 586   mTs2b(1,1) = -ca*cb;
 587   mTs2b(1,2) = -ca*sb;
 588   mTs2b(1,3) = sa;
 589   mTs2b(2,1) = -sb;
 590   mTs2b(2,2) = cb;
 591   mTs2b(2,3) = 0.0;
 592   mTs2b(3,1) = -sa*cb;
 593   mTs2b(3,2) = -sa*sb;
 594   mTs2b(3,3) = -ca;
 595
 596   return mTs2b;
 597 }
 598
 599 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 600
 601 void FGState::Debug(void)
 602 {
 603     //TODO: Add your source code here
 604 }
 605