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   ifstream resetfile(resetDef.c_str(), ios::in | ios::binary );
 348
 349   if (resetfile) {
 350     resetfile >> U;
 351     resetfile >> V;
 352     resetfile >> W;
 353     resetfile >> latitude;
 354     resetfile >> longitude;
 355     resetfile >> phi;
 356     resetfile >> tht;
 357     resetfile >> psi;
 358     resetfile >> h;
 359     resetfile.close();
 360
 361     Position->SetLatitude(latitude*DEGTORAD);
 362     Position->SetLongitude(longitude*DEGTORAD);
 363     Position->Seth(h);
 364
 365     Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD,
 366                latitude*DEGTORAD, longitude*DEGTORAD, h);
 367
 368     return true;
 369   } else {
 370     cerr << "Unable to load reset file " << fname << endl;
 371     return false;
 372   }
 373 }
 374
 375 //***************************************************************************
 376 //
 377 // Initialize: Assume all angles GIVEN IN RADIANS !!
 378 //
 379
 380 void FGState::Initialize(float U, float V, float W,
 381                          float phi, float tht, float psi,
 382                          float Latitude, float Longitude, float H) {
 383   FGColumnVector vUVW(3);
 384   FGColumnVector vLocalVelNED(3);
 385   FGColumnVector vLocalEuler(3);
 386   float alpha, beta;
 387   float qbar, Vt;
 388
 389   Position->SetLatitude(Latitude);
 390   Position->SetLongitude(Longitude);
 391   Position->Seth(H);
 392
 393   Atmosphere->Run();
 394
 395   if (W != 0.0)
 396     alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
 397   else
 398     alpha = 0.0;
 399   if (V != 0.0)
 400     beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
 401   else
 402     beta = 0.0;
 403
 404   vUVW << U << V << W;
 405   Translation->SetUVW(vUVW);
 406
 407   vLocalEuler << phi << tht << psi;
 408   Rotation->SetEuler(vLocalEuler);
 409
 410   Translation->SetAB(alpha, beta);
 411
 412   Vt = sqrt(U*U + V*V + W*W);
 413   Translation->SetVt(Vt);
 414
 415   Translation->SetMach(Vt/Atmosphere->GetSoundSpeed());
 416
 417   qbar = 0.5*(U*U + V*V + W*W)*Atmosphere->GetDensity();
 418   Translation->Setqbar(qbar);
 419
 420   InitMatrices(phi, tht, psi);
 421
 422   vLocalVelNED = mTb2l*vUVW;
 423   Position->SetvVel(vLocalVelNED);
 424 }
 425
 426 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 427
 428 void FGState::Initialize(FGInitialCondition *FGIC) {
 429
 430   float tht,psi,phi;
 431   float U, V, W, h;
 432   float latitude, longitude;
 433
 434   latitude = FGIC->GetLatitudeRadIC();
 435   longitude = FGIC->GetLongitudeRadIC();
 436   h = FGIC->GetAltitudeFtIC();
 437   U = FGIC->GetUBodyFpsIC();
 438   V = FGIC->GetVBodyFpsIC();
 439   W = FGIC->GetWBodyFpsIC();
 440   tht = FGIC->GetThetaRadIC();
 441   phi = FGIC->GetPhiRadIC();
 442   psi = FGIC->GetPsiRadIC();
 443
 444   Initialize(U, V, W, phi, tht, psi, latitude, longitude, h);
 445
 446   Position->SetSeaLevelRadius( FGIC->GetSeaLevelRadiusFtIC() );
 447   Position->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() +
 448                                              FGIC->GetTerrainAltitudeFtIC() );
 449
 450 }
 451
 452 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 453
 454 bool FGState::StoreData(string fname) {
 455   ofstream datafile(fname.c_str());
 456
 457   if (datafile) {
 458     datafile << Translation->GetUVW(eU);
 459     datafile << Translation->GetUVW(eV);
 460     datafile << Translation->GetUVW(eW);
 461     datafile << Position->GetLatitude();
 462     datafile << Position->GetLongitude();
 463     datafile << Rotation->GetEuler(ePhi);
 464     datafile << Rotation->GetEuler(eTht);
 465     datafile << Rotation->GetEuler(ePsi);
 466     datafile << Position->Geth();
 467     datafile.close();
 468     return true;
 469   } else {
 470     cerr << "Could not open dump file " << fname << endl;
 471     return false;
 472   }
 473 }
 474
 475 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 476
 477 void FGState::InitMatrices(float phi, float tht, float psi) {
 478   float thtd2, psid2, phid2;
 479   float Sthtd2, Spsid2, Sphid2;
 480   float Cthtd2, Cpsid2, Cphid2;
 481   float Cphid2Cthtd2;
 482   float Cphid2Sthtd2;
 483   float Sphid2Sthtd2;
 484   float Sphid2Cthtd2;
 485
 486   thtd2 = tht/2.0;
 487   psid2 = psi/2.0;
 488   phid2 = phi/2.0;
 489
 490   Sthtd2 = sin(thtd2);
 491   Spsid2 = sin(psid2);
 492   Sphid2 = sin(phid2);
 493
 494   Cthtd2 = cos(thtd2);
 495   Cpsid2 = cos(psid2);
 496   Cphid2 = cos(phid2);
 497
 498   Cphid2Cthtd2 = Cphid2*Cthtd2;
 499   Cphid2Sthtd2 = Cphid2*Sthtd2;
 500   Sphid2Sthtd2 = Sphid2*Sthtd2;
 501   Sphid2Cthtd2 = Sphid2*Cthtd2;
 502
 503   vQtrn(1) = Cphid2Cthtd2*Cpsid2 + Sphid2Sthtd2*Spsid2;
 504   vQtrn(2) = Sphid2Cthtd2*Cpsid2 - Cphid2Sthtd2*Spsid2;
 505   vQtrn(3) = Cphid2Sthtd2*Cpsid2 + Sphid2Cthtd2*Spsid2;
 506   vQtrn(4) = Cphid2Cthtd2*Spsid2 - Sphid2Sthtd2*Cpsid2;
 507
 508   CalcMatrices();
 509 }
 510
 511 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 512
 513 void FGState::CalcMatrices(void) {
 514   float Q0Q0, Q1Q1, Q2Q2, Q3Q3;
 515   float Q0Q1, Q0Q2, Q0Q3, Q1Q2;
 516   float Q1Q3, Q2Q3;
 517
 518   Q0Q0 = vQtrn(1)*vQtrn(1);
 519   Q1Q1 = vQtrn(2)*vQtrn(2);
 520   Q2Q2 = vQtrn(3)*vQtrn(3);
 521   Q3Q3 = vQtrn(4)*vQtrn(4);
 522   Q0Q1 = vQtrn(1)*vQtrn(2);
 523   Q0Q2 = vQtrn(1)*vQtrn(3);
 524   Q0Q3 = vQtrn(1)*vQtrn(4);
 525   Q1Q2 = vQtrn(2)*vQtrn(3);
 526   Q1Q3 = vQtrn(2)*vQtrn(4);
 527   Q2Q3 = vQtrn(3)*vQtrn(4);
 528
 529   mTl2b(1,1) = Q0Q0 + Q1Q1 - Q2Q2 - Q3Q3;
 530   mTl2b(1,2) = 2*(Q1Q2 + Q0Q3);
 531   mTl2b(1,3) = 2*(Q1Q3 - Q0Q2);
 532   mTl2b(2,1) = 2*(Q1Q2 - Q0Q3);
 533   mTl2b(2,2) = Q0Q0 - Q1Q1 + Q2Q2 - Q3Q3;
 534   mTl2b(2,3) = 2*(Q2Q3 + Q0Q1);
 535   mTl2b(3,1) = 2*(Q1Q3 + Q0Q2);
 536   mTl2b(3,2) = 2*(Q2Q3 - Q0Q1);
 537   mTl2b(3,3) = Q0Q0 - Q1Q1 - Q2Q2 + Q3Q3;
 538
 539   mTb2l = mTl2b;
 540   mTb2l.T();
 541 }
 542
 543 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 544
 545 void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) {
 546   vQdot(1) = -0.5*(vQtrn(2)*vPQR(eP) + vQtrn(3)*vPQR(eQ) + vQtrn(4)*vPQR(eR));
 547   vQdot(2) =  0.5*(vQtrn(1)*vPQR(eP) + vQtrn(3)*vPQR(eR) - vQtrn(4)*vPQR(eQ));
 548   vQdot(3) =  0.5*(vQtrn(1)*vPQR(eQ) + vQtrn(4)*vPQR(eP) - vQtrn(2)*vPQR(eR));
 549   vQdot(4) =  0.5*(vQtrn(1)*vPQR(eR) + vQtrn(2)*vPQR(eQ) - vQtrn(3)*vPQR(eP));
 550   vQtrn += 0.5*dt*rate*(vlastQdot + vQdot);
 551
 552   vQtrn.Normalize();
 553
 554   vlastQdot = vQdot;
 555 }
 556
 557 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 558
 559 FGColumnVector FGState::CalcEuler(void) {
 560   if (mTl2b(3,3) == 0.0) mTl2b(3,3) = 0.0000001;
 561   if (mTl2b(1,1) == 0.0) mTl2b(1,1) = 0.0000001;
 562
 563   vEuler(ePhi) = atan2(mTl2b(2,3), mTl2b(3,3));
 564   vEuler(eTht) = asin(-mTl2b(1,3));
 565   vEuler(ePsi) = atan2(mTl2b(1,2), mTl2b(1,1));
 566
 567   if (vEuler(ePsi) < 0.0) vEuler(ePsi) += 2*M_PI;
 568
 569   return vEuler;
 570 }
 571
 572 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 573
 574 FGMatrix FGState::GetTs2b(float alpha, float beta) {
 575   float ca, cb, sa, sb;
 576
 577   ca = cos(alpha);
 578   sa = sin(alpha);
 579   cb = cos(beta);
 580   sb = sin(beta);
 581
 582   mTs2b(1,1) = -ca*cb;
 583   mTs2b(1,2) = -ca*sb;
 584   mTs2b(1,3) = sa;
 585   mTs2b(2,1) = -sb;
 586   mTs2b(2,2) = cb;
 587   mTs2b(2,3) = 0.0;
 588   mTs2b(3,1) = -sa*cb;
 589   mTs2b(3,2) = -sa*sb;
 590   mTs2b(3,3) = -ca;
 591
 592   return mTs2b;
 593 }
 594
 595 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 596
 597 void FGState::Debug(void)
 598 {
 599     //TODO: Add your source code here
 600 }
 601