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