1 /***************************************************************************
5 ----------------------------------------------------------------------------
7 FUNCTION: Atmospheric and auxilary relationships for LaRCSim EOM
9 ----------------------------------------------------------------------------
11 MODULE STATUS: developmental
13 ----------------------------------------------------------------------------
15 GENEALOGY: Created 9208026 as part of C-castle simulation project.
17 ----------------------------------------------------------------------------
19 DESIGNED BY: B. Jackson
23 MAINTAINED BY: B. Jackson
25 ----------------------------------------------------------------------------
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.
36 931220 Added calculations for static and total temperatures & pressures,
37 as well as dynamic and impact pressures and calibrated airspeed.
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
42 950207 Changed use of "abs" to "fabs" in calculation of signU. EBJ
44 950228 Fixed bug in calculation of beta_dot. EBJ
46 CURRENT RCS HEADER INFO:
50 Revision 1.2 2002/11/08 17:03:50 curt
53 Latest revisions of the UIUC code.
55 Revision 1.1.1.1 2002/09/10 01:14:01 curt
56 Initial revision of FlightGear-0.9.0
58 Revision 1.3 2001/03/24 05:03:12 curt
61 Revision 1.2 2000/10/23 22:34:54 curt
63 LaRCsim c172 on-ground and in-air starts, reset: all work
64 UIUC Cessna172 on-ground and in-air starts work as expected, reset
65 results in an aircraft that is upside down but does not crash FG. I
66 don't know what it was like before, so it may well be no change.
67 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
68 trimmed), on-ground starts do not -- the c172 ends up on its back. I
69 suspect this is no worse than before.
72 Balloon (the weather code returns nan's for the atmosphere data --this
73 is in the weather module and apparently is a linux only bug)
75 MagicCarpet (needs work yet)
76 External (don't know how)
79 LaRCsim c172 on-ground starts with a negative terrain altitude (this
80 happens at KPAO when the scenery is not present). The FDM inits to
81 about 50 feet AGL and the model falls to the ground. It does stay
82 upright, however, and seems to be fine once it settles out, FWIW.
85 --implement set_Model on the bus
86 --bring Christian's weather data into JSBSim
87 -- add default method to bus for updating things like the sin and cos of
88 latitude (for Balloon, MagicCarpet)
94 -- all data members now declared protected instead of private.
95 -- eliminated all but a small set of 'setters', no change to getters.
96 -- that small set is declared virtual, the default implementation
97 provided preserves the old behavior
98 -- all of the vector data members are now initialized.
99 -- added busdump() method -- SG_LOG's all the bus data when called,
100 useful for diagnostics.
103 -- bus data members now directly assigned to
106 -- bus data members now directly assigned to
107 -- changed V_equiv_kts to V_calibrated_kts
111 -- bus data members now directly assigned to
112 -- implemented the FGInterface virtual setters with JSBSim specific
114 -- changed the static FDMExec to a dynamic fdmex (needed so that the
115 JSBSim object can be deleted when a model change is called for)
116 -- implemented constructor and destructor, moved some of the logic
117 formerly in init() to constructor
118 -- added logic to bring up FGEngInterface objects and set the RPM and
123 -- bus data members now directly assigned to
124 -- implemented the FGInterface virtual setters with LaRCsim specific
125 logic, uses LaRCsimIC
126 -- implemented constructor and destructor, moved some of the logic
127 formerly in init() to constructor
128 -- moved default inertias to here from fg_init.cxx
129 -- eliminated the climb rate calculation. The equivalent, climb_rate =
130 -1*vdown, is now in copy_from_LaRCsim().
132 src/FDM/LaRCsimIC.cxx
133 src/FDM/LaRCsimIC.hxx
134 -- similar to FGInitialCondition, this class has all the logic needed to
135 turn data like Vc and Mach into the more fundamental quantities LaRCsim
137 -- put it in src/FDM since it is a class
139 src/FDM/MagicCarpet.cxx
140 -- bus data members now directly assigned to
143 -- adds LaRCsimIC.hxx and cxx
145 src/FDM/JSBSim/FGAtmosphere.h
146 src/FDM/JSBSim/FGDefs.h
147 src/FDM/JSBSim/FGInitialCondition.cpp
148 src/FDM/JSBSim/FGInitialCondition.h
149 src/FDM/JSBSim/JSBSim.cpp
150 -- changes to accomodate the new bus
152 src/FDM/LaRCsim/atmos_62.h
153 src/FDM/LaRCsim/ls_geodesy.h
154 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
155 here are needed in LaRCsimIC
157 src/FDM/LaRCsim/c172_main.c
158 src/FDM/LaRCsim/cherokee_aero.c
159 src/FDM/LaRCsim/ls_aux.c
160 src/FDM/LaRCsim/ls_constants.h
161 src/FDM/LaRCsim/ls_geodesy.c
162 src/FDM/LaRCsim/ls_geodesy.h
163 src/FDM/LaRCsim/ls_step.c
164 src/FDM/UIUCModel/uiuc_betaprobe.cpp
165 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
168 src/FDM/LaRCsim/ls_interface.c
169 src/FDM/LaRCsim/ls_interface.h
170 -- added function ls_set_model_dt()
173 -- eliminated calls that set the NED speeds to body components. They
174 are no longer needed and confuse the new bus.
177 -- eliminated calls that just brought the bus data up-to-date (e.g.
178 set_sin_cos_latitude). or set default values. The bus now handles the
179 defaults and updates itself when the setters are called (for LaRCsim and
180 JSBSim). A default method for doing this needs to be added to the bus.
181 -- added fgVelocityInit() to set the speed the user asked for. Both
182 JSBSim and LaRCsim can now be initialized using any of:
183 vc,mach, NED components, UVW components.
186 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
187 onto the bus every update()
191 -- added enum to keep track of the speed requested by the user
192 -- eliminated calls to set NED velocity properties to body speeds, they
193 are no longer needed.
194 -- added options for the NED components.
196 src/Network/garmin.cxx
198 --eliminated calls that just brought the bus data up-to-date (e.g.
199 set_sin_cos_latitude). The bus now updates itself when the setters are
200 called (for LaRCsim and JSBSim). A default method for doing this needs
201 to be added to the bus.
202 -- changed set_V_equiv_kts to set_V_calibrated_kts. set_V_equiv_kts no
203 longer exists ( get_V_equiv_kts still does, though)
205 src/WeatherCM/FGLocalWeatherDatabase.cpp
206 -- commented out the code to put the weather data on the bus, a
207 different scheme for this is needed.
209 Revision 1.1.1.1 1999/06/17 18:07:33 curt
210 Start of 0.7.x branch
212 Revision 1.1.1.1 1999/04/05 21:32:45 curt
213 Start of 0.6.x branch.
215 Revision 1.4 1998/08/24 20:09:26 curt
216 Code optimization tweaks from Norman Vine.
218 Revision 1.3 1998/08/06 12:46:38 curt
221 Revision 1.2 1998/01/19 18:40:24 curt
222 Tons of little changes to clean up the code and to remove fatal errors
223 when building with the c++ compiler.
225 Revision 1.1 1997/05/29 00:09:54 curt
226 Initial Flight Gear revision.
228 * Revision 1.12 1995/02/28 17:57:16 bjax
229 * Corrected calculation of beta_dot. EBJ
231 * Revision 1.11 1995/02/07 21:09:47 bjax
232 * Corrected calculation of "signU"; was using divide by
233 * abs(), which returns an integer; now using fabs(), which
234 * returns a double. EBJ
236 * Revision 1.10 1994/05/10 20:09:42 bjax
237 * Fixed a major problem with dx_pilot_from_cg, etc. not being calculated locally.
239 * Revision 1.9 1994/01/11 18:44:33 bjax
240 * Changed header files to use ls_types, ls_constants, and ls_generic.
242 * Revision 1.8 1993/12/21 14:36:33 bjax
243 * Added calcs of pressures, temps and calibrated airspeeds.
245 * Revision 1.7 1993/10/14 11:25:38 bjax
246 * Changed calculation of Alpha to use 'atan2' instead of 'atan' so alphas
247 * larger than +/- 90 degrees are calculated correctly. EBJ
249 * Revision 1.6 1993/10/07 18:45:56 bjax
250 * A little cleanup; no significant changes. EBJ
252 * Revision 1.5 1993/10/07 18:42:22 bjax
253 * Moved calculations of auxiliary accelerations here from ls_aux, and
254 * corrected sign on Q_body*P_body*d_x_pilot term of A_Y_pilot calc. EBJ
256 * Revision 1.4 1993/07/16 18:28:58 bjax
257 * Changed call from atmos_62 to ls_atmos. EBJ
259 * Revision 1.3 1993/06/02 15:02:42 bjax
260 * Changed call to geodesy calcs from ls_geodesy to ls_geoc_to_geod.
262 * Revision 1.1 92/12/30 13:17:39 bjax
267 ----------------------------------------------------------------------------
269 REFERENCES: [ 1] Shapiro, Ascher H.: "The Dynamics and Thermodynamics
270 of Compressible Fluid Flow", Volume I, The Ronald
273 ----------------------------------------------------------------------------
277 ----------------------------------------------------------------------------
281 ----------------------------------------------------------------------------
285 ----------------------------------------------------------------------------
289 --------------------------------------------------------------------------*/
290 #include "ls_types.h"
291 #include "ls_constants.h"
292 #include "ls_generic.h"
296 #include "atmos_62.h"
297 #include "ls_geodesy.h"
298 #include "ls_gravity.h"
302 #include "uiuc_getwind.h" //For wind gradient functions
304 void ls_aux( void ) {
306 static double uiuc_wind[3] = {0, 0, 0}; //The UIUC wind vector (initialized to zero)
308 SCALAR dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg;
309 /* SCALAR inv_Mass; */
310 SCALAR v_XZ_plane_2, signU, v_tangential;
311 /* SCALAR inv_radius_ratio; */
312 SCALAR cos_rwy_hdg, sin_rwy_hdg;
313 SCALAR mach2, temp_ratio, pres_ratio;
316 /* update geodetic position */
318 ls_geoc_to_geod( Lat_geocentric, Radius_to_vehicle,
319 &Latitude, &Altitude, &Sea_level_radius );
320 Longitude = Lon_geocentric - Earth_position_angle;
322 /* Calculate body axis velocities */
324 /* Form relative velocity vector */
326 V_north_rel_ground = V_north;
327 V_east_rel_ground = V_east
328 - OMEGA_EARTH*Sea_level_radius*cos( Lat_geocentric );
329 V_down_rel_ground = V_down;
331 //BEGIN Modified UIUC arbitrary wind calculations:
332 uiuc_getwind(uiuc_wind); //Update the UIUC wind vector
333 V_north_rel_airmass = V_north_rel_ground - uiuc_wind[0] - V_north_airmass;
334 V_east_rel_airmass = V_east_rel_ground - uiuc_wind[1] - V_east_airmass;
335 V_down_rel_airmass = V_down_rel_ground - uiuc_wind[2] - V_down_airmass;
338 // V_north_rel_airmass = V_north_rel_ground - V_north_airmass;
339 // V_east_rel_airmass = V_east_rel_ground - V_east_airmass;
340 // V_down_rel_airmass = V_down_rel_ground - V_down_airmass;
342 U_body = T_local_to_body_11*V_north_rel_airmass
343 + T_local_to_body_12*V_east_rel_airmass
344 + T_local_to_body_13*V_down_rel_airmass + U_gust;
345 V_body = T_local_to_body_21*V_north_rel_airmass
346 + T_local_to_body_22*V_east_rel_airmass
347 + T_local_to_body_23*V_down_rel_airmass + V_gust;
348 W_body = T_local_to_body_31*V_north_rel_airmass
349 + T_local_to_body_32*V_east_rel_airmass
350 + T_local_to_body_33*V_down_rel_airmass + W_gust;
352 V_rel_wind = sqrt(U_body*U_body + V_body*V_body + W_body*W_body);
355 /* Calculate alpha and beta rates */
357 v_XZ_plane_2 = (U_body*U_body + W_body*W_body);
362 signU = U_body/fabs(U_body);
364 if( (v_XZ_plane_2 == 0) || (V_rel_wind == 0) )
371 Alpha_dot = (U_body*W_dot_body - W_body*U_dot_body)/
373 Beta_dot = (signU*v_XZ_plane_2*V_dot_body
374 - V_body*(U_body*U_dot_body + W_body*W_dot_body))
375 /(V_rel_wind*V_rel_wind*sqrt(v_XZ_plane_2));
378 /* Calculate flight path and other flight condition values */
383 Alpha = atan2( W_body, U_body );
385 Cos_alpha = cos(Alpha);
386 Sin_alpha = sin(Alpha);
391 Beta = asin( V_body/ V_rel_wind );
393 Cos_beta = cos(Beta);
394 Sin_beta = sin(Beta);
396 V_true_kts = V_rel_wind * V_TO_KNOTS;
398 V_ground_speed = sqrt(V_north_rel_ground*V_north_rel_ground
399 + V_east_rel_ground*V_east_rel_ground );
401 V_rel_ground = sqrt(V_ground_speed*V_ground_speed
402 + V_down_rel_ground*V_down_rel_ground );
404 v_tangential = sqrt(V_north*V_north + V_east*V_east);
406 V_inertial = sqrt(v_tangential*v_tangential + V_down*V_down);
408 if( (V_ground_speed == 0) && (V_down == 0) )
411 Gamma_vert_rad = atan2( -V_down, V_ground_speed );
413 if( (V_north_rel_ground == 0) && (V_east_rel_ground == 0) )
416 Gamma_horiz_rad = atan2( V_east_rel_ground, V_north_rel_ground );
418 if (Gamma_horiz_rad < 0)
419 Gamma_horiz_rad = Gamma_horiz_rad + 2*LS_PI;
421 /* Calculate local gravity */
423 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
425 /* call function for (smoothed) density ratio, sonic velocity, and
428 ls_atmos(Altitude, &Sigma, &V_sound,
429 &Static_temperature, &Static_pressure);
431 Density = Sigma*SEA_LEVEL_DENSITY;
433 Mach_number = V_rel_wind / V_sound;
435 V_equiv = V_rel_wind*sqrt(Sigma);
437 V_equiv_kts = V_equiv*V_TO_KNOTS;
439 /* calculate temperature and pressure ratios (from [1]) */
441 mach2 = Mach_number*Mach_number;
442 temp_ratio = 1.0 + 0.2*mach2;
444 pres_ratio = pow( temp_ratio, tmp );
446 Total_temperature = temp_ratio*Static_temperature;
447 Total_pressure = pres_ratio*Static_pressure;
449 /* calculate impact and dynamic pressures */
451 Impact_pressure = Total_pressure - Static_pressure;
453 Dynamic_pressure = 0.5*Density*V_rel_wind*V_rel_wind;
455 /* calculate calibrated airspeed indication */
457 V_calibrated = sqrt( 2.0*Dynamic_pressure / SEA_LEVEL_DENSITY );
458 V_calibrated_kts = V_calibrated*V_TO_KNOTS;
460 Centrifugal_relief = 1 - v_tangential/(Radius_to_vehicle*Gravity);
462 /* Determine location in runway coordinates */
464 Radius_to_rwy = Sea_level_radius + Runway_altitude;
465 cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD);
466 sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD);
468 D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude);
469 D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude)
470 *(Longitude - Runway_longitude);
471 D_cg_above_rwy = Radius_to_vehicle - Radius_to_rwy;
473 X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg
474 + D_cg_east_of_rwy*sin_rwy_hdg;
475 Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg
476 + D_cg_east_of_rwy*cos_rwy_hdg;
477 H_cg_rwy = D_cg_above_rwy;
479 dx_pilot_from_cg = Dx_pilot - Dx_cg;
480 dy_pilot_from_cg = Dy_pilot - Dy_cg;
481 dz_pilot_from_cg = Dz_pilot - Dz_cg;
483 D_pilot_north_of_rwy = D_cg_north_of_rwy
484 + T_local_to_body_11*dx_pilot_from_cg
485 + T_local_to_body_21*dy_pilot_from_cg
486 + T_local_to_body_31*dz_pilot_from_cg;
487 D_pilot_east_of_rwy = D_cg_east_of_rwy
488 + T_local_to_body_12*dx_pilot_from_cg
489 + T_local_to_body_22*dy_pilot_from_cg
490 + T_local_to_body_32*dz_pilot_from_cg;
491 D_pilot_above_rwy = D_cg_above_rwy
492 - T_local_to_body_13*dx_pilot_from_cg
493 - T_local_to_body_23*dy_pilot_from_cg
494 - T_local_to_body_33*dz_pilot_from_cg;
496 X_pilot_rwy = D_pilot_north_of_rwy*cos_rwy_hdg
497 + D_pilot_east_of_rwy*sin_rwy_hdg;
498 Y_pilot_rwy = -D_pilot_north_of_rwy*sin_rwy_hdg
499 + D_pilot_east_of_rwy*cos_rwy_hdg;
500 H_pilot_rwy = D_pilot_above_rwy;
502 /* Calculate Euler rates */
506 Sin_theta = sin(Theta);
507 Cos_theta = cos(Theta);
514 Psi_dot = (Q_total*Sin_phi + R_total*Cos_phi)/Cos_theta;
516 Theta_dot = Q_total*Cos_phi - R_total*Sin_phi;
517 Phi_dot = P_total + Psi_dot*Sin_theta;
522 /*************************************************************************/