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
11 Software Foundation; either version 2 of the License, or (at your option) any
12 later 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
20 with this program; if not, write to the Free Software Foundation, Inc., 59
21 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
22
23 Further information about the GNU Lesser General Public License can also be
24 found on 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 "models/FGAccelerations.h"
42 #include "models/FGMassBalance.h"
43 #include "models/FGFCS.h"
44
45 using namespace std;
46
47 namespace JSBSim {
48
49 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
50 CLASS IMPLEMENTATION
51 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
52
FGAccelerometer(FGFCS * fcs,Element * element)53 FGAccelerometer::FGAccelerometer(FGFCS* fcs, Element* element)
54 : FGSensor(fcs, element),
55 FGSensorOrientation(element)
56 {
57 Propagate = fcs->GetExec()->GetPropagate();
58 Accelerations = fcs->GetExec()->GetAccelerations();
59 MassBalance = fcs->GetExec()->GetMassBalance();
60
61 Element* location_element = element->FindElement("location");
62 if (location_element)
63 vLocation = location_element->FindElementTripletConvertTo("IN");
64 else {
65 cerr << element->ReadFrom()
66 << "No location given for accelerometer. " << endl;
67 throw("Malformed accelerometer specification");
68 }
69
70 vRadius = MassBalance->StructuralToBody(vLocation);
71
72 Debug(0);
73 }
74
75 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
76
~FGAccelerometer()77 FGAccelerometer::~FGAccelerometer()
78 {
79 Debug(1);
80 }
81
82 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
83
Run(void)84 bool FGAccelerometer::Run(void )
85 {
86 // There is no input assumed. This is a dedicated acceleration sensor.
87
88 vRadius = MassBalance->StructuralToBody(vLocation);
89
90 //aircraft forces
91 vAccel = (Accelerations->GetBodyAccel()
92 + Accelerations->GetPQRidot() * vRadius
93 + Propagate->GetPQRi() * (Propagate->GetPQRi() * vRadius));
94
95 // transform to the specified orientation
96 vAccel = mT * vAccel;
97
98 Input = vAccel(axis);
99
100 ProcessSensorSignal();
101
102 SetOutput();
103
104 return true;
105 }
106
107 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
108 // The bitmasked value choices are as follows:
109 // unset: In this case (the default) JSBSim would only print
110 // out the normally expected messages, essentially echoing
111 // the config files as they are read. If the environment
112 // variable is not set, debug_lvl is set to 1 internally
113 // 0: This requests JSBSim not to output any messages
114 // whatsoever.
115 // 1: This value explicity requests the normal JSBSim
116 // startup messages
117 // 2: This value asks for a message to be printed out when
118 // a class is instantiated
119 // 4: When this value is set, a message is displayed when a
120 // FGModel object executes its Run() method
121 // 8: When this value is set, various runtime state variables
122 // are printed out periodically
123 // 16: When set various parameters are sanity checked and
124 // a message is printed out when they go out of bounds
125
Debug(int from)126 void FGAccelerometer::Debug(int from)
127 {
128 string ax[4] = {"none", "X", "Y", "Z"};
129
130 if (debug_lvl <= 0) return;
131
132 if (debug_lvl & 1) { // Standard console startup message output
133 if (from == 0) { // Constructor
134 cout << " Axis: " << ax[axis] << endl;
135 }
136 }
137 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
138 if (from == 0) cout << "Instantiated: FGAccelerometer" << endl;
139 if (from == 1) cout << "Destroyed: FGAccelerometer" << endl;
140 }
141 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
142 }
143 if (debug_lvl & 8 ) { // Runtime state variables
144 }
145 if (debug_lvl & 16) { // Sanity checking
146 }
147 if (debug_lvl & 64) {
148 if (from == 0) { // Constructor
149 }
150 }
151 }
152 }
153