Andreas Gaeb: fix #222 (JSBSIm reset problems)

[flightgear.git] / src / FDM / JSBSim / models / flight_control / FGAccelerometer.cpp

diff --git a/src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp b/src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp
index af9ca54b218b9adf5cfe2ea0a3896d245a198585..30c2f2244f0dafd45dbce930af012b4c4f65f42d 100755 (executable)
--- a/src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp
+++ b/src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp
@@ -38,21 +38,27 @@ INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGAccelerometer.h"
+#include <iostream>
+#include <cstdlib>
+
+using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGAccelerometer.cpp,v 1.8 2009/10/24 22:59:30 jberndt Exp $";
 static const char *IdHdr = ID_ACCELEROMETER;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 CLASS IMPLEMENTATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-
-FGAccelerometer::FGAccelerometer(FGFCS* fcs, Element* element) : FGSensor(fcs, element)
+FGAccelerometer::FGAccelerometer(FGFCS* fcs, Element* element)
+  : FGSensor(fcs, element),
+    FGSensorOrientation(element)
 {
   Propagate = fcs->GetExec()->GetPropagate();
   MassBalance = fcs->GetExec()->GetMassBalance();
+  Inertial = fcs->GetExec()->GetInertial();
   
   Element* location_element = element->FindElement("location");
   if (location_element) vLocation = location_element->FindElementTripletConvertTo("IN");
@@ -60,27 +66,6 @@ FGAccelerometer::FGAccelerometer(FGFCS* fcs, Element* element) : FGSensor(fcs, e
 
   vRadius = MassBalance->StructuralToBody(vLocation);
 
-  Element* orient_element = element->FindElement("orientation");
-  if (orient_element) vOrient = orient_element->FindElementTripletConvertTo("RAD");
-  else {cerr << "No orientation given for accelerometer. " << endl;}
-
-  Element* axis_element = element->FindElement("axis");
-  if (axis_element) {
-    string sAxis = element->FindElementValue("axis");
-    if (sAxis == "X" || sAxis == "x") {
-      axis = 1;
-    } else if (sAxis == "Y" || sAxis == "y") {
-      axis = 2;
-    } else if (sAxis == "Z" || sAxis == "z") {
-      axis = 3;
-    } else {
-      cerr << "  Incorrect/no axis specified for accelerometer; assuming X axis" << endl;
-      axis = 1;
-    }
-  }
-
-  CalculateTransformMatrix();
-
   Debug(0);
 }
 
@@ -96,67 +81,27 @@ FGAccelerometer::~FGAccelerometer()
 bool FGAccelerometer::Run(void )
 {
   // There is no input assumed. This is a dedicated acceleration sensor.
-  
-  vRadius = MassBalance->StructuralToBody(vLocation);
-
-  vAccel = mT * (Propagate->GetUVWdot()
-                 + Propagate->GetPQRdot() * vRadius
-                 + Propagate->GetPQR() * (Propagate->GetPQR() * vRadius));
-  
-  Input = vAccel(axis);
 
-  Output = Input; // perfect accelerometer
+  vRadius = MassBalance->StructuralToBody(vLocation);
+    
+  //gravitational forces
+  vAccel = Propagate->GetTl2b() * FGColumnVector3(0, 0, Inertial->gravity());
 
-  // Degrade signal as specified
+  //aircraft forces
+  vAccel += (Propagate->GetUVWdot()
+              + Propagate->GetPQRdot() * vRadius
+              + Propagate->GetPQR() * (Propagate->GetPQR() * vRadius));
 
-  if (fail_stuck) {
-    Output = PreviousOutput;
-    return true;
-  }
+  // transform to the specified orientation
+  vAccel = mT * vAccel;
 
-  if (lag != 0.0)            Lag();       // models accelerometer lag
-  if (noise_variance != 0.0) Noise();     // models noise
-  if (drift_rate != 0.0)     Drift();     // models drift over time
-  if (bias != 0.0)           Bias();      // models a finite bias
-
-  if (fail_low)  Output = -HUGE_VAL;
-  if (fail_high) Output =  HUGE_VAL;
+  Input = vAccel(axis);
 
-  if (bits != 0)             Quantize();  // models quantization degradation
-//  if (delay != 0.0)          Delay();     // models system signal transport latencies
+  ProcessSensorSignal();
 
-  Clip(); // Is it right to clip an accelerometer?
   return true;
 }
 
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-void FGAccelerometer::CalculateTransformMatrix(void)
-{
-  double cp,sp,cr,sr,cy,sy;
-
-  cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
-  cr=cos(vOrient(eRoll));  sr=sin(vOrient(eRoll));
-  cy=cos(vOrient(eYaw));   sy=sin(vOrient(eYaw));
-
-  mT(1,1) =  cp*cy;
-  mT(1,2) =  cp*sy;
-  mT(1,3) = -sp;
-
-  mT(2,1) = sr*sp*cy - cr*sy;
-  mT(2,2) = sr*sp*sy + cr*cy;
-  mT(2,3) = sr*cp;
-
-  mT(3,1) = cr*sp*cy + sr*sy;
-  mT(3,2) = cr*sp*sy - sr*cy;
-  mT(3,3) = cr*cp;
-  
-  // This transform is different than for FGForce, where we want a native nozzle
-  // force in body frame. Here we calculate the body frame accel and want it in
-  // the transformed accelerometer frame. So, the next line is commented out.
-  // mT = mT.Inverse();
-}
-
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 //    The bitmasked value choices are as follows:
 //    unset: In this case (the default) JSBSim would only print