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.3 2001/03/24 05:03:12 curt
46 Revision 1.2 2000/10/23 22:34:54 curt
48 LaRCsim c172 on-ground and in-air starts, reset: all work
49 UIUC Cessna172 on-ground and in-air starts work as expected, reset
50 results in an aircraft that is upside down but does not crash FG. I
51 don't know what it was like before, so it may well be no change.
52 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
53 trimmed), on-ground starts do not -- the c172 ends up on its back. I
54 suspect this is no worse than before.
57 Balloon (the weather code returns nan's for the atmosphere data --this
58 is in the weather module and apparently is a linux only bug)
60 MagicCarpet (needs work yet)
61 External (don't know how)
64 LaRCsim c172 on-ground starts with a negative terrain altitude (this
65 happens at KPAO when the scenery is not present). The FDM inits to
66 about 50 feet AGL and the model falls to the ground. It does stay
67 upright, however, and seems to be fine once it settles out, FWIW.
70 --implement set_Model on the bus
71 --bring Christian's weather data into JSBSim
72 -- add default method to bus for updating things like the sin and cos of
73 latitude (for Balloon, MagicCarpet)
79 -- all data members now declared protected instead of private.
80 -- eliminated all but a small set of 'setters', no change to getters.
81 -- that small set is declared virtual, the default implementation
82 provided preserves the old behavior
83 -- all of the vector data members are now initialized.
84 -- added busdump() method -- SG_LOG's all the bus data when called,
85 useful for diagnostics.
88 -- bus data members now directly assigned to
91 -- bus data members now directly assigned to
92 -- changed V_equiv_kts to V_calibrated_kts
96 -- bus data members now directly assigned to
97 -- implemented the FGInterface virtual setters with JSBSim specific
99 -- changed the static FDMExec to a dynamic fdmex (needed so that the
100 JSBSim object can be deleted when a model change is called for)
101 -- implemented constructor and destructor, moved some of the logic
102 formerly in init() to constructor
103 -- added logic to bring up FGEngInterface objects and set the RPM and
108 -- bus data members now directly assigned to
109 -- implemented the FGInterface virtual setters with LaRCsim specific
110 logic, uses LaRCsimIC
111 -- implemented constructor and destructor, moved some of the logic
112 formerly in init() to constructor
113 -- moved default inertias to here from fg_init.cxx
114 -- eliminated the climb rate calculation. The equivalent, climb_rate =
115 -1*vdown, is now in copy_from_LaRCsim().
117 src/FDM/LaRCsimIC.cxx
118 src/FDM/LaRCsimIC.hxx
119 -- similar to FGInitialCondition, this class has all the logic needed to
120 turn data like Vc and Mach into the more fundamental quantities LaRCsim
122 -- put it in src/FDM since it is a class
124 src/FDM/MagicCarpet.cxx
125 -- bus data members now directly assigned to
128 -- adds LaRCsimIC.hxx and cxx
130 src/FDM/JSBSim/FGAtmosphere.h
131 src/FDM/JSBSim/FGDefs.h
132 src/FDM/JSBSim/FGInitialCondition.cpp
133 src/FDM/JSBSim/FGInitialCondition.h
134 src/FDM/JSBSim/JSBSim.cpp
135 -- changes to accomodate the new bus
137 src/FDM/LaRCsim/atmos_62.h
138 src/FDM/LaRCsim/ls_geodesy.h
139 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
140 here are needed in LaRCsimIC
142 src/FDM/LaRCsim/c172_main.c
143 src/FDM/LaRCsim/cherokee_aero.c
144 src/FDM/LaRCsim/ls_aux.c
145 src/FDM/LaRCsim/ls_constants.h
146 src/FDM/LaRCsim/ls_geodesy.c
147 src/FDM/LaRCsim/ls_geodesy.h
148 src/FDM/LaRCsim/ls_step.c
149 src/FDM/UIUCModel/uiuc_betaprobe.cpp
150 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
153 src/FDM/LaRCsim/ls_interface.c
154 src/FDM/LaRCsim/ls_interface.h
155 -- added function ls_set_model_dt()
158 -- eliminated calls that set the NED speeds to body components. They
159 are no longer needed and confuse the new bus.
162 -- eliminated calls that just brought the bus data up-to-date (e.g.
163 set_sin_cos_latitude). or set default values. The bus now handles the
164 defaults and updates itself when the setters are called (for LaRCsim and
165 JSBSim). A default method for doing this needs to be added to the bus.
166 -- added fgVelocityInit() to set the speed the user asked for. Both
167 JSBSim and LaRCsim can now be initialized using any of:
168 vc,mach, NED components, UVW components.
171 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
172 onto the bus every update()
176 -- added enum to keep track of the speed requested by the user
177 -- eliminated calls to set NED velocity properties to body speeds, they
178 are no longer needed.
179 -- added options for the NED components.
181 src/Network/garmin.cxx
183 --eliminated calls that just brought the bus data up-to-date (e.g.
184 set_sin_cos_latitude). The bus now updates itself when the setters are
185 called (for LaRCsim and JSBSim). A default method for doing this needs
186 to be added to the bus.
187 -- changed set_V_equiv_kts to set_V_calibrated_kts. set_V_equiv_kts no
188 longer exists ( get_V_equiv_kts still does, though)
190 src/WeatherCM/FGLocalWeatherDatabase.cpp
191 -- commented out the code to put the weather data on the bus, a
192 different scheme for this is needed.
194 Revision 1.1.1.1 1999/06/17 18:07:34 curt
195 Start of 0.7.x branch
197 Revision 1.1.1.1 1999/04/05 21:32:45 curt
198 Start of 0.6.x branch.
200 Revision 1.3 1998/07/08 14:41:37 curt
203 Revision 1.2 1998/01/19 18:40:25 curt
204 Tons of little changes to clean up the code and to remove fatal errors
205 when building with the c++ compiler.
207 Revision 1.1 1997/05/29 00:09:56 curt
208 Initial Flight Gear revision.
210 * Revision 1.5 1994/01/11 18:47:05 bjax
211 * Changed include files to use types and constants, not ls_eom.h
212 * Also changed DATA type to SCALAR type.
214 * Revision 1.4 1993/12/14 21:06:47 bjax
215 * Removed global variable references Altitude and Latitude. EBJ
217 * Revision 1.3 1993/06/02 15:03:40 bjax
218 * Made new subroutine for calculating geodetic to geocentric; changed name
219 * of forward conversion routine from ls_geodesy to ls_geoc_to_geod.
222 ----------------------------------------------------------------------------
226 [ 1] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
227 Control and Simulation", Wiley and Sons, 1992.
231 ----------------------------------------------------------------------------
235 ----------------------------------------------------------------------------
239 ----------------------------------------------------------------------------
242 lat_geoc Geocentric latitude, radians, + = North
243 radius C.G. radius to earth center, ft
245 ----------------------------------------------------------------------------
248 lat_geod Geodetic latitude, radians, + = North
249 alt C.G. altitude above mean sea level, ft
250 sea_level_r radius from earth center to sea level at
251 local vertical (surface normal) of C.G.
253 --------------------------------------------------------------------------*/
255 #include "ls_types.h"
256 #include "ls_constants.h"
257 #include "ls_geodesy.h"
260 /* ONE_SECOND is pi/180/60/60, or about 100 feet at earths' equator */
261 #define ONE_SECOND 4.848136811E-6
262 #define HALF_PI 0.5*LS_PI
265 void ls_geoc_to_geod( SCALAR lat_geoc, SCALAR radius, SCALAR *lat_geod,
266 SCALAR *alt, SCALAR *sea_level_r )
268 SCALAR t_lat, x_alpha, mu_alpha, delt_mu, r_alpha, l_point, rho_alpha;
269 SCALAR sin_mu_a, denom,delt_lambda, lambda_sl, sin_lambda_sl;
271 if( ( (HALF_PI - lat_geoc) < ONE_SECOND ) /* near North pole */
272 || ( (HALF_PI + lat_geoc) < ONE_SECOND ) ) /* near South pole */
274 *lat_geod = lat_geoc;
275 *sea_level_r = EQUATORIAL_RADIUS*E;
276 *alt = radius - *sea_level_r;
280 t_lat = tan(lat_geoc);
281 x_alpha = E*EQUATORIAL_RADIUS/sqrt(t_lat*t_lat + E*E);
282 mu_alpha = atan2(sqrt(RESQ - x_alpha*x_alpha),E*x_alpha);
283 if (lat_geoc < 0) mu_alpha = - mu_alpha;
284 sin_mu_a = sin(mu_alpha);
285 delt_lambda = mu_alpha - lat_geoc;
286 r_alpha = x_alpha/cos(lat_geoc);
287 l_point = radius - r_alpha;
288 *alt = l_point*cos(delt_lambda);
289 denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a);
290 rho_alpha = EQUATORIAL_RADIUS*(1-EPS)/
292 delt_mu = atan2(l_point*sin(delt_lambda),rho_alpha + *alt);
293 *lat_geod = mu_alpha - delt_mu;
294 lambda_sl = atan( E*E * tan(*lat_geod) ); /* SL geoc. latitude */
295 sin_lambda_sl = sin( lambda_sl );
296 *sea_level_r = sqrt(RESQ
297 /(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
302 void ls_geod_to_geoc( SCALAR lat_geod, SCALAR alt,
303 SCALAR *sl_radius, SCALAR *lat_geoc )
305 SCALAR lambda_sl, sin_lambda_sl, cos_lambda_sl, sin_mu, cos_mu, px, py;
307 lambda_sl = atan( E*E * tan(lat_geod) ); /* sea level geocentric latitude */
308 sin_lambda_sl = sin( lambda_sl );
309 cos_lambda_sl = cos( lambda_sl );
310 sin_mu = sin(lat_geod); /* Geodetic (map makers') latitude */
311 cos_mu = cos(lat_geod);
312 *sl_radius = sqrt(RESQ
313 /(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
314 py = *sl_radius*sin_lambda_sl + alt*sin_mu;
315 px = *sl_radius*cos_lambda_sl + alt*cos_mu;
316 *lat_geoc = atan2( py, px );