]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/models/flight_control/FGSensorOrientation.h
Merge branch 'next' of git://gitorious.org/fg/flightgear into next
[flightgear.git] / 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