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 HAVE_CONFIG_H
  40 #  include <config.h>
  41 #endif
  42
  43 #ifdef FGFS
  44 #  include <simgear/compiler.h>
  45 #  include <math.h>
  46 #else
  47 #  if defined(sgi) && !defined(__GNUC__)
  48 #    include <math.h>
  49 #  else
  50 #    include <cmath>
  51 #  endif
  52 #endif
  53
  54 #ifdef _WIN32
  55 //#define snprintf _snprintf
  56 #endif
  57
  58 #include "FGState.h"
  59
  60 namespace JSBSim {
  61
  62 static const char *IdSrc = "$Id$";
  63 static const char *IdHdr = ID_STATE;
  64
  65 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  66 MACROS
  67 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  68
  69 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  70 CLASS IMPLEMENTATION
  71 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  72
  73 FGState::FGState(FGFDMExec* fdex)
  74 {
  75   FDMExec = fdex;
  76
  77   sim_time = 0.0;
  78   dt = 1.0/120.0;
  79
  80   Aircraft     = FDMExec->GetAircraft();
  81   Propagate    = FDMExec->GetPropagate();
  82   Auxiliary    = FDMExec->GetAuxiliary();
  83   FCS          = FDMExec->GetFCS();
  84   Output       = FDMExec->GetOutput();
  85   Atmosphere   = FDMExec->GetAtmosphere();
  86   Aerodynamics = FDMExec->GetAerodynamics();
  87   GroundReactions = FDMExec->GetGroundReactions();
  88   Propulsion      = FDMExec->GetPropulsion();
  89   PropertyManager = FDMExec->GetPropertyManager();
  90
  91   bind();
  92
  93   Debug(0);
  94 }
  95
  96 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  97
  98 FGState::~FGState()
  99 {
 100   unbind();
 101   Debug(1);
 102 }
 103
 104 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 105
 106 void FGState::Initialize(FGInitialCondition *FGIC)
 107 {
 108   sim_time = 0.0;
 109
 110   Propagate->SetInitialState( FGIC );
 111
 112   Atmosphere->Run();
 113   Atmosphere->SetWindNED( FGIC->GetWindNFpsIC(),
 114                           FGIC->GetWindEFpsIC(),
 115                           FGIC->GetWindDFpsIC() );
 116
 117   FGColumnVector3 vAeroUVW;
 118   vAeroUVW = Propagate->GetUVW() + Propagate->GetTl2b()*Atmosphere->GetWindNED();
 119
 120   double alpha, beta;
 121   if (vAeroUVW(eW) != 0.0)
 122     alpha = vAeroUVW(eU)*vAeroUVW(eU) > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0;
 123   else
 124     alpha = 0.0;
 125   if (vAeroUVW(eV) != 0.0)
 126     beta = vAeroUVW(eU)*vAeroUVW(eU)+vAeroUVW(eW)*vAeroUVW(eW) > 0.0 ? atan2(vAeroUVW(eV), (fabs(vAeroUVW(eU))/vAeroUVW(eU))*sqrt(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW))) : 0.0;
 127   else
 128     beta = 0.0;
 129
 130   Auxiliary->SetAB(alpha, beta);
 131
 132   double Vt = vAeroUVW.Magnitude();
 133   Auxiliary->SetVt(Vt);
 134
 135   Auxiliary->SetMach(Vt/Atmosphere->GetSoundSpeed());
 136
 137   double qbar = 0.5*Vt*Vt*Atmosphere->GetDensity();
 138   Auxiliary->Setqbar(qbar);
 139 }
 140
 141 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 142
 143 FGMatrix33& FGState::GetTs2b(void)
 144 {
 145   double ca, cb, sa, sb;
 146
 147   double alpha = Auxiliary->Getalpha();
 148   double beta  = Auxiliary->Getbeta();
 149
 150   ca = cos(alpha);
 151   sa = sin(alpha);
 152   cb = cos(beta);
 153   sb = sin(beta);
 154
 155   mTs2b(1,1) = ca*cb;
 156   mTs2b(1,2) = -ca*sb;
 157   mTs2b(1,3) = -sa;
 158   mTs2b(2,1) = sb;
 159   mTs2b(2,2) = cb;
 160   mTs2b(2,3) = 0.0;
 161   mTs2b(3,1) = sa*cb;
 162   mTs2b(3,2) = -sa*sb;
 163   mTs2b(3,3) = ca;
 164
 165   return mTs2b;
 166 }
 167
 168 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 169
 170 FGMatrix33& FGState::GetTb2s(void)
 171 {
 172   double alpha,beta;
 173   double ca, cb, sa, sb;
 174
 175   alpha = Auxiliary->Getalpha();
 176   beta  = Auxiliary->Getbeta();
 177
 178   ca = cos(alpha);
 179   sa = sin(alpha);
 180   cb = cos(beta);
 181   sb = sin(beta);
 182
 183   mTb2s(1,1) = ca*cb;
 184   mTb2s(1,2) = sb;
 185   mTb2s(1,3) = sa*cb;
 186   mTb2s(2,1) = -ca*sb;
 187   mTb2s(2,2) = cb;
 188   mTb2s(2,3) = -sa*sb;
 189   mTb2s(3,1) = -sa;
 190   mTb2s(3,2) = 0.0;
 191   mTb2s(3,3) = ca;
 192
 193   return mTb2s;
 194 }
 195
 196 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 197
 198 void FGState::ReportState(void)
 199 {
 200 #if 0
 201 #if !defined(__BORLANDCPP__)
 202   char out[80], flap[10], gear[12];
 203
 204   cout << endl << "  JSBSim State" << endl;
 205   snprintf(out,80,"    Weight: %7.0f lbs.  CG: %5.1f, %5.1f, %5.1f inches\n",
 206                    FDMExec->GetMassBalance()->GetWeight(),
 207                    FDMExec->GetMassBalance()->GetXYZcg(1),
 208                    FDMExec->GetMassBalance()->GetXYZcg(2),
 209                    FDMExec->GetMassBalance()->GetXYZcg(3));
 210   cout << out;
 211   if ( FCS->GetDfPos() <= 0.01)
 212     snprintf(flap,10,"Up");
 213   else
 214     snprintf(flap,10,"%2.0f",FCS->GetDfPos());
 215
 216   if (FCS->GetGearPos() < 0.01)
 217     snprintf(gear,12,"Up");
 218   else if (FCS->GetGearPos() > 0.99)
 219     snprintf(gear,12,"Down");
 220   else
 221     snprintf(gear,12,"In Transit");
 222
 223   snprintf(out,80, "    Flaps: %3s  Gear: %12s\n",flap,gear);
 224   cout << out;
 225   snprintf(out,80, "    Speed: %4.0f KCAS  Mach: %5.2f\n",
 226                     Auxiliary->GetVcalibratedKTS(),
 227                     Auxiliary->GetMach() );
 228   cout << out;
 229   snprintf(out,80, "    Altitude: %7.0f ft.  AGL Altitude: %7.0f ft.\n",
 230                     Propagate->Geth(),
 231                     Propagate->GetDistanceAGL() );
 232   cout << out;
 233   snprintf(out,80, "    Angle of Attack: %6.2f deg  Pitch Angle: %6.2f deg\n",
 234                     Auxiliary->Getalpha()*radtodeg,
 235                     Propagate->Gettht()*radtodeg );
 236   cout << out;
 237   snprintf(out,80, "    Flight Path Angle: %6.2f deg  Climb Rate: %5.0f ft/min\n",
 238                     Auxiliary->GetGamma()*radtodeg,
 239                     Propagate->Gethdot()*60 );
 240   cout << out;
 241   snprintf(out,80, "    Normal Load Factor: %4.2f g's  Pitch Rate: %5.2f deg/s\n",
 242                     Aircraft->GetNlf(),
 243                     Propagate->GetPQR(2)*radtodeg );
 244   cout << out;
 245   snprintf(out,80, "    Heading: %3.0f deg true  Sideslip: %5.2f deg  Yaw Rate: %5.2f deg/s\n",
 246                     Propagate->Getpsi()*radtodeg,
 247                     Auxiliary->Getbeta()*radtodeg,
 248                     Propagate->GetPQR(3)*radtodeg  );
 249   cout << out;
 250   snprintf(out,80, "    Bank Angle: %5.2f deg  Roll Rate: %5.2f deg/s\n",
 251                     Propagate->Getphi()*radtodeg,
 252                     Propagate->GetPQR(1)*radtodeg );
 253   cout << out;
 254   snprintf(out,80, "    Elevator: %5.2f deg  Left Aileron: %5.2f deg  Rudder: %5.2f deg\n",
 255                     FCS->GetDePos(ofRad)*radtodeg,
 256                     FCS->GetDaLPos(ofRad)*radtodeg,
 257                     FCS->GetDrPos(ofRad)*radtodeg );
 258   cout << out;
 259   snprintf(out,80, "    Throttle: %5.2f%c\n",
 260                     FCS->GetThrottlePos(0)*100,'%' );
 261   cout << out;
 262
 263   snprintf(out,80, "    Wind Components: %5.2f kts head wind, %5.2f kts cross wind\n",
 264                     Auxiliary->GetHeadWind()*fpstokts,
 265                     Auxiliary->GetCrossWind()*fpstokts );
 266   cout << out;
 267
 268   snprintf(out,80, "    Ground Speed: %4.0f knots , Ground Track: %3.0f deg true\n",
 269                     Auxiliary->GetVground()*fpstokts,
 270                     Auxiliary->GetGroundTrack()*radtodeg );
 271   cout << out;
 272 #endif
 273 #endif
 274 }
 275
 276 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 277
 278 void FGState::bind(void)
 279 {
 280   PropertyManager->Tie("sim-time-sec",this,
 281                         &FGState::Getsim_time);
 282 }
 283
 284 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 285
 286 void FGState::unbind(void)
 287 {
 288   PropertyManager->Untie("sim-time-sec");
 289 }
 290
 291 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 292 //    The bitmasked value choices are as follows:
 293 //    unset: In this case (the default) JSBSim would only print
 294 //       out the normally expected messages, essentially echoing
 295 //       the config files as they are read. If the environment
 296 //       variable is not set, debug_lvl is set to 1 internally
 297 //    0: This requests JSBSim not to output any messages
 298 //       whatsoever.
 299 //    1: This value explicity requests the normal JSBSim
 300 //       startup messages
 301 //    2: This value asks for a message to be printed out when
 302 //       a class is instantiated
 303 //    4: When this value is set, a message is displayed when a
 304 //       FGModel object executes its Run() method
 305 //    8: When this value is set, various runtime state variables
 306 //       are printed out periodically
 307 //    16: When set various parameters are sanity checked and
 308 //       a message is printed out when they go out of bounds
 309
 310 void FGState::Debug(int from)
 311 {
 312   if (debug_lvl <= 0) return;
 313
 314   if (debug_lvl & 1) { // Standard console startup message output
 315     if (from == 0) { // Constructor
 316
 317     }
 318   }
 319   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 320     if (from == 0) cout << "Instantiated: FGState" << endl;
 321     if (from == 1) cout << "Destroyed:    FGState" << endl;
 322   }
 323   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 324   }
 325   if (debug_lvl & 8 ) { // Runtime state variables
 326   }
 327   if (debug_lvl & 16) { // Sanity checking
 328   }
 329   if (debug_lvl & 64) {
 330     if (from == 0) { // Constructor
 331       cout << IdSrc << endl;
 332       cout << IdHdr << endl;
 333     }
 334   }
 335 }
 336 }