Latest JSBSim changes.

[flightgear.git] / src / FDM / JSBSim / FGAuxiliary.cpp

diff --git a/src/FDM/JSBSim/FGAuxiliary.cpp b/src/FDM/JSBSim/FGAuxiliary.cpp
index c8ddc8a3a2ddae52da3a3f903da522b7c10c89d6..4657cb34c16ea652b3908a5669935d292e49a552 100644 (file)
--- a/src/FDM/JSBSim/FGAuxiliary.cpp
+++ b/src/FDM/JSBSim/FGAuxiliary.cpp
@@ -50,21 +50,20 @@ INCLUDES
 #include "FGAircraft.h"
 #include "FGPosition.h"
 #include "FGOutput.h"
-#include "FGMatrix.h"
+#include "FGInertial.h"
+#include "FGMatrix33.h"
+#include "FGColumnVector3.h"
+#include "FGColumnVector4.h"
 
 static const char *IdSrc = "$Id$";
 static const char *IdHdr = ID_AUXILIARY;
 
-extern short debug_lvl;
-
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 CLASS IMPLEMENTATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 
-FGAuxiliary::FGAuxiliary(FGFDMExec* fdmex) : FGModel(fdmex),
-  vPilotAccel(3),
-  vToEyePt(3)
+FGAuxiliary::FGAuxiliary(FGFDMExec* fdmex) : FGModel(fdmex)
 {
   Name = "FGAuxiliary";
   vcas = veas = mach = qbar = pt = 0;
@@ -85,7 +84,7 @@ FGAuxiliary::~FGAuxiliary()
 
 bool FGAuxiliary::Run()
 {
-  float A,B,D;
+  double A,B,D;
 
   if (!FGModel::Run()) {
     GetState();
@@ -109,14 +108,52 @@ bool FGAuxiliary::Run()
     vcas = sqrt(7*psl/rhosl*(A-1));
     veas = sqrt(2*qbar/rhosl);
 
-    // vPilotAccel = Translation->GetUVWdot() + Aircraft->GetXYZep() * Rotation->GetPQRdot();
-    
+    // Pilot sensed accelerations are calculated here. This is used
+    // for the coordinated turn ball instrument. Motion base platforms sometimes
+    // use the derivative of pilot sensed accelerations as the driving parameter,
+    // rather than straight accelerations.
+    //
+    // The theory behind pilot-sensed calculations is presented:
+    //
+    // For purposes of discussion and calculation, assume for a minute that the
+    // pilot is in space and motionless in inertial space. She will feel
+    // no accelerations. If the aircraft begins to accelerate along any axis or
+    // axes (without rotating), the pilot will sense those accelerations. If
+    // any rotational moment is applied, the pilot will sense an acceleration
+    // due to that motion in the amount:
+    //
+    // [wdot X R]  +  [w X (w X R)]
+    //   Term I          Term II
+    //
+    // where:
+    //
+    // wdot = omegadot, the rotational acceleration rate vector
+    // w    = omega, the rotational rate vector
+    // R    = the vector from the aircraft CG to the pilot eyepoint
+    //
+    // The sum total of these two terms plus the acceleration of the aircraft
+    // body axis gives the acceleration the pilot senses in inertial space.
+    // In the presence of a large body such as a planet, a gravity field also
+    // provides an accelerating attraction. This acceleration can be transformed
+    // from the reference frame of the planet so as to be expressed in the frame
+    // of reference of the aircraft. This gravity field accelerating attraction
+    // is felt by the pilot as a force on her tushie as she sits in her aircraft
+    // on the runway awaiting takeoff clearance.
+    //
+    // In JSBSim the acceleration of the body frame in inertial space is given
+    // by the F = ma relation. If the vForces vector is divided by the aircraft
+    // mass, the acceleration vector is calculated. The term wdot is equivalent
+    // to the JSBSim vPQRdot vector, and the w parameter is equivalent to vPQR.
+    // The radius R is calculated below in the vector vToEyePt.
+        
     vToEyePt = Aircraft->GetXYZep() - MassBalance->GetXYZcg();
-    vPilotAccel = Translation->GetUVWdot()
-                    + Rotation->GetPQRdot() * vToEyePt
-                    + Rotation->GetPQR() * (Rotation->GetPQR() * vToEyePt);
 
-    earthPosAngle += State->Getdt()*OMEGA_EARTH;
+    vPilotAccel = Aircraft->GetBodyAccel()
+                  + Rotation->GetPQRdot() * vToEyePt
+                  + Rotation->GetPQR() * (Rotation->GetPQR() * vToEyePt)
+                  + Inertial->GetGravity();
+
+    earthPosAngle += State->Getdt()*Inertial->omega();
     return false;
   } else {
     return true;
@@ -125,22 +162,22 @@ bool FGAuxiliary::Run()
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-float FGAuxiliary::GetHeadWind(void)
+double FGAuxiliary::GetHeadWind(void)
 {
-  float psiw,vw,psi;
+  double psiw,vw,psi;
 
   psiw = Atmosphere->GetWindPsi();
   psi = Rotation->Getpsi();
   vw = Atmosphere->GetWindNED().Magnitude();
 
-  return -vw*cos(psiw - psi);
+  return vw*cos(psiw - psi);
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-float FGAuxiliary::GetCrossWind(void)
+double FGAuxiliary::GetCrossWind(void)
 {
-  float psiw,vw,psi;
+  double psiw,vw,psi;
 
   psiw = Atmosphere->GetWindPsi();
   psi = Rotation->Getpsi();
@@ -151,6 +188,20 @@ float FGAuxiliary::GetCrossWind(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+FGColumnVector3 FGAuxiliary::GetNpilot(void)
+{
+  return vPilotAccel/Inertial->gravity();
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+double FGAuxiliary::GetNpilot(int idx)
+{
+  return (vPilotAccel/Inertial->gravity())(idx);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGAuxiliary::GetState(void)
 {
   qbar = Translation->Getqbar();
@@ -162,7 +213,7 @@ void FGAuxiliary::GetState(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-void FGAuxiliary::Debug(void)
+void FGAuxiliary::Debug(int from)
 {
     //TODO: Add your source code here
 }