1 /***************************************************************************
5 ----------------------------------------------------------------------------
7 FUNCTION: Converts geocentric coordinates to geodetic positions
9 ----------------------------------------------------------------------------
11 MODULE STATUS: developmental
13 ----------------------------------------------------------------------------
15 GENEALOGY: Written as part of LaRCSim project by E. B. Jackson
17 ----------------------------------------------------------------------------
19 DESIGNED BY: E. B. Jackson
21 CODED BY: E. B. Jackson
23 MAINTAINED BY: E. B. Jackson
25 ----------------------------------------------------------------------------
31 930208 Modified to avoid singularity near polar region. EBJ
32 930602 Moved backwards calcs here from ls_step. EBJ
33 931214 Changed erroneous Latitude and Altitude variables to
34 *lat_geod and *alt in routine ls_geoc_to_geod. EBJ
35 940111 Changed header files from old ls_eom.h style to ls_types,
36 and ls_constants. Also replaced old DATA type with new
43 Revision 1.2 2000/10/23 22:34:54 curt
45 LaRCsim c172 on-ground and in-air starts, reset: all work
46 UIUC Cessna172 on-ground and in-air starts work as expected, reset
47 results in an aircraft that is upside down but does not crash FG. I
48 don't know what it was like before, so it may well be no change.
49 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
50 trimmed), on-ground starts do not -- the c172 ends up on its back. I
51 suspect this is no worse than before.
54 Balloon (the weather code returns nan's for the atmosphere data --this
55 is in the weather module and apparently is a linux only bug)
57 MagicCarpet (needs work yet)
58 External (don't know how)
61 LaRCsim c172 on-ground starts with a negative terrain altitude (this
62 happens at KPAO when the scenery is not present). The FDM inits to
63 about 50 feet AGL and the model falls to the ground. It does stay
64 upright, however, and seems to be fine once it settles out, FWIW.
67 --implement set_Model on the bus
68 --bring Christian's weather data into JSBSim
69 -- add default method to bus for updating things like the sin and cos of
70 latitude (for Balloon, MagicCarpet)
76 -- all data members now declared protected instead of private.
77 -- eliminated all but a small set of 'setters', no change to getters.
78 -- that small set is declared virtual, the default implementation
79 provided preserves the old behavior
80 -- all of the vector data members are now initialized.
81 -- added busdump() method -- FG_LOG's all the bus data when called,
82 useful for diagnostics.
85 -- bus data members now directly assigned to
88 -- bus data members now directly assigned to
89 -- changed V_equiv_kts to V_calibrated_kts
93 -- bus data members now directly assigned to
94 -- implemented the FGInterface virtual setters with JSBSim specific
96 -- changed the static FDMExec to a dynamic fdmex (needed so that the
97 JSBSim object can be deleted when a model change is called for)
98 -- implemented constructor and destructor, moved some of the logic
99 formerly in init() to constructor
100 -- added logic to bring up FGEngInterface objects and set the RPM and
105 -- bus data members now directly assigned to
106 -- implemented the FGInterface virtual setters with LaRCsim specific
107 logic, uses LaRCsimIC
108 -- implemented constructor and destructor, moved some of the logic
109 formerly in init() to constructor
110 -- moved default inertias to here from fg_init.cxx
111 -- eliminated the climb rate calculation. The equivalent, climb_rate =
112 -1*vdown, is now in copy_from_LaRCsim().
114 src/FDM/LaRCsimIC.cxx
115 src/FDM/LaRCsimIC.hxx
116 -- similar to FGInitialCondition, this class has all the logic needed to
117 turn data like Vc and Mach into the more fundamental quantities LaRCsim
119 -- put it in src/FDM since it is a class
121 src/FDM/MagicCarpet.cxx
122 -- bus data members now directly assigned to
125 -- adds LaRCsimIC.hxx and cxx
127 src/FDM/JSBSim/FGAtmosphere.h
128 src/FDM/JSBSim/FGDefs.h
129 src/FDM/JSBSim/FGInitialCondition.cpp
130 src/FDM/JSBSim/FGInitialCondition.h
131 src/FDM/JSBSim/JSBSim.cpp
132 -- changes to accomodate the new bus
134 src/FDM/LaRCsim/atmos_62.h
135 src/FDM/LaRCsim/ls_geodesy.h
136 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
137 here are needed in LaRCsimIC
139 src/FDM/LaRCsim/c172_main.c
140 src/FDM/LaRCsim/cherokee_aero.c
141 src/FDM/LaRCsim/ls_aux.c
142 src/FDM/LaRCsim/ls_constants.h
143 src/FDM/LaRCsim/ls_geodesy.c
144 src/FDM/LaRCsim/ls_geodesy.h
145 src/FDM/LaRCsim/ls_step.c
146 src/FDM/UIUCModel/uiuc_betaprobe.cpp
147 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
150 src/FDM/LaRCsim/ls_interface.c
151 src/FDM/LaRCsim/ls_interface.h
152 -- added function ls_set_model_dt()
155 -- eliminated calls that set the NED speeds to body components. They
156 are no longer needed and confuse the new bus.
159 -- eliminated calls that just brought the bus data up-to-date (e.g.
160 set_sin_cos_latitude). or set default values. The bus now handles the
161 defaults and updates itself when the setters are called (for LaRCsim and
162 JSBSim). A default method for doing this needs to be added to the bus.
163 -- added fgVelocityInit() to set the speed the user asked for. Both
164 JSBSim and LaRCsim can now be initialized using any of:
165 vc,mach, NED components, UVW components.
168 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
169 onto the bus every update()
173 -- added enum to keep track of the speed requested by the user
174 -- eliminated calls to set NED velocity properties to body speeds, they
175 are no longer needed.
176 -- added options for the NED components.
178 src/Network/garmin.cxx
180 --eliminated calls that just brought the bus data up-to-date (e.g.
181 set_sin_cos_latitude). The bus now updates itself when the setters are
182 called (for LaRCsim and JSBSim). A default method for doing this needs
183 to be added to the bus.
184 -- changed set_V_equiv_kts to set_V_calibrated_kts. set_V_equiv_kts no
185 longer exists ( get_V_equiv_kts still does, though)
187 src/WeatherCM/FGLocalWeatherDatabase.cpp
188 -- commented out the code to put the weather data on the bus, a
189 different scheme for this is needed.
191 Revision 1.1.1.1 1999/06/17 18:07:34 curt
192 Start of 0.7.x branch
194 Revision 1.1.1.1 1999/04/05 21:32:45 curt
195 Start of 0.6.x branch.
197 Revision 1.3 1998/07/08 14:41:37 curt
200 Revision 1.2 1998/01/19 18:40:25 curt
201 Tons of little changes to clean up the code and to remove fatal errors
202 when building with the c++ compiler.
204 Revision 1.1 1997/05/29 00:09:56 curt
205 Initial Flight Gear revision.
207 * Revision 1.5 1994/01/11 18:47:05 bjax
208 * Changed include files to use types and constants, not ls_eom.h
209 * Also changed DATA type to SCALAR type.
211 * Revision 1.4 1993/12/14 21:06:47 bjax
212 * Removed global variable references Altitude and Latitude. EBJ
214 * Revision 1.3 1993/06/02 15:03:40 bjax
215 * Made new subroutine for calculating geodetic to geocentric; changed name
216 * of forward conversion routine from ls_geodesy to ls_geoc_to_geod.
219 ----------------------------------------------------------------------------
223 [ 1] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
224 Control and Simulation", Wiley and Sons, 1992.
228 ----------------------------------------------------------------------------
232 ----------------------------------------------------------------------------
236 ----------------------------------------------------------------------------
239 lat_geoc Geocentric latitude, radians, + = North
240 radius C.G. radius to earth center, ft
242 ----------------------------------------------------------------------------
245 lat_geod Geodetic latitude, radians, + = North
246 alt C.G. altitude above mean sea level, ft
247 sea_level_r radius from earth center to sea level at
248 local vertical (surface normal) of C.G.
250 --------------------------------------------------------------------------*/
252 #include "ls_types.h"
253 #include "ls_constants.h"
254 #include "ls_geodesy.h"
257 /* ONE_SECOND is pi/180/60/60, or about 100 feet at earths' equator */
258 #define ONE_SECOND 4.848136811E-6
259 #define HALF_PI 0.5*LS_PI
262 void ls_geoc_to_geod( SCALAR lat_geoc, SCALAR radius, SCALAR *lat_geod,
263 SCALAR *alt, SCALAR *sea_level_r )
265 SCALAR t_lat, x_alpha, mu_alpha, delt_mu, r_alpha, l_point, rho_alpha;
266 SCALAR sin_mu_a, denom,delt_lambda, lambda_sl, sin_lambda_sl;
268 if( ( (HALF_PI - lat_geoc) < ONE_SECOND ) /* near North pole */
269 || ( (HALF_PI + lat_geoc) < ONE_SECOND ) ) /* near South pole */
271 *lat_geod = lat_geoc;
272 *sea_level_r = EQUATORIAL_RADIUS*E;
273 *alt = radius - *sea_level_r;
277 t_lat = tan(lat_geoc);
278 x_alpha = E*EQUATORIAL_RADIUS/sqrt(t_lat*t_lat + E*E);
279 mu_alpha = atan2(sqrt(RESQ - x_alpha*x_alpha),E*x_alpha);
280 if (lat_geoc < 0) mu_alpha = - mu_alpha;
281 sin_mu_a = sin(mu_alpha);
282 delt_lambda = mu_alpha - lat_geoc;
283 r_alpha = x_alpha/cos(lat_geoc);
284 l_point = radius - r_alpha;
285 *alt = l_point*cos(delt_lambda);
286 denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a);
287 rho_alpha = EQUATORIAL_RADIUS*(1-EPS)/
289 delt_mu = atan2(l_point*sin(delt_lambda),rho_alpha + *alt);
290 *lat_geod = mu_alpha - delt_mu;
291 lambda_sl = atan( E*E * tan(*lat_geod) ); /* SL geoc. latitude */
292 sin_lambda_sl = sin( lambda_sl );
293 *sea_level_r = sqrt(RESQ
294 /(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
299 void ls_geod_to_geoc( SCALAR lat_geod, SCALAR alt,
300 SCALAR *sl_radius, SCALAR *lat_geoc )
302 SCALAR lambda_sl, sin_lambda_sl, cos_lambda_sl, sin_mu, cos_mu, px, py;
304 lambda_sl = atan( E*E * tan(lat_geod) ); /* sea level geocentric latitude */
305 sin_lambda_sl = sin( lambda_sl );
306 cos_lambda_sl = cos( lambda_sl );
307 sin_mu = sin(lat_geod); /* Geodetic (map makers') latitude */
308 cos_mu = cos(lat_geod);
309 *sl_radius = sqrt(RESQ
310 /(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
311 py = *sl_radius*sin_lambda_sl + alt*sin_mu;
312 px = *sl_radius*cos_lambda_sl + alt*cos_mu;
313 *lat_geoc = atan2( py, px );