%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGAccelerometer.h"
+#include <iostream>
+#include <cstdlib>
+
+#include "models/FGPropagate.h"
+#include "models/FGAccelerations.h"
+#include "models/FGMassBalance.h"
+#include "models/FGInertial.h"
+
+using namespace std;
namespace JSBSim {
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGAccelerometer.cpp,v 1.9 2011/07/17 13:51:23 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();
+ Accelerations = fcs->GetExec()->GetAccelerations();
MassBalance = fcs->GetExec()->GetMassBalance();
Inertial = fcs->GetExec()->GetInertial();
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);
}
bool FGAccelerometer::Run(void )
{
// There is no input assumed. This is a dedicated acceleration sensor.
-
+
vRadius = MassBalance->StructuralToBody(vLocation);
//gravitational forces
vAccel = Propagate->GetTl2b() * FGColumnVector3(0, 0, Inertial->gravity());
//aircraft forces
- vAccel += (Propagate->GetUVWdot()
- + Propagate->GetPQRdot() * vRadius
+ vAccel += (Accelerations->GetUVWdot()
+ + Accelerations->GetPQRdot() * vRadius
+ Propagate->GetPQR() * (Propagate->GetPQR() * vRadius));
// transform to the specified orientation
Input = vAccel(axis);
- Output = Input; // perfect accelerometer
-
- // Degrade signal as specified
-
- if (fail_stuck) {
- Output = PreviousOutput;
- return true;
- }
-
- 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 (gain != 0.0) Gain(); // models a gain
+ ProcessSensorSignal();
- 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
-
- 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