]> git.mxchange.org Git - flightgear.git/blob - src/FDM/LaRCsim/ls_geodesy.c
Adds a basic FDM model for LaRCsim debugging purposes.
[flightgear.git] / src / FDM / LaRCsim / ls_geodesy.c
1 /***************************************************************************
2
3         TITLE:  ls_geodesy
4         
5 ----------------------------------------------------------------------------
6
7         FUNCTION:       Converts geocentric coordinates to geodetic positions
8
9 ----------------------------------------------------------------------------
10
11         MODULE STATUS:  developmental
12
13 ----------------------------------------------------------------------------
14
15         GENEALOGY:      Written as part of LaRCSim project by E. B. Jackson
16
17 ----------------------------------------------------------------------------
18
19         DESIGNED BY:    E. B. Jackson
20         
21         CODED BY:       E. B. Jackson
22         
23         MAINTAINED BY:  E. B. Jackson
24
25 ----------------------------------------------------------------------------
26
27         MODIFICATION HISTORY:
28         
29         DATE    PURPOSE                                         BY
30         
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
37                 SCALAR type.                                            EBJ
38
39         CURRENT RCS HEADER:
40
41 $Header$
42 $Log$
43 Revision 1.1  2002/09/10 01:14:02  curt
44 Initial revision
45
46 Revision 1.3  2001/03/24 05:03:12  curt
47 SG-ified logstream.
48
49 Revision 1.2  2000/10/23 22:34:54  curt
50 I tested:
51 LaRCsim c172 on-ground and in-air starts, reset: all work
52 UIUC Cessna172 on-ground and in-air starts work as expected, reset
53 results in an aircraft that is upside down but does not crash FG.   I
54 don't know what it was like before, so it may well be no change.
55 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
56 trimmed), on-ground starts do not -- the c172 ends up on its back.  I
57 suspect this is no worse than before.
58
59 I did not test:
60 Balloon (the weather code returns nan's for the atmosphere data --this
61 is in the weather module and apparently is a linux only bug)
62 ADA (don't know how)
63 MagicCarpet  (needs work yet)
64 External (don't know how)
65
66 known to be broken:
67 LaRCsim c172 on-ground starts with a negative terrain altitude (this
68 happens at KPAO when the scenery is not present).   The FDM inits to
69 about 50 feet AGL and the model falls to the ground.  It does stay
70 upright, however, and seems to be fine once it settles out, FWIW.
71
72 To do:
73 --implement set_Model on the bus
74 --bring Christian's weather data into JSBSim
75 -- add default method to bus for updating things like the sin and cos of
76 latitude (for Balloon, MagicCarpet)
77 -- lots of cleanup
78
79 The files:
80 src/FDM/flight.cxx
81 src/FDM/flight.hxx
82 -- all data members now declared protected instead of private.
83 -- eliminated all but a small set of 'setters', no change to getters.
84 -- that small set is declared virtual, the default implementation
85 provided preserves the old behavior
86 -- all of the vector data members are now initialized.
87 -- added busdump() method -- SG_LOG's  all the bus data when called,
88 useful for diagnostics.
89
90 src/FDM/ADA.cxx
91 -- bus data members now directly assigned to
92
93 src/FDM/Balloon.cxx
94 -- bus data members now directly assigned to
95 -- changed V_equiv_kts to V_calibrated_kts
96
97 src/FDM/JSBSim.cxx
98 src/FDM/JSBSim.hxx
99 -- bus data members now directly assigned to
100 -- implemented the FGInterface virtual setters with JSBSim specific
101 logic
102 -- changed the static FDMExec to a dynamic fdmex (needed so that the
103 JSBSim object can be deleted when a model change is called for)
104 -- implemented constructor and destructor, moved some of the logic
105 formerly in init() to constructor
106 -- added logic to bring up FGEngInterface objects and set the RPM and
107 throttle values.
108
109 src/FDM/LaRCsim.cxx
110 src/FDM/LaRCsim.hxx
111 -- bus data members now directly assigned to
112 -- implemented the FGInterface virtual setters with LaRCsim specific
113 logic, uses LaRCsimIC
114 -- implemented constructor and destructor, moved some of the logic
115 formerly in init() to constructor
116 -- moved default inertias to here from fg_init.cxx
117 -- eliminated the climb rate calculation.  The equivalent, climb_rate =
118 -1*vdown, is now in copy_from_LaRCsim().
119
120 src/FDM/LaRCsimIC.cxx
121 src/FDM/LaRCsimIC.hxx
122 -- similar to FGInitialCondition, this class has all the logic needed to
123 turn data like Vc and Mach into the more fundamental quantities LaRCsim
124 needs to initialize.
125 -- put it in src/FDM since it is a class
126
127 src/FDM/MagicCarpet.cxx
128  -- bus data members now directly assigned to
129
130 src/FDM/Makefile.am
131 -- adds LaRCsimIC.hxx and cxx
132
133 src/FDM/JSBSim/FGAtmosphere.h
134 src/FDM/JSBSim/FGDefs.h
135 src/FDM/JSBSim/FGInitialCondition.cpp
136 src/FDM/JSBSim/FGInitialCondition.h
137 src/FDM/JSBSim/JSBSim.cpp
138 -- changes to accomodate the new bus
139
140 src/FDM/LaRCsim/atmos_62.h
141 src/FDM/LaRCsim/ls_geodesy.h
142 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
143 here are needed in LaRCsimIC
144
145 src/FDM/LaRCsim/c172_main.c
146 src/FDM/LaRCsim/cherokee_aero.c
147 src/FDM/LaRCsim/ls_aux.c
148 src/FDM/LaRCsim/ls_constants.h
149 src/FDM/LaRCsim/ls_geodesy.c
150 src/FDM/LaRCsim/ls_geodesy.h
151 src/FDM/LaRCsim/ls_step.c
152 src/FDM/UIUCModel/uiuc_betaprobe.cpp
153 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
154 weather module
155
156 src/FDM/LaRCsim/ls_interface.c
157 src/FDM/LaRCsim/ls_interface.h
158 -- added function ls_set_model_dt()
159
160 src/Main/bfi.cxx
161 -- eliminated calls that set the NED speeds to body components.  They
162 are no longer needed and confuse the new bus.
163
164 src/Main/fg_init.cxx
165 -- eliminated calls that just brought the bus data up-to-date (e.g.
166 set_sin_cos_latitude). or set default values.   The bus now handles the
167 defaults and updates itself when the setters are called (for LaRCsim and
168 JSBSim).  A default method for doing this needs to be added to the bus.
169 -- added fgVelocityInit() to set the speed the user asked for.  Both
170 JSBSim and LaRCsim can now be initialized using any of:
171 vc,mach, NED components, UVW components.
172
173 src/Main/main.cxx
174 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
175 onto the bus every update()
176
177 src/Main/options.cxx
178 src/Main/options.hxx
179 -- added enum to keep track of the speed requested by the user
180 -- eliminated calls to set NED velocity properties to body speeds, they
181 are no longer needed.
182 -- added options for the NED components.
183
184 src/Network/garmin.cxx
185 src/Network/nmea.cxx
186 --eliminated calls that just brought the bus data up-to-date (e.g.
187 set_sin_cos_latitude).  The bus now updates itself when the setters are
188 called (for LaRCsim and JSBSim).  A default method for doing this needs
189 to be added to the bus.
190 -- changed set_V_equiv_kts to set_V_calibrated_kts.  set_V_equiv_kts no
191 longer exists ( get_V_equiv_kts still does, though)
192
193 src/WeatherCM/FGLocalWeatherDatabase.cpp
194 -- commented out the code to put the weather data on the bus, a
195 different scheme for this is needed.
196
197 Revision 1.1.1.1  1999/06/17 18:07:34  curt
198 Start of 0.7.x branch
199
200 Revision 1.1.1.1  1999/04/05 21:32:45  curt
201 Start of 0.6.x branch.
202
203 Revision 1.3  1998/07/08 14:41:37  curt
204 .
205
206 Revision 1.2  1998/01/19 18:40:25  curt
207 Tons of little changes to clean up the code and to remove fatal errors
208 when building with the c++ compiler.
209
210 Revision 1.1  1997/05/29 00:09:56  curt
211 Initial Flight Gear revision.
212
213  * Revision 1.5  1994/01/11  18:47:05  bjax
214  * Changed include files to use types and constants, not ls_eom.h
215  * Also changed DATA type to SCALAR type.
216  *
217  * Revision 1.4  1993/12/14  21:06:47  bjax
218  * Removed global variable references Altitude and Latitude.   EBJ
219  *
220  * Revision 1.3  1993/06/02  15:03:40  bjax
221  * Made new subroutine for calculating geodetic to geocentric; changed name
222  * of forward conversion routine from ls_geodesy to ls_geoc_to_geod.
223  *
224
225 ----------------------------------------------------------------------------
226
227         REFERENCES:
228
229                 [ 1]    Stevens, Brian L.; and Lewis, Frank L.: "Aircraft 
230                         Control and Simulation", Wiley and Sons, 1992.
231                         ISBN 0-471-61397-5                    
232
233
234 ----------------------------------------------------------------------------
235
236         CALLED BY:      ls_aux
237
238 ----------------------------------------------------------------------------
239
240         CALLS TO:
241
242 ----------------------------------------------------------------------------
243
244         INPUTS: 
245                 lat_geoc        Geocentric latitude, radians, + = North
246                 radius          C.G. radius to earth center, ft
247
248 ----------------------------------------------------------------------------
249
250         OUTPUTS:
251                 lat_geod        Geodetic latitude, radians, + = North
252                 alt             C.G. altitude above mean sea level, ft
253                 sea_level_r     radius from earth center to sea level at
254                                 local vertical (surface normal) of C.G.
255
256 --------------------------------------------------------------------------*/
257
258 #include "ls_types.h"
259 #include "ls_constants.h"
260 #include "ls_geodesy.h"
261 #include <math.h>
262
263 /* ONE_SECOND is pi/180/60/60, or about 100 feet at earths' equator */
264 #define ONE_SECOND 4.848136811E-6
265 #define HALF_PI 0.5*LS_PI
266
267
268 void ls_geoc_to_geod( SCALAR lat_geoc, SCALAR radius, SCALAR *lat_geod, 
269                       SCALAR *alt, SCALAR *sea_level_r )
270 {
271         SCALAR  t_lat, x_alpha, mu_alpha, delt_mu, r_alpha, l_point, rho_alpha;
272         SCALAR  sin_mu_a, denom,delt_lambda, lambda_sl, sin_lambda_sl;
273
274         if(   ( (HALF_PI - lat_geoc) < ONE_SECOND )     /* near North pole */
275            || ( (HALF_PI + lat_geoc) < ONE_SECOND ) )   /* near South pole */
276           {
277             *lat_geod = lat_geoc;
278             *sea_level_r = EQUATORIAL_RADIUS*E;
279             *alt = radius - *sea_level_r;
280           }
281         else
282           {
283             t_lat = tan(lat_geoc);
284             x_alpha = E*EQUATORIAL_RADIUS/sqrt(t_lat*t_lat + E*E);
285             mu_alpha = atan2(sqrt(RESQ - x_alpha*x_alpha),E*x_alpha);
286             if (lat_geoc < 0) mu_alpha = - mu_alpha;
287             sin_mu_a = sin(mu_alpha);
288             delt_lambda = mu_alpha - lat_geoc;
289             r_alpha = x_alpha/cos(lat_geoc);
290             l_point = radius - r_alpha;
291             *alt = l_point*cos(delt_lambda);
292             denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a);
293             rho_alpha = EQUATORIAL_RADIUS*(1-EPS)/
294               (denom*denom*denom);
295             delt_mu = atan2(l_point*sin(delt_lambda),rho_alpha + *alt);
296             *lat_geod = mu_alpha - delt_mu;
297             lambda_sl = atan( E*E * tan(*lat_geod) ); /* SL geoc. latitude */
298             sin_lambda_sl = sin( lambda_sl );
299             *sea_level_r = sqrt(RESQ
300                            /(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
301           }
302 }
303
304
305 void ls_geod_to_geoc( SCALAR lat_geod, SCALAR alt, 
306                       SCALAR *sl_radius, SCALAR *lat_geoc )
307 {
308     SCALAR lambda_sl, sin_lambda_sl, cos_lambda_sl, sin_mu, cos_mu, px, py;
309     
310     lambda_sl = atan( E*E * tan(lat_geod) ); /* sea level geocentric latitude */
311     sin_lambda_sl = sin( lambda_sl );
312     cos_lambda_sl = cos( lambda_sl );
313     sin_mu = sin(lat_geod);     /* Geodetic (map makers') latitude */
314     cos_mu = cos(lat_geod);
315     *sl_radius = sqrt(RESQ
316         /(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
317     py = *sl_radius*sin_lambda_sl + alt*sin_mu;
318     px = *sl_radius*cos_lambda_sl + alt*cos_mu;
319     *lat_geoc = atan2( py, px );
320 }