Merge branch 'next' of gitorious.org:fg/flightgear into next

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

diff --git a/src/FDM/JSBSim/models/flight_control/FGGyro.cpp b/src/FDM/JSBSim/models/flight_control/FGGyro.cpp
old mode 100755 (executable)
new mode 100644 (file)
index 0fcd127..7ee6abe
--- a/src/FDM/JSBSim/models/flight_control/FGGyro.cpp
+++ b/src/FDM/JSBSim/models/flight_control/FGGyro.cpp
@@ -38,41 +38,26 @@ INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGGyro.h"
+#include <iostream>
+
+#include "models/FGAccelerations.h"
+
+using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGGyro.cpp,v 1.6 2011/07/17 13:51:23 jberndt Exp $";
 static const char *IdHdr = ID_GYRO;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 CLASS IMPLEMENTATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-FGGyro::FGGyro(FGFCS* fcs, Element* element) : FGSensor(fcs, element)
+FGGyro::FGGyro(FGFCS* fcs, Element* element) : FGSensor(fcs, element),
+                                               FGSensorOrientation(element)
 {
-  Propagate = fcs->GetExec()->GetPropagate();
+  Accelerations = fcs->GetExec()->GetAccelerations();
   
-  Element* orient_element = element->FindElement("orientation");
-  if (orient_element) vOrient = orient_element->FindElementTripletConvertTo("RAD");
-  else {cerr << "No orientation given for gyro. " << endl;}
-
-  Element* axis_element = element->FindElement("axis");
-  if (axis_element) {
-    string sAxis = element->FindElementValue("axis");
-    if (sAxis == "ROLL" || sAxis == "roll") {
-      axis = 1;
-    } else if (sAxis == "PITCH" || sAxis == "pitch") {
-      axis = 2;
-    } else if (sAxis == "YAW" || sAxis == "yaw") {
-      axis = 3;
-    } else {
-      cerr << "  Incorrect/no axis specified for gyro; assuming Roll axis" << endl;
-      axis = 1;
-    }
-  }
-
-  CalculateTransformMatrix();
-
   Debug(0);
 }
 
@@ -90,63 +75,15 @@ bool FGGyro::Run(void )
   // There is no input assumed. This is a dedicated angular acceleration sensor.
   
   //aircraft rates
-  vAccel = mT * Propagate->GetPQRdot();
+  vAccel = mT * Accelerations->GetPQRdot();
 
   Input = vAccel(axis);
 
-  Output = Input; // perfect gyro
-
-  // Degrade signal as specified
-
-  if (fail_stuck) {
-    Output = PreviousOutput;
-    return true;
-  }
-
-  if (lag != 0.0)            Lag();       // models gyro 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 (gain != 0.0)           Gain();      // models a gain
-
-  if (fail_low)  Output = -HUGE_VAL;
-  if (fail_high) Output =  HUGE_VAL;
+  ProcessSensorSignal();
 
-  if (bits != 0)             Quantize();  // models quantization degradation
-//  if (delay != 0.0)          Delay();     // models system signal transport latencies
-
-  Clip(); // Is it right to clip a gyro?
   return true;
 }
 
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-void FGGyro::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 gyro 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