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