1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
5 Date started: 29 August 2009
7 ------------- Copyright (C) 2009 -------------
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
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
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.
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.
26 FUNCTIONAL DESCRIPTION
27 --------------------------------------------------------------------------------
30 --------------------------------------------------------------------------------
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33 COMMENTS, REFERENCES, and NOTES
34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
44 static const char *IdSrc = "$Id$";
45 static const char *IdHdr = ID_GYRO;
47 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
51 FGGyro::FGGyro(FGFCS* fcs, Element* element) : FGSensor(fcs, element)
53 Propagate = fcs->GetExec()->GetPropagate();
55 Element* orient_element = element->FindElement("orientation");
56 if (orient_element) vOrient = orient_element->FindElementTripletConvertTo("RAD");
57 else {cerr << "No orientation given for gyro. " << endl;}
59 Element* axis_element = element->FindElement("axis");
61 string sAxis = element->FindElementValue("axis");
62 if (sAxis == "ROLL" || sAxis == "roll") {
64 } else if (sAxis == "PITCH" || sAxis == "pitch") {
66 } else if (sAxis == "YAW" || sAxis == "yaw") {
69 cerr << " Incorrect/no axis specified for gyro; assuming Roll axis" << endl;
74 CalculateTransformMatrix();
79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
86 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
88 bool FGGyro::Run(void )
90 // There is no input assumed. This is a dedicated angular acceleration sensor.
93 vAccel = mT * Propagate->GetPQRdot();
97 Output = Input; // perfect gyro
99 // Degrade signal as specified
102 Output = PreviousOutput;
106 if (lag != 0.0) Lag(); // models gyro lag
107 if (noise_variance != 0.0) Noise(); // models noise
108 if (drift_rate != 0.0) Drift(); // models drift over time
109 if (bias != 0.0) Bias(); // models a finite bias
110 if (gain != 0.0) Gain(); // models a gain
112 if (fail_low) Output = -HUGE_VAL;
113 if (fail_high) Output = HUGE_VAL;
115 if (bits != 0) Quantize(); // models quantization degradation
116 // if (delay != 0.0) Delay(); // models system signal transport latencies
118 Clip(); // Is it right to clip a gyro?
122 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
124 void FGGyro::CalculateTransformMatrix(void)
126 double cp,sp,cr,sr,cy,sy;
128 cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
129 cr=cos(vOrient(eRoll)); sr=sin(vOrient(eRoll));
130 cy=cos(vOrient(eYaw)); sy=sin(vOrient(eYaw));
136 mT(2,1) = sr*sp*cy - cr*sy;
137 mT(2,2) = sr*sp*sy + cr*cy;
140 mT(3,1) = cr*sp*cy + sr*sy;
141 mT(3,2) = cr*sp*sy - sr*cy;
144 // This transform is different than for FGForce, where we want a native nozzle
145 // force in body frame. Here we calculate the body frame accel and want it in
146 // the transformed gyro frame. So, the next line is commented out.
147 // mT = mT.Inverse();
150 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
151 // The bitmasked value choices are as follows:
152 // unset: In this case (the default) JSBSim would only print
153 // out the normally expected messages, essentially echoing
154 // the config files as they are read. If the environment
155 // variable is not set, debug_lvl is set to 1 internally
156 // 0: This requests JSBSim not to output any messages
158 // 1: This value explicity requests the normal JSBSim
160 // 2: This value asks for a message to be printed out when
161 // a class is instantiated
162 // 4: When this value is set, a message is displayed when a
163 // FGModel object executes its Run() method
164 // 8: When this value is set, various runtime state variables
165 // are printed out periodically
166 // 16: When set various parameters are sanity checked and
167 // a message is printed out when they go out of bounds
169 void FGGyro::Debug(int from)
171 string ax[4] = {"none", "X", "Y", "Z"};
173 if (debug_lvl <= 0) return;
175 if (debug_lvl & 1) { // Standard console startup message output
176 if (from == 0) { // Constructor
177 cout << " Axis: " << ax[axis] << endl;
180 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
181 if (from == 0) cout << "Instantiated: FGGyro" << endl;
182 if (from == 1) cout << "Destroyed: FGGyro" << endl;
184 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
186 if (debug_lvl & 8 ) { // Runtime state variables
188 if (debug_lvl & 16) { // Sanity checking
190 if (debug_lvl & 64) {
191 if (from == 0) { // Constructor
192 cout << IdSrc << endl;
193 cout << IdHdr << endl;