src/FDM/JSBSim/models/flight_control/FGSensorOrientation.h

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Header:       FGSensorOrientation.h
   4  Author:       Jon Berndt
   5  Date started: September 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 HISTORY
  27 --------------------------------------------------------------------------------
  28
  29 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  30 SENTRY
  31 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  32
  33 #ifndef FGSENSORORIENTATION_H
  34 #define FGSENSORORIENTATION_H
  35
  36 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  37 INCLUDES
  38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  39
  40 #include "FGSensor.h"
  41 #include "input_output/FGXMLElement.h"
  42 #include "math/FGColumnVector3.h"
  43 #include "math/FGMatrix33.h"
  44
  45 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  46 DEFINITIONS
  47 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  48
  49 #define ID_SensorOrientation "$Id$"
  50
  51 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  52 FORWARD DECLARATIONS
  53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  54
  55 namespace JSBSim {
  56
  57 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  58 CLASS DOCUMENTATION
  59 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  60
  61 /** Encapsulates a SensorOrientation capability for a sensor.
  62
  63 Syntax:
  64
  65 @author Jon S. Berndt
  66 @version $Revision$
  67 */
  68
  69 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  70 CLASS DECLARATION
  71 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  72
  73 class FGSensorOrientation  : public FGJSBBase
  74 {
  75 public:
  76   FGSensorOrientation(Element* element)
  77   {
  78     Element* orient_element = element->FindElement("orientation");
  79     if (orient_element) vOrient = orient_element->FindElementTripletConvertTo("RAD");
  80     else {cerr << "No orientation given for this sensor. " << endl;}
  81
  82     Element* axis_element = element->FindElement("axis");
  83     if (axis_element) {
  84       string sAxis = element->FindElementValue("axis");
  85       if (sAxis == "X" || sAxis == "x") {
  86         axis = 1;
  87       } else if (sAxis == "Y" || sAxis == "y") {
  88         axis = 2;
  89       } else if (sAxis == "Z" || sAxis == "z") {
  90         axis = 3;
  91       } else {
  92         cerr << "  Incorrect/no axis specified for this sensor; assuming X axis" << endl;
  93         axis = 1;
  94       }
  95     }
  96
  97     CalculateTransformMatrix();
  98   }
  99
 100 //  ~FGSensorOrientation();
 101
 102 protected:
 103   FGColumnVector3 vOrient;
 104   FGMatrix33 mT;
 105   int axis;
 106   void CalculateTransformMatrix(void)
 107   {
 108     double cp,sp,cr,sr,cy,sy;
 109
 110     cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
 111     cr=cos(vOrient(eRoll));  sr=sin(vOrient(eRoll));
 112     cy=cos(vOrient(eYaw));   sy=sin(vOrient(eYaw));
 113
 114     mT(1,1) =  cp*cy;
 115     mT(1,2) =  cp*sy;
 116     mT(1,3) = -sp;
 117
 118     mT(2,1) = sr*sp*cy - cr*sy;
 119     mT(2,2) = sr*sp*sy + cr*cy;
 120     mT(2,3) = sr*cp;
 121
 122     mT(3,1) = cr*sp*cy + sr*sy;
 123     mT(3,2) = cr*sp*sy - sr*cy;
 124     mT(3,3) = cr*cp;
 125
 126     // This transform is different than for FGForce, where we want a native nozzle
 127     // force in body frame. Here we calculate the body frame accel and want it in
 128     // the transformed accelerometer frame. So, the next line is commented out.
 129     // mT = mT.Inverse();
 130   }
 131
 132 private:
 133   void Debug(int from);
 134 };
 135 }
 136 #endif