]> git.mxchange.org Git - flightgear.git/blob - src/FDM/LaRCsim/ls_aux.c
Adds a basic FDM model for LaRCsim debugging purposes.
[flightgear.git] / src / FDM / LaRCsim / ls_aux.c
1 /***************************************************************************
2
3     TITLE:              ls_aux
4                 
5 ----------------------------------------------------------------------------
6
7     FUNCTION:   Atmospheric and auxilary relationships for LaRCSim EOM
8
9 ----------------------------------------------------------------------------
10
11     MODULE STATUS:      developmental
12
13 ----------------------------------------------------------------------------
14
15     GENEALOGY:  Created 9208026 as part of C-castle simulation project.
16
17 ----------------------------------------------------------------------------
18
19     DESIGNED BY:        B. Jackson
20     
21     CODED BY:           B. Jackson
22     
23     MAINTAINED BY:      B. Jackson
24
25 ----------------------------------------------------------------------------
26
27     MODIFICATION HISTORY:
28     
29     DATE    PURPOSE     
30     
31     931006  Moved calculations of auxiliary accelerations from here
32             to ls_accel.c and corrected minus sign in front of A_Y_Pilot
33             contribution from Q_body*P_body*D_X_pilot term.         EBJ
34     931014  Changed calculation of Alpha from atan to atan2 so sign is correct.
35                                                                     EBJ
36     931220  Added calculations for static and total temperatures & pressures,
37             as well as dynamic and impact pressures and calibrated airspeed.
38                                                                     EBJ
39     940111  Changed #included header files from old "ls_eom.h" to newer
40             "ls_types.h", "ls_constants.h" and "ls_generic.h".      EBJ
41
42     950207  Changed use of "abs" to "fabs" in calculation of signU. EBJ
43     
44     950228  Fixed bug in calculation of beta_dot.                   EBJ
45
46     CURRENT RCS HEADER INFO:
47
48 $Header$
49 $Log$
50 Revision 1.4  2003/05/25 12:14:46  ehofman
51 Rename some defines to prevent a namespace clash
52
53 Revision 1.3  2003/05/13 18:45:06  curt
54 Robert Deters:
55
56   I have attached some revisions for the UIUCModel and some LaRCsim.
57   The only thing you should need to check is LaRCsim.cxx.  The file
58   I attached is a revised version of 1.5 and the latest is 1.7.  Also,
59   uiuc_getwind.c and uiuc_getwind.h are no longer in the LaRCsim
60   directory.  They have been moved over to UIUCModel.
61
62 Revision 1.2  2003/03/31 03:05:39  m-selig
63 uiuc wind changes, MSS
64
65 Revision 1.1.1.1  2003/02/28 01:33:39  rob
66 uiuc version of FlightGear v0.9.0
67
68 Revision 1.2  2002/10/22 21:06:49  rob
69 *** empty log message ***
70
71 Revision 1.1.1.1  2002/04/24 17:08:23  rob
72 UIUC version of FlightGear-0.7.pre11
73
74 Revision 1.3  2001/03/24 05:03:12  curt
75 SG-ified logstream.
76
77 Revision 1.2  2000/10/23 22:34:54  curt
78 I tested:
79 LaRCsim c172 on-ground and in-air starts, reset: all work
80 UIUC Cessna172 on-ground and in-air starts work as expected, reset
81 results in an aircraft that is upside down but does not crash FG.   I
82 don't know what it was like before, so it may well be no change.
83 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
84 trimmed), on-ground starts do not -- the c172 ends up on its back.  I
85 suspect this is no worse than before.
86
87 I did not test:
88 Balloon (the weather code returns nan's for the atmosphere data --this
89 is in the weather module and apparently is a linux only bug)
90 ADA (don't know how)
91 MagicCarpet  (needs work yet)
92 External (don't know how)
93
94 known to be broken:
95 LaRCsim c172 on-ground starts with a negative terrain altitude (this
96 happens at KPAO when the scenery is not present).   The FDM inits to
97 about 50 feet AGL and the model falls to the ground.  It does stay
98 upright, however, and seems to be fine once it settles out, FWIW.
99
100 To do:
101 --implement set_Model on the bus
102 --bring Christian's weather data into JSBSim
103 -- add default method to bus for updating things like the sin and cos of
104 latitude (for Balloon, MagicCarpet)
105 -- lots of cleanup
106
107 The files:
108 src/FDM/flight.cxx
109 src/FDM/flight.hxx
110 -- all data members now declared protected instead of private.
111 -- eliminated all but a small set of 'setters', no change to getters.
112 -- that small set is declared virtual, the default implementation
113 provided preserves the old behavior
114 -- all of the vector data members are now initialized.
115 -- added busdump() method -- SG_LOG's  all the bus data when called,
116 useful for diagnostics.
117
118 src/FDM/ADA.cxx
119 -- bus data members now directly assigned to
120
121 src/FDM/Balloon.cxx
122 -- bus data members now directly assigned to
123 -- changed V_equiv_kts to V_calibrated_kts
124
125 src/FDM/JSBSim.cxx
126 src/FDM/JSBSim.hxx
127 -- bus data members now directly assigned to
128 -- implemented the FGInterface virtual setters with JSBSim specific
129 logic
130 -- changed the static FDMExec to a dynamic fdmex (needed so that the
131 JSBSim object can be deleted when a model change is called for)
132 -- implemented constructor and destructor, moved some of the logic
133 formerly in init() to constructor
134 -- added logic to bring up FGEngInterface objects and set the RPM and
135 throttle values.
136
137 src/FDM/LaRCsim.cxx
138 src/FDM/LaRCsim.hxx
139 -- bus data members now directly assigned to
140 -- implemented the FGInterface virtual setters with LaRCsim specific
141 logic, uses LaRCsimIC
142 -- implemented constructor and destructor, moved some of the logic
143 formerly in init() to constructor
144 -- moved default inertias to here from fg_init.cxx
145 -- eliminated the climb rate calculation.  The equivalent, climb_rate =
146 -1*vdown, is now in copy_from_LaRCsim().
147
148 src/FDM/LaRCsimIC.cxx
149 src/FDM/LaRCsimIC.hxx
150 -- similar to FGInitialCondition, this class has all the logic needed to
151 turn data like Vc and Mach into the more fundamental quantities LaRCsim
152 needs to initialize.
153 -- put it in src/FDM since it is a class
154
155 src/FDM/MagicCarpet.cxx
156  -- bus data members now directly assigned to
157
158 src/FDM/Makefile.am
159 -- adds LaRCsimIC.hxx and cxx
160
161 src/FDM/JSBSim/FGAtmosphere.h
162 src/FDM/JSBSim/FGDefs.h
163 src/FDM/JSBSim/FGInitialCondition.cpp
164 src/FDM/JSBSim/FGInitialCondition.h
165 src/FDM/JSBSim/JSBSim.cpp
166 -- changes to accomodate the new bus
167
168 src/FDM/LaRCsim/atmos_62.h
169 src/FDM/LaRCsim/ls_geodesy.h
170 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
171 here are needed in LaRCsimIC
172
173 src/FDM/LaRCsim/c172_main.c
174 src/FDM/LaRCsim/cherokee_aero.c
175 src/FDM/LaRCsim/ls_aux.c
176 src/FDM/LaRCsim/ls_constants.h
177 src/FDM/LaRCsim/ls_geodesy.c
178 src/FDM/LaRCsim/ls_geodesy.h
179 src/FDM/LaRCsim/ls_step.c
180 src/FDM/UIUCModel/uiuc_betaprobe.cpp
181 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
182 weather module
183
184 src/FDM/LaRCsim/ls_interface.c
185 src/FDM/LaRCsim/ls_interface.h
186 -- added function ls_set_model_dt()
187
188 src/Main/bfi.cxx
189 -- eliminated calls that set the NED speeds to body components.  They
190 are no longer needed and confuse the new bus.
191
192 src/Main/fg_init.cxx
193 -- eliminated calls that just brought the bus data up-to-date (e.g.
194 set_sin_cos_latitude). or set default values.   The bus now handles the
195 defaults and updates itself when the setters are called (for LaRCsim and
196 JSBSim).  A default method for doing this needs to be added to the bus.
197 -- added fgVelocityInit() to set the speed the user asked for.  Both
198 JSBSim and LaRCsim can now be initialized using any of:
199 vc,mach, NED components, UVW components.
200
201 src/Main/main.cxx
202 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
203 onto the bus every update()
204
205 src/Main/options.cxx
206 src/Main/options.hxx
207 -- added enum to keep track of the speed requested by the user
208 -- eliminated calls to set NED velocity properties to body speeds, they
209 are no longer needed.
210 -- added options for the NED components.
211
212 src/Network/garmin.cxx
213 src/Network/nmea.cxx
214 --eliminated calls that just brought the bus data up-to-date (e.g.
215 set_sin_cos_latitude).  The bus now updates itself when the setters are
216 called (for LaRCsim and JSBSim).  A default method for doing this needs
217 to be added to the bus.
218 -- changed set_V_equiv_kts to set_V_calibrated_kts.  set_V_equiv_kts no
219 longer exists ( get_V_equiv_kts still does, though)
220
221 src/WeatherCM/FGLocalWeatherDatabase.cpp
222 -- commented out the code to put the weather data on the bus, a
223 different scheme for this is needed.
224
225 Revision 1.1.1.1  1999/06/17 18:07:33  curt
226 Start of 0.7.x branch
227
228 Revision 1.1.1.1  1999/04/05 21:32:45  curt
229 Start of 0.6.x branch.
230
231 Revision 1.4  1998/08/24 20:09:26  curt
232 Code optimization tweaks from Norman Vine.
233
234 Revision 1.3  1998/08/06 12:46:38  curt
235 Header change.
236
237 Revision 1.2  1998/01/19 18:40:24  curt
238 Tons of little changes to clean up the code and to remove fatal errors
239 when building with the c++ compiler.
240
241 Revision 1.1  1997/05/29 00:09:54  curt
242 Initial Flight Gear revision.
243
244  * Revision 1.12  1995/02/28  17:57:16  bjax
245  * Corrected calculation of beta_dot. EBJ
246  *
247  * Revision 1.11  1995/02/07  21:09:47  bjax
248  * Corrected calculation of "signU"; was using divide by
249  * abs(), which returns an integer; now using fabs(), which
250  * returns a double.  EBJ
251  *
252  * Revision 1.10  1994/05/10  20:09:42  bjax
253  * Fixed a major problem with dx_pilot_from_cg, etc. not being calculated locally.
254  *
255  * Revision 1.9  1994/01/11  18:44:33  bjax
256  * Changed header files to use ls_types, ls_constants, and ls_generic.
257  *
258  * Revision 1.8  1993/12/21  14:36:33  bjax
259  * Added calcs of pressures, temps and calibrated airspeeds.
260  *
261  * Revision 1.7  1993/10/14  11:25:38  bjax
262  * Changed calculation of Alpha to use 'atan2' instead of 'atan' so alphas
263  * larger than +/- 90 degrees are calculated correctly.                 EBJ
264  *
265  * Revision 1.6  1993/10/07  18:45:56  bjax
266  * A little cleanup; no significant changes. EBJ
267  *
268  * Revision 1.5  1993/10/07  18:42:22  bjax
269  * Moved calculations of auxiliary accelerations here from ls_aux, and
270  * corrected sign on Q_body*P_body*d_x_pilot term of A_Y_pilot calc.  EBJ
271  *
272  * Revision 1.4  1993/07/16  18:28:58  bjax
273  * Changed call from atmos_62 to ls_atmos. EBJ
274  *
275  * Revision 1.3  1993/06/02  15:02:42  bjax
276  * Changed call to geodesy calcs from ls_geodesy to ls_geoc_to_geod.
277  *
278  * Revision 1.1  92/12/30  13:17:39  bjax
279  * Initial revision
280  * 
281
282
283 ----------------------------------------------------------------------------
284
285     REFERENCES: [ 1] Shapiro, Ascher H.: "The Dynamics and Thermodynamics
286                         of Compressible Fluid Flow", Volume I, The Ronald 
287                         Press, 1953.
288
289 ----------------------------------------------------------------------------
290
291                 CALLED BY:
292
293 ----------------------------------------------------------------------------
294
295                 CALLS TO:
296
297 ----------------------------------------------------------------------------
298
299                 INPUTS:
300
301 ----------------------------------------------------------------------------
302
303                 OUTPUTS:
304
305 --------------------------------------------------------------------------*/
306 #include "ls_types.h"
307 #include "ls_constants.h"
308 #include "ls_generic.h"
309
310 #include "ls_aux.h"
311
312 #include "atmos_62.h"
313 #include "ls_geodesy.h"
314 #include "ls_gravity.h"
315
316 #include <math.h>
317
318 void ls_aux( void ) {
319
320         SCALAR  dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg;
321         /* SCALAR inv_Mass; */
322         SCALAR  v_XZ_plane_2, signU, v_tangential;
323         /* SCALAR inv_radius_ratio; */
324         SCALAR  cos_rwy_hdg, sin_rwy_hdg;
325         SCALAR  mach2, temp_ratio, pres_ratio;
326         SCALAR  tmp;
327         
328     /* update geodetic position */
329
330         ls_geoc_to_geod( Lat_geocentric, Radius_to_vehicle, 
331                                 &Latitude, &Altitude, &Sea_level_radius );
332         Longitude = Lon_geocentric - Earth_position_angle;
333
334     /* Calculate body axis velocities */
335
336         /* Form relative velocity vector */
337
338         V_north_rel_ground = V_north;
339         V_east_rel_ground  = V_east 
340           - OMEGA_EARTH*Sea_level_radius*cos( Lat_geocentric );
341         V_down_rel_ground  = V_down;
342
343         V_north_rel_airmass = V_north_rel_ground - V_north_airmass;
344         V_east_rel_airmass  = V_east_rel_ground  - V_east_airmass;
345         V_down_rel_airmass  = V_down_rel_ground  - V_down_airmass;
346         
347         U_body = T_local_to_body_11*V_north_rel_airmass 
348           + T_local_to_body_12*V_east_rel_airmass
349             + T_local_to_body_13*V_down_rel_airmass + U_gust;
350         V_body = T_local_to_body_21*V_north_rel_airmass 
351           + T_local_to_body_22*V_east_rel_airmass
352             + T_local_to_body_23*V_down_rel_airmass + V_gust;
353         W_body = T_local_to_body_31*V_north_rel_airmass 
354           + T_local_to_body_32*V_east_rel_airmass
355             + T_local_to_body_33*V_down_rel_airmass + W_gust;
356                                 
357         V_rel_wind = sqrt(U_body*U_body + V_body*V_body + W_body*W_body);
358
359
360     /* Calculate alpha and beta rates   */
361
362         v_XZ_plane_2 = (U_body*U_body + W_body*W_body);
363         
364         if (U_body == 0)
365                 signU = 1;
366         else
367                 signU = U_body/fabs(U_body);
368                 
369         if( (v_XZ_plane_2 == 0) || (V_rel_wind == 0) )
370         {
371                 Std_Alpha_dot = 0;
372                 Std_Beta_dot = 0;
373         }
374         else
375         {
376                 Std_Alpha_dot = (U_body*W_dot_body - W_body*U_dot_body)/
377                   v_XZ_plane_2;
378                 Std_Beta_dot = (signU*v_XZ_plane_2*V_dot_body 
379                   - V_body*(U_body*U_dot_body + W_body*W_dot_body))
380                     /(V_rel_wind*V_rel_wind*sqrt(v_XZ_plane_2));
381         }
382
383     /* Calculate flight path and other flight condition values */
384
385         if (U_body == 0) 
386                 Std_Alpha = 0;
387         else
388                 Std_Alpha = atan2( W_body, U_body );
389                 
390         Cos_alpha = cos(Std_Alpha);
391         Sin_alpha = sin(Std_Alpha);
392         
393         if (V_rel_wind == 0)
394                 Std_Beta = 0;
395         else
396                 Std_Beta = asin( V_body/ V_rel_wind );
397                 
398         Cos_beta = cos(Std_Beta);
399         Sin_beta = sin(Std_Beta);
400         
401         V_true_kts = V_rel_wind * V_TO_KNOTS;
402         
403         V_ground_speed = sqrt(V_north_rel_ground*V_north_rel_ground
404                               + V_east_rel_ground*V_east_rel_ground );
405
406         V_rel_ground = sqrt(V_ground_speed*V_ground_speed
407                             + V_down_rel_ground*V_down_rel_ground );
408         
409         v_tangential = sqrt(V_north*V_north + V_east*V_east);
410         
411         V_inertial = sqrt(v_tangential*v_tangential + V_down*V_down);
412         
413         if( (V_ground_speed == 0) && (V_down == 0) )
414           Gamma_vert_rad = 0;
415         else
416           Gamma_vert_rad = atan2( -V_down, V_ground_speed );
417                 
418         if( (V_north_rel_ground == 0) && (V_east_rel_ground == 0) )
419           Gamma_horiz_rad = 0;
420         else
421           Gamma_horiz_rad = atan2( V_east_rel_ground, V_north_rel_ground );
422         
423         if (Gamma_horiz_rad < 0) 
424           Gamma_horiz_rad = Gamma_horiz_rad + 2*LS_PI;
425         
426     /* Calculate local gravity  */
427         
428         ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
429         
430     /* call function for (smoothed) density ratio, sonic velocity, and
431            ambient pressure */
432
433         ls_atmos(Altitude, &Sigma, &V_sound, 
434                  &Static_temperature, &Static_pressure);
435         
436         Density = Sigma*SEA_LEVEL_DENSITY;
437         
438         Mach_number = V_rel_wind / V_sound;
439         
440         V_equiv = V_rel_wind*sqrt(Sigma);
441         
442         V_equiv_kts = V_equiv*V_TO_KNOTS;
443
444     /* calculate temperature and pressure ratios (from [1]) */
445
446         mach2 = Mach_number*Mach_number;
447         temp_ratio = 1.0 + 0.2*mach2; 
448         tmp = 3.5;
449         pres_ratio = pow( temp_ratio, tmp );
450
451         Total_temperature = temp_ratio*Static_temperature;
452         Total_pressure    = pres_ratio*Static_pressure;
453
454     /* calculate impact and dynamic pressures */
455         
456         Impact_pressure = Total_pressure - Static_pressure; 
457
458         Dynamic_pressure = 0.5*Density*V_rel_wind*V_rel_wind;
459
460     /* calculate calibrated airspeed indication */
461
462         V_calibrated = sqrt( 2.0*Dynamic_pressure / SEA_LEVEL_DENSITY );
463         V_calibrated_kts = V_calibrated*V_TO_KNOTS;
464         
465         Centrifugal_relief = 1 - v_tangential/(Radius_to_vehicle*Gravity);
466         
467 /* Determine location in runway coordinates */
468
469         Radius_to_rwy = Sea_level_radius + Runway_altitude;
470         cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD);
471         sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD);
472         
473         D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude);
474         D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude)
475                 *(Longitude - Runway_longitude);
476         D_cg_above_rwy  = Radius_to_vehicle - Radius_to_rwy;
477         
478         X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg 
479           + D_cg_east_of_rwy*sin_rwy_hdg;
480         Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg 
481           + D_cg_east_of_rwy*cos_rwy_hdg;
482         H_cg_rwy = D_cg_above_rwy;
483         
484         dx_pilot_from_cg = Dx_pilot - Dx_cg;
485         dy_pilot_from_cg = Dy_pilot - Dy_cg;
486         dz_pilot_from_cg = Dz_pilot - Dz_cg;
487
488         D_pilot_north_of_rwy = D_cg_north_of_rwy 
489           + T_local_to_body_11*dx_pilot_from_cg 
490             + T_local_to_body_21*dy_pilot_from_cg
491               + T_local_to_body_31*dz_pilot_from_cg;
492         D_pilot_east_of_rwy  = D_cg_east_of_rwy 
493           + T_local_to_body_12*dx_pilot_from_cg 
494             + T_local_to_body_22*dy_pilot_from_cg
495               + T_local_to_body_32*dz_pilot_from_cg;
496         D_pilot_above_rwy    = D_cg_above_rwy 
497           - T_local_to_body_13*dx_pilot_from_cg 
498             - T_local_to_body_23*dy_pilot_from_cg
499               - T_local_to_body_33*dz_pilot_from_cg;
500                                                         
501         X_pilot_rwy =  D_pilot_north_of_rwy*cos_rwy_hdg
502           + D_pilot_east_of_rwy*sin_rwy_hdg;
503         Y_pilot_rwy = -D_pilot_north_of_rwy*sin_rwy_hdg
504           + D_pilot_east_of_rwy*cos_rwy_hdg;
505         H_pilot_rwy =  D_pilot_above_rwy;
506                                                                 
507 /* Calculate Euler rates */
508         
509         Sin_phi = sin(Phi);
510         Cos_phi = cos(Phi);
511         Sin_theta = sin(Theta);
512         Cos_theta = cos(Theta);
513         Sin_psi = sin(Psi);
514         Cos_psi = cos(Psi);
515         
516         if( Cos_theta == 0 )
517           Psi_dot = 0;
518         else
519           Psi_dot = (Q_total*Sin_phi + R_total*Cos_phi)/Cos_theta;
520         
521         Theta_dot = Q_total*Cos_phi - R_total*Sin_phi;
522         Phi_dot = P_total + Psi_dot*Sin_theta;
523         
524 /* end of ls_aux */
525
526 }
527 /*************************************************************************/