]> git.mxchange.org Git - flightgear.git/blobdiff - src/FDM/JSBSim/FGState.cpp
Latest JSBSim changes, including some gear tweaking from Jon and some
[flightgear.git] / src / FDM / JSBSim / FGState.cpp
index 84ef65274ba14d05e00efb72c5a6e474b6cfb9e9..2dc11a08e6508d1623fc093245484a0a2c31cda5 100644 (file)
@@ -47,6 +47,10 @@ INCLUDES
 #  endif
 #endif
 
+#ifdef _MSC_VER
+#define snprintf _snprintf
+#endif
+
 #include "FGState.h"
 
 static const char *IdSrc = "$Id$";
@@ -66,20 +70,10 @@ CLASS IMPLEMENTATION
 //
 // For every term registered here there must be a corresponding handler in
 // GetParameter() below that retrieves that parameter. Also, there must be an
-// entry in the enum eParam definition in FGDefs.h. The ID is what must be used
+// entry in the enum eParam definition in FGJSBBase.h. The ID is what must be used
 // in any config file entry which references that item.
 
-FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
-    mTl2b(3,3),
-    mTs2b(3,3),
-    vQtrn(4),
-    vlastQdot(4),
-    vQdot(4),
-    vTmp(4),
-    vEuler(3),
-    vUVW(3),
-    vLocalVelNED(3),
-    vLocalEuler(3)
+FGState::FGState(FGFDMExec* fdex)
 {
   FDMExec = fdex;
 
@@ -107,6 +101,7 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
   RegisterVariable(FG_ALPHA,          " alpha "          );
   RegisterVariable(FG_ALPHADOT,       " alphadot "       );
   RegisterVariable(FG_BETA,           " beta "           );
+  RegisterVariable(FG_ABETA,          " |beta| "         );
   RegisterVariable(FG_BETADOT,        " betadot "        );
   RegisterVariable(FG_PHI,            " roll_angle "     );
   RegisterVariable(FG_THT,            " pitch_angle "    );
@@ -114,6 +109,9 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
   RegisterVariable(FG_PITCHRATE,      " pitch_rate "     );
   RegisterVariable(FG_ROLLRATE,       " roll_rate "      );
   RegisterVariable(FG_YAWRATE,        " yaw_rate "       );
+  RegisterVariable(FG_AEROQ,          " aero_pitch_rate ");
+  RegisterVariable(FG_AEROP,          " aero_roll_rate " );
+  RegisterVariable(FG_AEROR,          " aero_yaw_rate "  );
   RegisterVariable(FG_CL_SQRD,        " Clift_sqrd "     );
   RegisterVariable(FG_MACH,           " mach "           );
   RegisterVariable(FG_ALTITUDE,       " altitude "       );
@@ -125,6 +123,7 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
   RegisterVariable(FG_SPDBRAKE_POS,   " speedbrake_pos " );
   RegisterVariable(FG_SPOILERS_POS,   " spoiler_pos "    );
   RegisterVariable(FG_FLAPS_POS,      " flaps_pos "      );
+  RegisterVariable(FG_GEAR_POS,       " gear_pos "       );
   RegisterVariable(FG_ELEVATOR_CMD,   " elevator_cmd "   );
   RegisterVariable(FG_AILERON_CMD,    " aileron_cmd "    );
   RegisterVariable(FG_RUDDER_CMD,     " rudder_cmd "     );
@@ -132,14 +131,17 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
   RegisterVariable(FG_SPOILERS_CMD,   " spoiler_cmd "    );
   RegisterVariable(FG_FLAPS_CMD,      " flaps_cmd "      );
   RegisterVariable(FG_THROTTLE_CMD,   " throttle_cmd "   );
+  RegisterVariable(FG_GEAR_CMD,       " gear_cmd "       );
   RegisterVariable(FG_THROTTLE_POS,   " throttle_pos "   );
-  RegisterVariable(FG_MIXTURE_CMD,    " mixture_cmd "   );
-  RegisterVariable(FG_MIXTURE_POS,    " mixture_pos "   );
-  RegisterVariable(FG_MAGNETO_CMD,    " magneto_cmd "   );
-  RegisterVariable(FG_STARTER_CMD,    " starter_cmd "   );
+  RegisterVariable(FG_MIXTURE_CMD,    " mixture_cmd "    );
+  RegisterVariable(FG_MIXTURE_POS,    " mixture_pos "    );
+  RegisterVariable(FG_MAGNETO_CMD,    " magneto_cmd "    );
+  RegisterVariable(FG_STARTER_CMD,    " starter_cmd "    );
   RegisterVariable(FG_ACTIVE_ENGINE,  " active_engine "  );
   RegisterVariable(FG_HOVERB,         " height/span "    );
   RegisterVariable(FG_PITCH_TRIM_CMD, " pitch_trim_cmd " );
+  RegisterVariable(FG_YAW_TRIM_CMD,   " yaw_trim_cmd " );
+  RegisterVariable(FG_ROLL_TRIM_CMD,  " roll_trim_cmd " );
   RegisterVariable(FG_LEFT_BRAKE_CMD, " left_brake_cmd " );
   RegisterVariable(FG_RIGHT_BRAKE_CMD," right_brake_cmd ");
   RegisterVariable(FG_CENTER_BRAKE_CMD," center_brake_cmd ");
@@ -153,20 +155,20 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
   RegisterVariable(FG_VBARV,           " v-tail volume " );
   RegisterVariable(FG_SET_LOGGING,    " data_logging "   );
 
-  if (debug_lvl & 2) cout << "Instantiated: FGState" << endl;
+  Debug(0);
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 FGState::~FGState()
 {
-  if (debug_lvl & 2) cout << "Destroyed:    FGState" << endl;
+  Debug(1);
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-float FGState::GetParameter(eParam val_idx) {
-  float scratch;
+double FGState::GetParameter(eParam val_idx) {
+  double scratch;
   
   switch(val_idx) {
   case FG_TIME:
@@ -199,6 +201,8 @@ float FGState::GetParameter(eParam val_idx) {
     return Translation->Getadot();
   case FG_BETA:
     return Translation->Getbeta();
+  case FG_ABETA:
+    return fabs(Translation->Getbeta());   
   case FG_BETADOT:
     return Translation->Getbdot();
   case FG_PHI:
@@ -213,6 +217,12 @@ float FGState::GetParameter(eParam val_idx) {
     return Rotation->GetPQR(eP);
   case FG_YAWRATE:
     return Rotation->GetPQR(eR);
+  case FG_AEROP:
+    return Rotation->GetAeroPQR(eP);
+  case FG_AEROQ:
+    return Rotation->GetAeroPQR(eQ);
+  case FG_AEROR:
+    return Rotation->GetAeroPQR(eR);
   case FG_CL_SQRD:
     if (Translation->Getqbar() > 0.00)
       scratch = Aerodynamics->GetvLastFs(eLift)/(Aircraft->GetWingArea()*Translation->Getqbar());
@@ -281,9 +291,17 @@ float FGState::GetParameter(eParam val_idx) {
     if (ActiveEngine < 0) return FCS->GetMixturePos(0);
     else return FCS->GetMixturePos(ActiveEngine);
   case FG_HOVERB:
-    return Position->GetHOverB();
+    return Position->GetHOverBMAC();
   case FG_PITCH_TRIM_CMD:
     return FCS->GetPitchTrimCmd();
+  case FG_YAW_TRIM_CMD:
+    return FCS->GetYawTrimCmd();
+  case FG_ROLL_TRIM_CMD:
+    return FCS->GetRollTrimCmd();
+  case FG_GEAR_CMD:
+    return FCS->GetGearCmd();
+  case FG_GEAR_POS:
+    return FCS->GetGearPos();    
   default:
     cerr << "FGState::GetParameter() - No handler for parameter " << paramdef[val_idx] << endl;
     return 0.0;
@@ -293,19 +311,23 @@ float FGState::GetParameter(eParam val_idx) {
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-float FGState::GetParameter(string val_string) {
+double FGState::GetParameter(string val_string) {
   return GetParameter(coeffdef[val_string]);
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-eParam FGState::GetParameterIndex(string val_string) {
+eParam FGState::GetParameterIndex(string val_string)
+{
   return coeffdef[val_string];
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-void FGState::SetParameter(eParam val_idx, float val) {
+void FGState::SetParameter(eParam val_idx, double val)
+{
+  unsigned i;
+
   switch(val_idx) {
   case FG_ELEVATOR_POS:
     FCS->SetDePos(val);
@@ -326,10 +348,22 @@ void FGState::SetParameter(eParam val_idx, float val) {
     FCS->SetDfPos(val);
     break;
   case FG_THROTTLE_POS:
-    FCS->SetThrottlePos(ActiveEngine,val);
+    if (ActiveEngine == -1) {
+      for (i=0; i<Propulsion->GetNumEngines(); i++) {
+        FCS->SetThrottlePos(i,val);
+           }
+         } else {
+      FCS->SetThrottlePos(ActiveEngine,val);
+         }
     break;
   case FG_MIXTURE_POS:
-    FCS->SetMixturePos(ActiveEngine,val);
+    if (ActiveEngine == -1) {
+      for (i=0; i<Propulsion->GetNumEngines(); i++) {
+        FCS->SetMixturePos(i,val);
+           }
+         } else {
+      FCS->SetMixturePos(ActiveEngine,val);
+         }
     break;
 
   case FG_ELEVATOR_CMD:
@@ -351,19 +385,43 @@ void FGState::SetParameter(eParam val_idx, float val) {
     FCS->SetDfCmd(val);
     break;
   case FG_THROTTLE_CMD:
-    FCS->SetThrottleCmd(ActiveEngine,val);
+    if (ActiveEngine == -1) {
+      for (i=0; i<Propulsion->GetNumEngines(); i++) {
+        FCS->SetThrottleCmd(i,val);
+           }
+         } else {
+      FCS->SetThrottleCmd(ActiveEngine,val);
+         }
     break;
   case FG_MIXTURE_CMD:
-    FCS->SetMixtureCmd(ActiveEngine,val);
+    if (ActiveEngine == -1) {
+      for (i=0; i<Propulsion->GetNumEngines(); i++) {
+        FCS->SetMixtureCmd(i,val);
+           }
+         } else {
+      FCS->SetMixtureCmd(ActiveEngine,val);
+         }
     break;
   case FG_MAGNETO_CMD:
-    Propulsion->GetEngine(ActiveEngine)->SetMagnetos(val); // need to account for -1
+    if (ActiveEngine == -1) {
+      for (i=0; i<Propulsion->GetNumEngines(); i++) {
+        Propulsion->GetEngine(i)->SetMagnetos((int)val);
+      }
+    } else {
+      Propulsion->GetEngine(ActiveEngine)->SetMagnetos((int)val);
+    }
     break;
   case FG_STARTER_CMD:
-    if      (val < 0.001) 
-      Propulsion->GetEngine(ActiveEngine)->SetStarter(false); // need to account for -1
-    else if (val >=  0.001)
-      Propulsion->GetEngine(ActiveEngine)->SetStarter(true); // need to account for -1
+    if (ActiveEngine == -1) {
+      for (i=0; i<Propulsion->GetNumEngines(); i++) {
+        if (val < 0.001) 
+          Propulsion->GetEngine(i)->SetStarter(false);
+        else if (val >=  0.001)
+          Propulsion->GetEngine(i)->SetStarter(true);
+      }
+    } else {
+      Propulsion->GetEngine(ActiveEngine)->SetStarter(true);
+    }
     break;
   case FG_ACTIVE_ENGINE:
     ActiveEngine = (int)val;
@@ -378,7 +436,12 @@ void FGState::SetParameter(eParam val_idx, float val) {
   case FG_RIGHT_BRAKE_CMD:
     FCS->SetRBrake(val);
     break;
-
+  case FG_GEAR_CMD:
+    FCS->SetGearCmd(val);
+    break;
+  case  FG_GEAR_POS:
+    FCS->SetGearPos(val);
+    break; 
   case FG_SET_LOGGING:
     if      (val < -0.01) Output->Disable();
     else if (val >  0.01) Output->Enable();
@@ -400,10 +463,10 @@ bool FGState::Reset(string path, string acname, string fname)
   string resetDef;
   string token="";
 
-  float U, V, W;
-  float phi, tht, psi;
-  float latitude, longitude, h;
-  float wdir, wmag, wnorth, weast;
+  double U, V, W;
+  double phi, tht, psi;
+  double latitude, longitude, h;
+  double wdir, wmag, wnorth, weast;
 
 # ifndef macintosh
   resetDef = path + "/" + acname + "/" + fname + ".xml";
@@ -416,15 +479,17 @@ bool FGState::Reset(string path, string acname, string fname)
 
   resetfile.GetNextConfigLine();
   token = resetfile.GetValue();
-  if (token != "initialize") {
+  if (token != string("initialize")) {
     cerr << "The reset file " << resetDef
          << " does not appear to be a reset file" << endl;
     return false;
+  } else {
+    resetfile.GetNextConfigLine();
+    resetfile >> token;
+    cout << "Resetting using: " << token << endl << endl;
   }
   
-  resetfile.GetNextConfigLine();
-  resetfile >> token;
-  while (token != "/initialize" && token != "EOF") {
+  while (token != string("/initialize") && token != string("EOF")) {
     if (token == "UBODY") resetfile >> U;
     if (token == "VBODY") resetfile >> V;
     if (token == "WBODY") resetfile >> W;
@@ -441,15 +506,15 @@ bool FGState::Reset(string path, string acname, string fname)
   }
   
   
-  Position->SetLatitude(latitude*DEGTORAD);
-  Position->SetLongitude(longitude*DEGTORAD);
+  Position->SetLatitude(latitude*degtorad);
+  Position->SetLongitude(longitude*degtorad);
   Position->Seth(h);
 
-  wnorth = wmag*KTSTOFPS*cos(wdir*DEGTORAD);
-  weast = wmag*KTSTOFPS*sin(wdir*DEGTORAD);
+  wnorth = wmag*ktstofps*cos(wdir*degtorad);
+  weast = wmag*ktstofps*sin(wdir*degtorad);
   
-  Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD,
-               latitude*DEGTORAD, longitude*DEGTORAD, h, wnorth, weast, 0.0);
+  Initialize(U, V, W, phi*degtorad, tht*degtorad, psi*degtorad,
+               latitude*degtorad, longitude*degtorad, h, wnorth, weast, 0.0);
 
   return true;
 }
@@ -459,14 +524,14 @@ bool FGState::Reset(string path, string acname, string fname)
 // Initialize: Assume all angles GIVEN IN RADIANS !!
 //
 
-void FGState::Initialize(float U, float V, float W,
-                         float phi, float tht, float psi,
-                         float Latitude, float Longitude, float H,
-                         float wnorth, float weast, float wdown)
+void FGState::Initialize(double U, double V, double W,
+                         double phi, double tht, double psi,
+                         double Latitude, double Longitude, double H,
+                         double wnorth, double weast, double wdown)
 {
-  float alpha, beta;
-  float qbar, Vt;
-  FGColumnVector3 vAero;
+  double alpha, beta;
+  double qbar, Vt;
+  FGColumnVector3 vAeroUVW;
 
   Position->SetLatitude(Latitude);
   Position->SetLongitude(Longitude);
@@ -484,14 +549,14 @@ void FGState::Initialize(float U, float V, float W,
   
   Atmosphere->SetWindNED(wnorth, weast, wdown);
   
-  vAero = vUVW + mTl2b*Atmosphere->GetWindNED();
+  vAeroUVW = vUVW + mTl2b*Atmosphere->GetWindNED();
   
-  if (vAero(eW) != 0.0)
-    alpha = vAero(eU)*vAero(eU) > 0.0 ? atan2(vAero(eW), vAero(eU)) : 0.0;
+  if (vAeroUVW(eW) != 0.0)
+    alpha = vAeroUVW(eU)*vAeroUVW(eU) > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0;
   else
     alpha = 0.0;
-  if (vAero(eV) != 0.0)
-    beta = vAero(eU)*vAero(eU)+vAero(eW)*vAero(eW) > 0.0 ? atan2(vAero(eV), (fabs(vAero(eU))/vAero(eU))*sqrt(vAero(eU)*vAero(eU) + vAero(eW)*vAero(eW))) : 0.0;
+  if (vAeroUVW(eV) != 0.0)
+    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;
   else
     beta = 0.0;
 
@@ -513,10 +578,10 @@ void FGState::Initialize(float U, float V, float W,
 
 void FGState::Initialize(FGInitialCondition *FGIC) {
 
-  float tht,psi,phi;
-  float U, V, W, h;
-  float latitude, longitude;
-  float wnorth,weast, wdown;
+  double tht,psi,phi;
+  double U, V, W, h;
+  double latitude, longitude;
+  double wnorth,weast, wdown;
   
   latitude = FGIC->GetLatitudeRadIC();
   longitude = FGIC->GetLongitudeRadIC();
@@ -564,14 +629,14 @@ bool FGState::StoreData(string fname) {
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-void FGState::InitMatrices(float phi, float tht, float psi) {
-  float thtd2, psid2, phid2;
-  float Sthtd2, Spsid2, Sphid2;
-  float Cthtd2, Cpsid2, Cphid2;
-  float Cphid2Cthtd2;
-  float Cphid2Sthtd2;
-  float Sphid2Sthtd2;
-  float Sphid2Cthtd2;
+void FGState::InitMatrices(double phi, double tht, double psi) {
+  double thtd2, psid2, phid2;
+  double Sthtd2, Spsid2, Sphid2;
+  double Cthtd2, Cpsid2, Cphid2;
+  double Cphid2Cthtd2;
+  double Cphid2Sthtd2;
+  double Sphid2Sthtd2;
+  double Sphid2Cthtd2;
 
   thtd2 = tht/2.0;
   psid2 = psi/2.0;
@@ -601,9 +666,9 @@ void FGState::InitMatrices(float phi, float tht, float psi) {
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGState::CalcMatrices(void) {
-  float Q0Q0, Q1Q1, Q2Q2, Q3Q3;
-  float Q0Q1, Q0Q2, Q0Q3, Q1Q2;
-  float Q1Q3, Q2Q3;
+  double Q0Q0, Q1Q1, Q2Q2, Q3Q3;
+  double Q0Q1, Q0Q2, Q0Q3, Q1Q2;
+  double Q1Q3, Q2Q3;
 
   Q0Q0 = vQtrn(1)*vQtrn(1);
   Q1Q1 = vQtrn(2)*vQtrn(2);
@@ -661,33 +726,63 @@ FGColumnVector3& FGState::CalcEuler(void) {
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-FGMatrix33& FGState::GetTs2b(float alpha, float beta)
+FGMatrix33& FGState::GetTs2b(void)
 {
-  float ca, cb, sa, sb;
+  double ca, cb, sa, sb;
+
+  double alpha = Translation->Getalpha();
+  double beta  = Translation->Getbeta();
 
   ca = cos(alpha);
   sa = sin(alpha);
   cb = cos(beta);
   sb = sin(beta);
 
-  mTs2b(1,1) = -ca*cb;
+  mTs2b(1,1) = ca*cb;
   mTs2b(1,2) = -ca*sb;
-  mTs2b(1,3) = sa;
-  mTs2b(2,1) = -sb;
+  mTs2b(1,3) = -sa;
+  mTs2b(2,1) = sb;
   mTs2b(2,2) = cb;
   mTs2b(2,3) = 0.0;
-  mTs2b(3,1) = -sa*cb;
+  mTs2b(3,1) = sa*cb;
   mTs2b(3,2) = -sa*sb;
-  mTs2b(3,3) = -ca;
+  mTs2b(3,3) = ca;
 
   return mTs2b;
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33& FGState::GetTb2s(void)
+{
+  float alpha,beta;
+  float ca, cb, sa, sb;
+  
+  alpha = Translation->Getalpha();
+  beta  = Translation->Getbeta();
+  
+  ca = cos(alpha);
+  sa = sin(alpha);
+  cb = cos(beta);
+  sb = sin(beta);
+
+  mTb2s(1,1) = ca*cb;
+  mTb2s(1,2) = sb;
+  mTb2s(1,3) = sa*cb;
+  mTb2s(2,1) = -ca*sb;
+  mTb2s(2,2) = cb;
+  mTb2s(2,3) = -sa*sb;
+  mTb2s(3,1) = -sa;
+  mTb2s(3,2) = 0.0;
+  mTb2s(3,3) = ca;
+
+  return mTb2s;
+}
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGState::ReportState(void) {
-  char out[80], flap[10], gear[10];
+  char out[80], flap[10], gear[12];
   
   cout << endl << "  JSBSim State" << endl;
   snprintf(out,80,"    Weight: %7.0f lbs.  CG: %5.1f, %5.1f, %5.1f inches\n",
@@ -700,11 +795,13 @@ void FGState::ReportState(void) {
     snprintf(flap,10,"Up");
   else
     snprintf(flap,10,"%2.0f",FCS->GetDfPos());
-  if(GroundReactions->GetGearUp() == true)
-    snprintf(gear,10,"Up");
+  if(FCS->GetGearPos() < 0.01)
+    snprintf(gear,12,"Up");
+  else if(FCS->GetGearPos() > 0.99)
+    snprintf(gear,12,"Down");
   else
-    snprintf(gear,10,"Down");
-  snprintf(out,80, "    Flaps: %3s  Gear: %4s\n",flap,gear);
+    snprintf(gear,12,"In Transit");   
+  snprintf(out,80, "    Flaps: %3s  Gear: %12s\n",flap,gear);
   cout << out;
   snprintf(out,80, "    Speed: %4.0f KCAS  Mach: %5.2f\n",
                     FDMExec->GetAuxiliary()->GetVcalibratedKTS(),
@@ -715,48 +812,88 @@ void FGState::ReportState(void) {
                     Position->GetDistanceAGL() );
   cout << out;
   snprintf(out,80, "    Angle of Attack: %6.2f deg  Pitch Angle: %6.2f deg\n",
-                    GetParameter(FG_ALPHA)*RADTODEG,
-                    Rotation->Gettht()*RADTODEG );
+                    GetParameter(FG_ALPHA)*radtodeg,
+                    Rotation->Gettht()*radtodeg );
   cout << out;
   snprintf(out,80, "    Flight Path Angle: %6.2f deg  Climb Rate: %5.0f ft/min\n",
-                    Position->GetGamma()*RADTODEG,
+                    Position->GetGamma()*radtodeg,
                     Position->Gethdot()*60 );
   cout << out;                  
   snprintf(out,80, "    Normal Load Factor: %4.2f g's  Pitch Rate: %5.2f deg/s\n",
-                    Aerodynamics->GetNlf(),
-                    GetParameter(FG_PITCHRATE)*RADTODEG );
+                    Aircraft->GetNlf(),
+                    GetParameter(FG_PITCHRATE)*radtodeg );
   cout << out;
   snprintf(out,80, "    Heading: %3.0f deg true  Sideslip: %5.2f deg\n",
-                    Rotation->Getpsi()*RADTODEG,
-                    GetParameter(FG_BETA)*RADTODEG );                  
+                    Rotation->Getpsi()*radtodeg,
+                    GetParameter(FG_BETA)*radtodeg );                  
   cout << out;
   snprintf(out,80, "    Bank Angle: %5.2f deg\n",
-                    Rotation->Getphi()*RADTODEG );
+                    Rotation->Getphi()*radtodeg );
   cout << out;
   snprintf(out,80, "    Elevator: %5.2f deg  Left Aileron: %5.2f deg  Rudder: %5.2f deg\n",
-                    GetParameter(FG_ELEVATOR_POS)*RADTODEG,
-                    GetParameter(FG_AILERON_POS)*RADTODEG,
-                    GetParameter(FG_RUDDER_POS)*RADTODEG );
+                    GetParameter(FG_ELEVATOR_POS)*radtodeg,
+                    GetParameter(FG_AILERON_POS)*radtodeg,
+                    GetParameter(FG_RUDDER_POS)*radtodeg );
   cout << out;                  
   snprintf(out,80, "    Throttle: %5.2f%c\n",
                     FCS->GetThrottlePos(0)*100,'%' );
   cout << out;
   
   snprintf(out,80, "    Wind Components: %5.2f kts head wind, %5.2f kts cross wind\n",
-                    FDMExec->GetAuxiliary()->GetHeadWind()*jsbFPSTOKTS,
-                    FDMExec->GetAuxiliary()->GetCrossWind()*jsbFPSTOKTS );
+                    FDMExec->GetAuxiliary()->GetHeadWind()*fpstokts,
+                    FDMExec->GetAuxiliary()->GetCrossWind()*fpstokts );
   cout << out; 
   
   snprintf(out,80, "    Ground Speed: %4.0f knots , Ground Track: %3.0f deg true\n",
-                    Position->GetVground()*jsbFPSTOKTS,
-                    Position->GetGroundTrack()*RADTODEG );
+                    Position->GetVground()*fpstokts,
+                    Position->GetGroundTrack()*radtodeg );
   cout << out;                                   
 
 } 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-void FGState::Debug(void)
+//    The bitmasked value choices are as follows:
+//    unset: In this case (the default) JSBSim would only print
+//       out the normally expected messages, essentially echoing
+//       the config files as they are read. If the environment
+//       variable is not set, debug_lvl is set to 1 internally
+//    0: This requests JSBSim not to output any messages
+//       whatsoever.
+//    1: This value explicity requests the normal JSBSim
+//       startup messages
+//    2: This value asks for a message to be printed out when
+//       a class is instantiated
+//    4: When this value is set, a message is displayed when a
+//       FGModel object executes its Run() method
+//    8: When this value is set, various runtime state variables
+//       are printed out periodically
+//    16: When set various parameters are sanity checked and
+//       a message is printed out when they go out of bounds
+
+void FGState::Debug(int from)
 {
-    //TODO: Add your source code here
+  if (debug_lvl <= 0) return;
+
+  if (debug_lvl & 1) { // Standard console startup message output
+    if (from == 0) { // Constructor
+
+    }
+  }
+  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
+    if (from == 0) cout << "Instantiated: FGState" << endl;
+    if (from == 1) cout << "Destroyed:    FGState" << endl;
+  }
+  if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
+  }
+  if (debug_lvl & 8 ) { // Runtime state variables
+  }
+  if (debug_lvl & 16) { // Sanity checking
+  }
+  if (debug_lvl & 64) {
+    if (from == 0) { // Constructor
+      cout << IdSrc << endl;
+      cout << IdHdr << endl;
+    }
+  }
 }
 
+