]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp
Andreas Gaeb: fix #222 (JSBSIm reset problems)
[flightgear.git] / src / FDM / JSBSim / models / flight_control / FGAccelerometer.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3  Module:       FGAccelerometer.cpp
4  Author:       Jon Berndt
5  Date started: 9 July 2005
6
7  ------------- Copyright (C) 2005 -------------
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 "FGAccelerometer.h"
41 #include <iostream>
42 #include <cstdlib>
43
44 using namespace std;
45
46 namespace JSBSim {
47
48 static const char *IdSrc = "$Id: FGAccelerometer.cpp,v 1.8 2009/10/24 22:59:30 jberndt Exp $";
49 static const char *IdHdr = ID_ACCELEROMETER;
50
51 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
52 CLASS IMPLEMENTATION
53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
54
55 FGAccelerometer::FGAccelerometer(FGFCS* fcs, Element* element)
56   : FGSensor(fcs, element),
57     FGSensorOrientation(element)
58 {
59   Propagate = fcs->GetExec()->GetPropagate();
60   MassBalance = fcs->GetExec()->GetMassBalance();
61   Inertial = fcs->GetExec()->GetInertial();
62   
63   Element* location_element = element->FindElement("location");
64   if (location_element) vLocation = location_element->FindElementTripletConvertTo("IN");
65   else {cerr << "No location given for accelerometer. " << endl; exit(-1);}
66
67   vRadius = MassBalance->StructuralToBody(vLocation);
68
69   Debug(0);
70 }
71
72 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
73
74 FGAccelerometer::~FGAccelerometer()
75 {
76   Debug(1);
77 }
78
79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
80
81 bool FGAccelerometer::Run(void )
82 {
83   // There is no input assumed. This is a dedicated acceleration sensor.
84
85   vRadius = MassBalance->StructuralToBody(vLocation);
86     
87   //gravitational forces
88   vAccel = Propagate->GetTl2b() * FGColumnVector3(0, 0, Inertial->gravity());
89
90   //aircraft forces
91   vAccel += (Propagate->GetUVWdot()
92               + Propagate->GetPQRdot() * vRadius
93               + Propagate->GetPQR() * (Propagate->GetPQR() * vRadius));
94
95   // transform to the specified orientation
96   vAccel = mT * vAccel;
97
98   Input = vAccel(axis);
99
100   ProcessSensorSignal();
101
102   return true;
103 }
104
105 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
106 //    The bitmasked value choices are as follows:
107 //    unset: In this case (the default) JSBSim would only print
108 //       out the normally expected messages, essentially echoing
109 //       the config files as they are read. If the environment
110 //       variable is not set, debug_lvl is set to 1 internally
111 //    0: This requests JSBSim not to output any messages
112 //       whatsoever.
113 //    1: This value explicity requests the normal JSBSim
114 //       startup messages
115 //    2: This value asks for a message to be printed out when
116 //       a class is instantiated
117 //    4: When this value is set, a message is displayed when a
118 //       FGModel object executes its Run() method
119 //    8: When this value is set, various runtime state variables
120 //       are printed out periodically
121 //    16: When set various parameters are sanity checked and
122 //       a message is printed out when they go out of bounds
123
124 void FGAccelerometer::Debug(int from)
125 {
126   string ax[4] = {"none", "X", "Y", "Z"};
127
128   if (debug_lvl <= 0) return;
129
130   if (debug_lvl & 1) { // Standard console startup message output
131     if (from == 0) { // Constructor
132       cout << "        Axis: " << ax[axis] << endl;
133     }
134   }
135   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
136     if (from == 0) cout << "Instantiated: FGAccelerometer" << endl;
137     if (from == 1) cout << "Destroyed:    FGAccelerometer" << endl;
138   }
139   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
140   }
141   if (debug_lvl & 8 ) { // Runtime state variables
142   }
143   if (debug_lvl & 16) { // Sanity checking
144   }
145   if (debug_lvl & 64) {
146     if (from == 0) { // Constructor
147       cout << IdSrc << endl;
148       cout << IdHdr << endl;
149     }
150   }
151 }
152 }