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

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGGyro.cpp
   4  Author:       Jon Berndt
   5  Date started: 29 August 2009
   6
   7  ------------- Copyright (C) 2009 -------------
   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 "FGGyro.h"
  41
  42 namespace JSBSim {
  43
  44 static const char *IdSrc = "$Id$";
  45 static const char *IdHdr = ID_GYRO;
  46
  47 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  48 CLASS IMPLEMENTATION
  49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  50
  51 FGGyro::FGGyro(FGFCS* fcs, Element* element) : FGSensor(fcs, element)
  52 {
  53   Propagate = fcs->GetExec()->GetPropagate();
  54
  55   Element* orient_element = element->FindElement("orientation");
  56   if (orient_element) vOrient = orient_element->FindElementTripletConvertTo("RAD");
  57   else {cerr << "No orientation given for gyro. " << endl;}
  58
  59   Element* axis_element = element->FindElement("axis");
  60   if (axis_element) {
  61     string sAxis = element->FindElementValue("axis");
  62     if (sAxis == "ROLL" || sAxis == "roll") {
  63       axis = 1;
  64     } else if (sAxis == "PITCH" || sAxis == "pitch") {
  65       axis = 2;
  66     } else if (sAxis == "YAW" || sAxis == "yaw") {
  67       axis = 3;
  68     } else {
  69       cerr << "  Incorrect/no axis specified for gyro; assuming Roll axis" << endl;
  70       axis = 1;
  71     }
  72   }
  73
  74   CalculateTransformMatrix();
  75
  76   Debug(0);
  77 }
  78
  79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  80
  81 FGGyro::~FGGyro()
  82 {
  83   Debug(1);
  84 }
  85
  86 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  87
  88 bool FGGyro::Run(void )
  89 {
  90   // There is no input assumed. This is a dedicated angular acceleration sensor.
  91
  92   //aircraft rates
  93   vAccel = mT * Propagate->GetPQRdot();
  94
  95   Input = vAccel(axis);
  96
  97   Output = Input; // perfect gyro
  98
  99   // Degrade signal as specified
 100
 101   if (fail_stuck) {
 102     Output = PreviousOutput;
 103     return true;
 104   }
 105
 106   if (lag != 0.0)            Lag();       // models gyro lag
 107   if (noise_variance != 0.0) Noise();     // models noise
 108   if (drift_rate != 0.0)     Drift();     // models drift over time
 109   if (bias != 0.0)           Bias();      // models a finite bias
 110   if (gain != 0.0)           Gain();      // models a gain
 111
 112   if (fail_low)  Output = -HUGE_VAL;
 113   if (fail_high) Output =  HUGE_VAL;
 114
 115   if (bits != 0)             Quantize();  // models quantization degradation
 116 //  if (delay != 0.0)          Delay();     // models system signal transport latencies
 117
 118   Clip(); // Is it right to clip a gyro?
 119   return true;
 120 }
 121
 122 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 123
 124 void FGGyro::CalculateTransformMatrix(void)
 125 {
 126   double cp,sp,cr,sr,cy,sy;
 127
 128   cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
 129   cr=cos(vOrient(eRoll));  sr=sin(vOrient(eRoll));
 130   cy=cos(vOrient(eYaw));   sy=sin(vOrient(eYaw));
 131
 132   mT(1,1) =  cp*cy;
 133   mT(1,2) =  cp*sy;
 134   mT(1,3) = -sp;
 135
 136   mT(2,1) = sr*sp*cy - cr*sy;
 137   mT(2,2) = sr*sp*sy + cr*cy;
 138   mT(2,3) = sr*cp;
 139
 140   mT(3,1) = cr*sp*cy + sr*sy;
 141   mT(3,2) = cr*sp*sy - sr*cy;
 142   mT(3,3) = cr*cp;
 143
 144   // This transform is different than for FGForce, where we want a native nozzle
 145   // force in body frame. Here we calculate the body frame accel and want it in
 146   // the transformed gyro frame. So, the next line is commented out.
 147   // mT = mT.Inverse();
 148 }
 149
 150 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 151 //    The bitmasked value choices are as follows:
 152 //    unset: In this case (the default) JSBSim would only print
 153 //       out the normally expected messages, essentially echoing
 154 //       the config files as they are read. If the environment
 155 //       variable is not set, debug_lvl is set to 1 internally
 156 //    0: This requests JSBSim not to output any messages
 157 //       whatsoever.
 158 //    1: This value explicity requests the normal JSBSim
 159 //       startup messages
 160 //    2: This value asks for a message to be printed out when
 161 //       a class is instantiated
 162 //    4: When this value is set, a message is displayed when a
 163 //       FGModel object executes its Run() method
 164 //    8: When this value is set, various runtime state variables
 165 //       are printed out periodically
 166 //    16: When set various parameters are sanity checked and
 167 //       a message is printed out when they go out of bounds
 168
 169 void FGGyro::Debug(int from)
 170 {
 171   string ax[4] = {"none", "X", "Y", "Z"};
 172
 173   if (debug_lvl <= 0) return;
 174
 175   if (debug_lvl & 1) { // Standard console startup message output
 176     if (from == 0) { // Constructor
 177       cout << "        Axis: " << ax[axis] << endl;
 178     }
 179   }
 180   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 181     if (from == 0) cout << "Instantiated: FGGyro" << endl;
 182     if (from == 1) cout << "Destroyed:    FGGyro" << endl;
 183   }
 184   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 185   }
 186   if (debug_lvl & 8 ) { // Runtime state variables
 187   }
 188   if (debug_lvl & 16) { // Sanity checking
 189   }
 190   if (debug_lvl & 64) {
 191     if (from == 0) { // Constructor
 192       cout << IdSrc << endl;
 193       cout << IdHdr << endl;
 194     }
 195   }
 196 }
 197 }