Branch data Line data Source code
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 : 0 : {
77 : : public:
78 : 0 : FGSensorOrientation(Element* element)
79 : 0 : {
80 : 0 : Element* orient_element = element->FindElement("orientation");
81 [ # # ]: 0 : if (orient_element) vOrient = orient_element->FindElementTripletConvertTo("RAD");
82 : 0 : else { std::cerr << "No orientation given for this sensor. " << std::endl;}
83 : :
84 : 0 : Element* axis_element = element->FindElement("axis");
85 [ # # ]: 0 : if (axis_element) {
86 : 0 : string sAxis = element->FindElementValue("axis");
87 [ # # # # ]: 0 : if (sAxis == "X" || sAxis == "x") {
[ # # ]
88 : 0 : axis = 1;
89 [ # # # # ]: 0 : } else if (sAxis == "Y" || sAxis == "y") {
[ # # ]
90 : 0 : axis = 2;
91 [ # # # # ]: 0 : } else if (sAxis == "Z" || sAxis == "z") {
[ # # ]
92 : 0 : axis = 3;
93 : : } else {
94 : 0 : std::cerr << " Incorrect/no axis specified for this sensor; assuming X axis" << std::endl;
95 : 0 : axis = 1;
96 : 0 : }
97 : : }
98 : :
99 : : CalculateTransformMatrix();
100 : 0 : }
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 : 0 : cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
113 : 0 : cr=cos(vOrient(eRoll)); sr=sin(vOrient(eRoll));
114 : 0 : cy=cos(vOrient(eYaw)); sy=sin(vOrient(eYaw));
115 : :
116 : 0 : mT(1,1) = cp*cy;
117 : 0 : mT(1,2) = cp*sy;
118 : 0 : mT(1,3) = -sp;
119 : :
120 : 0 : mT(2,1) = sr*sp*cy - cr*sy;
121 : 0 : mT(2,2) = sr*sp*sy + cr*cy;
122 : 0 : mT(2,3) = sr*cp;
123 : :
124 : 0 : mT(3,1) = cr*sp*cy + sr*sy;
125 : 0 : mT(3,2) = cr*sp*sy - sr*cy;
126 : 0 : 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
|
