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