sync with JSB JSBSim CVS

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

diff --git a/src/FDM/JSBSim/models/flight_control/FGMagnetometer.cpp b/src/FDM/JSBSim/models/flight_control/FGMagnetometer.cpp
old mode 100755 (executable)
new mode 100644 (file)
index 561e0dc..3c04e68
--- a/src/FDM/JSBSim/models/flight_control/FGMagnetometer.cpp
+++ b/src/FDM/JSBSim/models/flight_control/FGMagnetometer.cpp
@@ -40,10 +40,14 @@ INCLUDES
 #include "FGMagnetometer.h"
 #include "simgear/magvar/coremag.hxx"
 #include <ctime>
+#include <cstdlib>
+#include <iostream>
+
+using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGMagnetometer.cpp,v 1.5 2009/10/24 22:59:30 jberndt Exp $";
 static const char *IdHdr = ID_MAGNETOMETER;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -51,7 +55,8 @@ CLASS IMPLEMENTATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 
-FGMagnetometer::FGMagnetometer(FGFCS* fcs, Element* element) : FGSensor(fcs, element),\
+FGMagnetometer::FGMagnetometer(FGFCS* fcs, Element* element) : FGSensor(fcs, element),
+                                                               FGSensorOrientation(element),
                                                                counter(0),
                                                                INERTIAL_UPDATE_RATE(1000)
 {
@@ -65,27 +70,6 @@ FGMagnetometer::FGMagnetometer(FGFCS* fcs, Element* element) : FGSensor(fcs, ele
 
   vRadius = MassBalance->StructuralToBody(vLocation);
 
-  Element* orient_element = element->FindElement("orientation");
-  if (orient_element) vOrient = orient_element->FindElementTripletConvertTo("RAD");
-  else {cerr << "No orientation given for magnetometer. " << 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 magnetometer; assuming X axis" << endl;
-      axis = 1;
-    }
-  }
-
-  CalculateTransformMatrix();
-
   //assuming date wont significantly change over a flight to affect mag field
   //would be better to get the date from the sim if its simulated...
   time_t rawtime;
@@ -114,7 +98,7 @@ FGMagnetometer::~FGMagnetometer()
 void FGMagnetometer::updateInertialMag(void)
 {
   counter++;
-  if(counter > INERTIAL_UPDATE_RATE)//dont need to update every iteration
+  if (counter > INERTIAL_UPDATE_RATE)//dont need to update every iteration
   {
       counter = 0;
 
@@ -123,11 +107,11 @@ void FGMagnetometer::updateInertialMag(void)
       usedAlt = (Propagate->GetGeodeticAltitude()*fttom*0.001);//km
 
       //this should be done whenever the position changes significantly (in nTesla)
-      double magvar = calc_magvar( usedLat,
-                                 usedLon,
-                                 usedAlt,
-                                 date,
-                                 field );
+      calc_magvar( usedLat,
+                   usedLon,
+                   usedAlt,
+                   date,
+                   field );
   }
 }
 
@@ -135,75 +119,25 @@ void FGMagnetometer::updateInertialMag(void)
 
 bool FGMagnetometer::Run(void )
 {
-  // There is no input assumed. This is a dedicated acceleration sensor.
+  // There is no input assumed. This is a dedicated magnetic field sensor.
   
   vRadius = MassBalance->StructuralToBody(vLocation);
 
-
   updateInertialMag();
-  //Inertial magnetic field rotated to the body frame
+
+  // Inertial magnetic field rotated to the body frame
   vMag = Propagate->GetTl2b() * FGColumnVector3(field[3], field[4], field[5]);
 
-  //allow for sensor orientation
+  // Allow for sensor orientation
   vMag = mT * vMag;
   
   Input = vMag(axis);
 
-  Output = Input; // perfect magnetometer
-
-  // Degrade signal as specified
-
-  if (fail_stuck) {
-    Output = PreviousOutput;
-    return true;
-  }
-
-  if (lag != 0.0)            Lag();       // models magnetometer 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;
-
-  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 magnetometer?
   return true;
 }
 
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-void FGMagnetometer::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 magnetometer 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