Branch data Line data Source code
1 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 : :
3 : : Module: FGInertial.cpp
4 : : Author: Jon S. Berndt
5 : : Date started: 09/13/00
6 : : Purpose: Encapsulates the inertial frame forces (coriolis and centrifugal)
7 : :
8 : : ------------- Copyright (C) 2000 Jon S. Berndt (jon@jsbsim.org) -------------
9 : :
10 : : This program is free software; you can redistribute it and/or modify it under
11 : : the terms of the GNU Lesser General Public License as published by the Free Software
12 : : Foundation; either version 2 of the License, or (at your option) any later
13 : : version.
14 : :
15 : : This program is distributed in the hope that it will be useful, but WITHOUT
16 : : ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
17 : : FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
18 : : details.
19 : :
20 : : You should have received a copy of the GNU Lesser General Public License along with
21 : : this program; if not, write to the Free Software Foundation, Inc., 59 Temple
22 : : Place - Suite 330, Boston, MA 02111-1307, USA.
23 : :
24 : : Further information about the GNU Lesser General Public License can also be found on
25 : : the world wide web at http://www.gnu.org.
26 : :
27 : : FUNCTIONAL DESCRIPTION
28 : : --------------------------------------------------------------------------------
29 : :
30 : : HISTORY
31 : : --------------------------------------------------------------------------------
32 : : 09/13/00 JSB Created
33 : :
34 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
35 : : INCLUDES
36 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
37 : :
38 : : #include "FGInertial.h"
39 : : #include "FGFDMExec.h"
40 : : #include "FGPropagate.h"
41 : : #include "FGMassBalance.h"
42 : : #include <iostream>
43 : :
44 : : using namespace std;
45 : :
46 : : namespace JSBSim {
47 : :
48 : : static const char *IdSrc = "$Id: FGInertial.cpp,v 1.18 2010/03/28 05:57:00 jberndt Exp $";
49 : : static const char *IdHdr = ID_INERTIAL;
50 : :
51 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
52 : : CLASS IMPLEMENTATION
53 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
54 : :
55 : :
56 : 1 : FGInertial::FGInertial(FGFDMExec* fgex) : FGModel(fgex)
57 : : {
58 : 1 : Name = "FGInertial";
59 : :
60 : : // Earth defaults
61 : 1 : RotationRate = 0.00007292115;
62 : 1 : GM = 14.07644180E15; // WGS84 value
63 : 1 : RadiusReference = 20925650.00; // Equatorial radius (WGS84)
64 : 1 : C2_0 = -4.84165371736E-04; // WGS84 value for the C2,0 coefficient
65 : 1 : J2 = 1.0826266836E-03; // WGS84 value for J2
66 : 1 : a = 20925646.3255; // WGS84 semimajor axis length in feet
67 : 1 : b = 20855486.5951; // WGS84 semiminor axis length in feet
68 : 1 : earthPosAngle = 0.0;
69 : :
70 : : // Lunar defaults
71 : : /*
72 : : RotationRate = 0.0000026617;
73 : : GM = 1.7314079E14; // Lunar GM
74 : : RadiusReference = 5702559.05; // Equatorial radius
75 : : C2_0 = 0; // value for the C2,0 coefficient
76 : : J2 = 2.033542482111609E-4; // value for J2
77 : : a = 5702559.05; // semimajor axis length in feet
78 : : b = 5695439.63; // semiminor axis length in feet
79 : : earthPosAngle = 0.0;
80 : : */
81 : :
82 : 1 : gAccelReference = GM/(RadiusReference*RadiusReference);
83 : 1 : gAccel = GM/(RadiusReference*RadiusReference);
84 : :
85 : 1 : bind();
86 : :
87 : 1 : Debug(0);
88 : 1 : }
89 : :
90 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
91 : :
92 : 1 : FGInertial::~FGInertial(void)
93 : : {
94 : 1 : Debug(1);
95 : 1 : }
96 : :
97 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
98 : :
99 : 1 : bool FGInertial::InitModel(void)
100 : : {
101 [ - + ]: 1 : if (!FGModel::InitModel()) return false;
102 : :
103 : 1 : earthPosAngle = 0.0;
104 : :
105 : 1 : return true;
106 : : }
107 : :
108 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
109 : :
110 : 54005 : bool FGInertial::Run(void)
111 : : {
112 : : // Fast return if we have nothing to do ...
113 [ - + ]: 54005 : if (FGModel::Run()) return true;
114 [ - + ]: 54005 : if (FDMExec->Holding()) return false;
115 : :
116 : 54005 : RunPreFunctions();
117 : :
118 : : // Gravitation accel
119 : 108010 : double r = Propagate->GetRadius();
120 : 54005 : gAccel = GetGAccel(r);
121 : 108010 : earthPosAngle += FDMExec->GetDeltaT()*RotationRate;
122 : :
123 : 54005 : RunPostFunctions();
124 : :
125 : 54005 : return false;
126 : : }
127 : :
128 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
129 : :
130 : 0 : double FGInertial::GetGAccel(double r) const
131 : : {
132 : 0 : return GM/(r*r);
133 : : }
134 : :
135 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
136 : : //
137 : : // Calculate the WGS84 gravitation value in ECEF frame. Pass in the ECEF position
138 : : // via the position parameter. The J2Gravity value returned is in ECEF frame,
139 : : // and therefore may need to be expressed (transformed) in another frame,
140 : : // depending on how it is used. See Stevens and Lewis eqn. 1.4-16.
141 : :
142 : 54006 : FGColumnVector3 FGInertial::GetGravityJ2(FGColumnVector3 position) const
143 : : {
144 : 54006 : FGColumnVector3 J2Gravity;
145 : :
146 : : // Gravitation accel
147 : 54006 : double r = position.Magnitude();
148 : 108012 : double lat = Propagate->GetLatitude();
149 : 54006 : double sinLat = sin(lat);
150 : :
151 : 54006 : double preCommon = 1.5*J2*(a/r)*(a/r);
152 : 54006 : double xy = 1.0 - 5.0*(sinLat*sinLat);
153 : 54006 : double z = 3.0 - 5.0*(sinLat*sinLat);
154 : 54006 : double GMOverr2 = GM/(r*r);
155 : :
156 : 162018 : J2Gravity(1) = -GMOverr2 * ((1.0 + (preCommon * xy)) * position(eX)/r);
157 : 162018 : J2Gravity(2) = -GMOverr2 * ((1.0 + (preCommon * xy)) * position(eY)/r);
158 : 54006 : J2Gravity(3) = -GMOverr2 * ((1.0 + (preCommon * z)) * position(eZ)/r);
159 : :
160 : 0 : return J2Gravity;
161 : : }
162 : :
163 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
164 : :
165 : 1 : void FGInertial::bind(void)
166 : : {
167 : 2 : PropertyManager->Tie("position/epa-rad", this, &FGInertial::GetEarthPositionAngle);
168 : 1 : }
169 : :
170 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
171 : : // The bitmasked value choices are as follows:
172 : : // unset: In this case (the default) JSBSim would only print
173 : : // out the normally expected messages, essentially echoing
174 : : // the config files as they are read. If the environment
175 : : // variable is not set, debug_lvl is set to 1 internally
176 : : // 0: This requests JSBSim not to output any messages
177 : : // whatsoever.
178 : : // 1: This value explicity requests the normal JSBSim
179 : : // startup messages
180 : : // 2: This value asks for a message to be printed out when
181 : : // a class is instantiated
182 : : // 4: When this value is set, a message is displayed when a
183 : : // FGModel object executes its Run() method
184 : : // 8: When this value is set, various runtime state variables
185 : : // are printed out periodically
186 : : // 16: When set various parameters are sanity checked and
187 : : // a message is printed out when they go out of bounds
188 : :
189 : 2 : void FGInertial::Debug(int from)
190 : : {
191 [ + - ]: 2 : if (debug_lvl <= 0) return;
192 : :
193 : 2 : if (debug_lvl & 1) { // Standard console startup message output
194 : : if (from == 0) { // Constructor
195 : :
196 : : }
197 : : }
198 [ - + ]: 2 : if (debug_lvl & 2 ) { // Instantiation/Destruction notification
199 [ # # ]: 0 : if (from == 0) cout << "Instantiated: FGInertial" << endl;
200 [ # # ]: 0 : if (from == 1) cout << "Destroyed: FGInertial" << endl;
201 : : }
202 : 2 : if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
203 : : }
204 : 2 : if (debug_lvl & 8 ) { // Runtime state variables
205 : : }
206 : 2 : if (debug_lvl & 16) { // Sanity checking
207 : : }
208 [ - + ]: 2 : if (debug_lvl & 64) {
209 [ # # ]: 0 : if (from == 0) { // Constructor
210 : 0 : cout << IdSrc << endl;
211 : 0 : cout << IdHdr << endl;
212 : : }
213 : : }
214 : : }
215 [ + + ][ + - ]: 12 : }
|
