src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGAccelerometer.cpp
   4  Author:       Jon Berndt
   5  Date started: 9 July 2005
   6
   7  ------------- Copyright (C) 2005 -------------
   8
   9  This program is free software; you can redistribute it and/or modify it under
  10  the terms of the GNU Lesser General Public License as published by the Free Software
  11  Foundation; either version 2 of the License, or (at your option) any later
  12  version.
  13
  14  This program is distributed in the hope that it will be useful, but WITHOUT
  15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  16  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU Lesser General Public License along with
  20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  21  Place - Suite 330, Boston, MA  02111-1307, USA.
  22
  23  Further information about the GNU Lesser General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26 FUNCTIONAL DESCRIPTION
  27 --------------------------------------------------------------------------------
  28
  29 HISTORY
  30 --------------------------------------------------------------------------------
  31
  32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  33 COMMENTS, REFERENCES,  and NOTES
  34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  35
  36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  37 INCLUDES
  38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  39
  40 #include <iostream>
  41 #include <cstdlib>
  42
  43 #include "FGAccelerometer.h"
  44 #include "models/FGPropagate.h"
  45 #include "models/FGAccelerations.h"
  46 #include "models/FGMassBalance.h"
  47 #include "models/FGInertial.h"
  48 #include "input_output/FGXMLElement.h"
  49
  50 using namespace std;
  51
  52 namespace JSBSim {
  53
  54 static const char *IdSrc = "$Id: FGAccelerometer.cpp,v 1.12 2013/11/24 11:40:56 bcoconni Exp $";
  55 static const char *IdHdr = ID_ACCELEROMETER;
  56
  57 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  58 CLASS IMPLEMENTATION
  59 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  60
  61 FGAccelerometer::FGAccelerometer(FGFCS* fcs, Element* element)
  62   : FGSensor(fcs, element),
  63     FGSensorOrientation(element)
  64 {
  65   Propagate = fcs->GetExec()->GetPropagate();
  66   Accelerations = fcs->GetExec()->GetAccelerations();
  67   MassBalance = fcs->GetExec()->GetMassBalance();
  68   Inertial = fcs->GetExec()->GetInertial();
  69
  70   Element* location_element = element->FindElement("location");
  71   if (location_element) vLocation = location_element->FindElementTripletConvertTo("IN");
  72   else {cerr << "No location given for accelerometer. " << endl; exit(-1);}
  73
  74   vRadius = MassBalance->StructuralToBody(vLocation);
  75
  76   Debug(0);
  77 }
  78
  79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  80
  81 FGAccelerometer::~FGAccelerometer()
  82 {
  83   Debug(1);
  84 }
  85
  86 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  87
  88 bool FGAccelerometer::Run(void )
  89 {
  90   // There is no input assumed. This is a dedicated acceleration sensor.
  91
  92   vRadius = MassBalance->StructuralToBody(vLocation);
  93
  94   //aircraft forces
  95   vAccel = (Accelerations->GetBodyAccel()
  96               + Propagate->GetTi2b() * Accelerations->GetGravAccel()
  97               + Accelerations->GetPQRdot() * vRadius
  98               + Propagate->GetPQR() * (Propagate->GetPQR() * vRadius));
  99
 100   // transform to the specified orientation
 101   vAccel = mT * vAccel;
 102
 103   Input = vAccel(axis);
 104
 105   ProcessSensorSignal();
 106
 107   if (IsOutput) SetOutput();
 108
 109   return true;
 110 }
 111
 112 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 113 //    The bitmasked value choices are as follows:
 114 //    unset: In this case (the default) JSBSim would only print
 115 //       out the normally expected messages, essentially echoing
 116 //       the config files as they are read. If the environment
 117 //       variable is not set, debug_lvl is set to 1 internally
 118 //    0: This requests JSBSim not to output any messages
 119 //       whatsoever.
 120 //    1: This value explicity requests the normal JSBSim
 121 //       startup messages
 122 //    2: This value asks for a message to be printed out when
 123 //       a class is instantiated
 124 //    4: When this value is set, a message is displayed when a
 125 //       FGModel object executes its Run() method
 126 //    8: When this value is set, various runtime state variables
 127 //       are printed out periodically
 128 //    16: When set various parameters are sanity checked and
 129 //       a message is printed out when they go out of bounds
 130
 131 void FGAccelerometer::Debug(int from)
 132 {
 133   string ax[4] = {"none", "X", "Y", "Z"};
 134
 135   if (debug_lvl <= 0) return;
 136
 137   if (debug_lvl & 1) { // Standard console startup message output
 138     if (from == 0) { // Constructor
 139       cout << "        Axis: " << ax[axis] << endl;
 140     }
 141   }
 142   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 143     if (from == 0) cout << "Instantiated: FGAccelerometer" << endl;
 144     if (from == 1) cout << "Destroyed:    FGAccelerometer" << endl;
 145   }
 146   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 147   }
 148   if (debug_lvl & 8 ) { // Runtime state variables
 149   }
 150   if (debug_lvl & 16) { // Sanity checking
 151   }
 152   if (debug_lvl & 64) {
 153     if (from == 0) { // Constructor
 154       cout << IdSrc << endl;
 155       cout << IdHdr << endl;
 156     }
 157   }
 158 }
 159 }