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.3 2001/03/24 05:03:12 curt
53 Revision 1.2 2000/10/23 22:34:54 curt
55 LaRCsim c172 on-ground and in-air starts, reset: all work
56 UIUC Cessna172 on-ground and in-air starts work as expected, reset
57 results in an aircraft that is upside down but does not crash FG. I
58 don't know what it was like before, so it may well be no change.
59 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
60 trimmed), on-ground starts do not -- the c172 ends up on its back. I
61 suspect this is no worse than before.
64 Balloon (the weather code returns nan's for the atmosphere data --this
65 is in the weather module and apparently is a linux only bug)
67 MagicCarpet (needs work yet)
68 External (don't know how)
71 LaRCsim c172 on-ground starts with a negative terrain altitude (this
72 happens at KPAO when the scenery is not present). The FDM inits to
73 about 50 feet AGL and the model falls to the ground. It does stay
74 upright, however, and seems to be fine once it settles out, FWIW.
77 --implement set_Model on the bus
78 --bring Christian's weather data into JSBSim
79 -- add default method to bus for updating things like the sin and cos of
80 latitude (for Balloon, MagicCarpet)
86 -- all data members now declared protected instead of private.
87 -- eliminated all but a small set of 'setters', no change to getters.
88 -- that small set is declared virtual, the default implementation
89 provided preserves the old behavior
90 -- all of the vector data members are now initialized.
91 -- added busdump() method -- SG_LOG's all the bus data when called,
92 useful for diagnostics.
95 -- bus data members now directly assigned to
98 -- bus data members now directly assigned to
99 -- changed V_equiv_kts to V_calibrated_kts
103 -- bus data members now directly assigned to
104 -- implemented the FGInterface virtual setters with JSBSim specific
106 -- changed the static FDMExec to a dynamic fdmex (needed so that the
107 JSBSim object can be deleted when a model change is called for)
108 -- implemented constructor and destructor, moved some of the logic
109 formerly in init() to constructor
110 -- added logic to bring up FGEngInterface objects and set the RPM and
115 -- bus data members now directly assigned to
116 -- implemented the FGInterface virtual setters with LaRCsim specific
117 logic, uses LaRCsimIC
118 -- implemented constructor and destructor, moved some of the logic
119 formerly in init() to constructor
120 -- moved default inertias to here from fg_init.cxx
121 -- eliminated the climb rate calculation. The equivalent, climb_rate =
122 -1*vdown, is now in copy_from_LaRCsim().
124 src/FDM/LaRCsimIC.cxx
125 src/FDM/LaRCsimIC.hxx
126 -- similar to FGInitialCondition, this class has all the logic needed to
127 turn data like Vc and Mach into the more fundamental quantities LaRCsim
129 -- put it in src/FDM since it is a class
131 src/FDM/MagicCarpet.cxx
132 -- bus data members now directly assigned to
135 -- adds LaRCsimIC.hxx and cxx
137 src/FDM/JSBSim/FGAtmosphere.h
138 src/FDM/JSBSim/FGDefs.h
139 src/FDM/JSBSim/FGInitialCondition.cpp
140 src/FDM/JSBSim/FGInitialCondition.h
141 src/FDM/JSBSim/JSBSim.cpp
142 -- changes to accomodate the new bus
144 src/FDM/LaRCsim/atmos_62.h
145 src/FDM/LaRCsim/ls_geodesy.h
146 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
147 here are needed in LaRCsimIC
149 src/FDM/LaRCsim/c172_main.c
150 src/FDM/LaRCsim/cherokee_aero.c
151 src/FDM/LaRCsim/ls_aux.c
152 src/FDM/LaRCsim/ls_constants.h
153 src/FDM/LaRCsim/ls_geodesy.c
154 src/FDM/LaRCsim/ls_geodesy.h
155 src/FDM/LaRCsim/ls_step.c
156 src/FDM/UIUCModel/uiuc_betaprobe.cpp
157 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
160 src/FDM/LaRCsim/ls_interface.c
161 src/FDM/LaRCsim/ls_interface.h
162 -- added function ls_set_model_dt()
165 -- eliminated calls that set the NED speeds to body components. They
166 are no longer needed and confuse the new bus.
169 -- eliminated calls that just brought the bus data up-to-date (e.g.
170 set_sin_cos_latitude). or set default values. The bus now handles the
171 defaults and updates itself when the setters are called (for LaRCsim and
172 JSBSim). A default method for doing this needs to be added to the bus.
173 -- added fgVelocityInit() to set the speed the user asked for. Both
174 JSBSim and LaRCsim can now be initialized using any of:
175 vc,mach, NED components, UVW components.
178 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
179 onto the bus every update()
183 -- added enum to keep track of the speed requested by the user
184 -- eliminated calls to set NED velocity properties to body speeds, they
185 are no longer needed.
186 -- added options for the NED components.
188 src/Network/garmin.cxx
190 --eliminated calls that just brought the bus data up-to-date (e.g.
191 set_sin_cos_latitude). The bus now updates itself when the setters are
192 called (for LaRCsim and JSBSim). A default method for doing this needs
193 to be added to the bus.
194 -- changed set_V_equiv_kts to set_V_calibrated_kts. set_V_equiv_kts no
195 longer exists ( get_V_equiv_kts still does, though)
197 src/WeatherCM/FGLocalWeatherDatabase.cpp
198 -- commented out the code to put the weather data on the bus, a
199 different scheme for this is needed.
201 Revision 1.1.1.1 1999/06/17 18:07:33 curt
202 Start of 0.7.x branch
204 Revision 1.1.1.1 1999/04/05 21:32:45 curt
205 Start of 0.6.x branch.
207 Revision 1.4 1998/08/24 20:09:26 curt
208 Code optimization tweaks from Norman Vine.
210 Revision 1.3 1998/08/06 12:46:38 curt
213 Revision 1.2 1998/01/19 18:40:24 curt
214 Tons of little changes to clean up the code and to remove fatal errors
215 when building with the c++ compiler.
217 Revision 1.1 1997/05/29 00:09:54 curt
218 Initial Flight Gear revision.
220 * Revision 1.12 1995/02/28 17:57:16 bjax
221 * Corrected calculation of beta_dot. EBJ
223 * Revision 1.11 1995/02/07 21:09:47 bjax
224 * Corrected calculation of "signU"; was using divide by
225 * abs(), which returns an integer; now using fabs(), which
226 * returns a double. EBJ
228 * Revision 1.10 1994/05/10 20:09:42 bjax
229 * Fixed a major problem with dx_pilot_from_cg, etc. not being calculated locally.
231 * Revision 1.9 1994/01/11 18:44:33 bjax
232 * Changed header files to use ls_types, ls_constants, and ls_generic.
234 * Revision 1.8 1993/12/21 14:36:33 bjax
235 * Added calcs of pressures, temps and calibrated airspeeds.
237 * Revision 1.7 1993/10/14 11:25:38 bjax
238 * Changed calculation of Alpha to use 'atan2' instead of 'atan' so alphas
239 * larger than +/- 90 degrees are calculated correctly. EBJ
241 * Revision 1.6 1993/10/07 18:45:56 bjax
242 * A little cleanup; no significant changes. EBJ
244 * Revision 1.5 1993/10/07 18:42:22 bjax
245 * Moved calculations of auxiliary accelerations here from ls_aux, and
246 * corrected sign on Q_body*P_body*d_x_pilot term of A_Y_pilot calc. EBJ
248 * Revision 1.4 1993/07/16 18:28:58 bjax
249 * Changed call from atmos_62 to ls_atmos. EBJ
251 * Revision 1.3 1993/06/02 15:02:42 bjax
252 * Changed call to geodesy calcs from ls_geodesy to ls_geoc_to_geod.
254 * Revision 1.1 92/12/30 13:17:39 bjax
259 ----------------------------------------------------------------------------
261 REFERENCES: [ 1] Shapiro, Ascher H.: "The Dynamics and Thermodynamics
262 of Compressible Fluid Flow", Volume I, The Ronald
265 ----------------------------------------------------------------------------
269 ----------------------------------------------------------------------------
273 ----------------------------------------------------------------------------
277 ----------------------------------------------------------------------------
281 --------------------------------------------------------------------------*/
282 #include "ls_types.h"
283 #include "ls_constants.h"
284 #include "ls_generic.h"
288 #include "atmos_62.h"
289 #include "ls_geodesy.h"
290 #include "ls_gravity.h"
295 void ls_aux( void ) {
297 SCALAR dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg;
298 /* SCALAR inv_Mass; */
299 SCALAR v_XZ_plane_2, signU, v_tangential;
300 /* SCALAR inv_radius_ratio; */
301 SCALAR cos_rwy_hdg, sin_rwy_hdg;
302 SCALAR mach2, temp_ratio, pres_ratio;
305 /* update geodetic position */
307 ls_geoc_to_geod( Lat_geocentric, Radius_to_vehicle,
308 &Latitude, &Altitude, &Sea_level_radius );
309 Longitude = Lon_geocentric - Earth_position_angle;
311 /* Calculate body axis velocities */
313 /* Form relative velocity vector */
315 V_north_rel_ground = V_north;
316 V_east_rel_ground = V_east
317 - OMEGA_EARTH*Sea_level_radius*cos( Lat_geocentric );
318 V_down_rel_ground = V_down;
320 V_north_rel_airmass = V_north_rel_ground - V_north_airmass;
321 V_east_rel_airmass = V_east_rel_ground - V_east_airmass;
322 V_down_rel_airmass = V_down_rel_ground - V_down_airmass;
324 U_body = T_local_to_body_11*V_north_rel_airmass
325 + T_local_to_body_12*V_east_rel_airmass
326 + T_local_to_body_13*V_down_rel_airmass + U_gust;
327 V_body = T_local_to_body_21*V_north_rel_airmass
328 + T_local_to_body_22*V_east_rel_airmass
329 + T_local_to_body_23*V_down_rel_airmass + V_gust;
330 W_body = T_local_to_body_31*V_north_rel_airmass
331 + T_local_to_body_32*V_east_rel_airmass
332 + T_local_to_body_33*V_down_rel_airmass + W_gust;
334 V_rel_wind = sqrt(U_body*U_body + V_body*V_body + W_body*W_body);
337 /* Calculate alpha and beta rates */
339 v_XZ_plane_2 = (U_body*U_body + W_body*W_body);
344 signU = U_body/fabs(U_body);
346 if( (v_XZ_plane_2 == 0) || (V_rel_wind == 0) )
353 Alpha_dot = (U_body*W_dot_body - W_body*U_dot_body)/
355 Beta_dot = (signU*v_XZ_plane_2*V_dot_body
356 - V_body*(U_body*U_dot_body + W_body*W_dot_body))
357 /(V_rel_wind*V_rel_wind*sqrt(v_XZ_plane_2));
360 /* Calculate flight path and other flight condition values */
365 Alpha = atan2( W_body, U_body );
367 Cos_alpha = cos(Alpha);
368 Sin_alpha = sin(Alpha);
373 Beta = asin( V_body/ V_rel_wind );
375 Cos_beta = cos(Beta);
376 Sin_beta = sin(Beta);
378 V_true_kts = V_rel_wind * V_TO_KNOTS;
380 V_ground_speed = sqrt(V_north_rel_ground*V_north_rel_ground
381 + V_east_rel_ground*V_east_rel_ground );
383 V_rel_ground = sqrt(V_ground_speed*V_ground_speed
384 + V_down_rel_ground*V_down_rel_ground );
386 v_tangential = sqrt(V_north*V_north + V_east*V_east);
388 V_inertial = sqrt(v_tangential*v_tangential + V_down*V_down);
390 if( (V_ground_speed == 0) && (V_down == 0) )
393 Gamma_vert_rad = atan2( -V_down, V_ground_speed );
395 if( (V_north_rel_ground == 0) && (V_east_rel_ground == 0) )
398 Gamma_horiz_rad = atan2( V_east_rel_ground, V_north_rel_ground );
400 if (Gamma_horiz_rad < 0)
401 Gamma_horiz_rad = Gamma_horiz_rad + 2*LS_PI;
403 /* Calculate local gravity */
405 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
407 /* call function for (smoothed) density ratio, sonic velocity, and
410 ls_atmos(Altitude, &Sigma, &V_sound,
411 &Static_temperature, &Static_pressure);
413 Density = Sigma*SEA_LEVEL_DENSITY;
415 Mach_number = V_rel_wind / V_sound;
417 V_equiv = V_rel_wind*sqrt(Sigma);
419 V_equiv_kts = V_equiv*V_TO_KNOTS;
421 /* calculate temperature and pressure ratios (from [1]) */
423 mach2 = Mach_number*Mach_number;
424 temp_ratio = 1.0 + 0.2*mach2;
426 pres_ratio = pow( temp_ratio, tmp );
428 Total_temperature = temp_ratio*Static_temperature;
429 Total_pressure = pres_ratio*Static_pressure;
431 /* calculate impact and dynamic pressures */
433 Impact_pressure = Total_pressure - Static_pressure;
435 Dynamic_pressure = 0.5*Density*V_rel_wind*V_rel_wind;
437 /* calculate calibrated airspeed indication */
439 V_calibrated = sqrt( 2.0*Dynamic_pressure / SEA_LEVEL_DENSITY );
440 V_calibrated_kts = V_calibrated*V_TO_KNOTS;
442 Centrifugal_relief = 1 - v_tangential/(Radius_to_vehicle*Gravity);
444 /* Determine location in runway coordinates */
446 Radius_to_rwy = Sea_level_radius + Runway_altitude;
447 cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD);
448 sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD);
450 D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude);
451 D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude)
452 *(Longitude - Runway_longitude);
453 D_cg_above_rwy = Radius_to_vehicle - Radius_to_rwy;
455 X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg
456 + D_cg_east_of_rwy*sin_rwy_hdg;
457 Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg
458 + D_cg_east_of_rwy*cos_rwy_hdg;
459 H_cg_rwy = D_cg_above_rwy;
461 dx_pilot_from_cg = Dx_pilot - Dx_cg;
462 dy_pilot_from_cg = Dy_pilot - Dy_cg;
463 dz_pilot_from_cg = Dz_pilot - Dz_cg;
465 D_pilot_north_of_rwy = D_cg_north_of_rwy
466 + T_local_to_body_11*dx_pilot_from_cg
467 + T_local_to_body_21*dy_pilot_from_cg
468 + T_local_to_body_31*dz_pilot_from_cg;
469 D_pilot_east_of_rwy = D_cg_east_of_rwy
470 + T_local_to_body_12*dx_pilot_from_cg
471 + T_local_to_body_22*dy_pilot_from_cg
472 + T_local_to_body_32*dz_pilot_from_cg;
473 D_pilot_above_rwy = D_cg_above_rwy
474 - T_local_to_body_13*dx_pilot_from_cg
475 - T_local_to_body_23*dy_pilot_from_cg
476 - T_local_to_body_33*dz_pilot_from_cg;
478 X_pilot_rwy = D_pilot_north_of_rwy*cos_rwy_hdg
479 + D_pilot_east_of_rwy*sin_rwy_hdg;
480 Y_pilot_rwy = -D_pilot_north_of_rwy*sin_rwy_hdg
481 + D_pilot_east_of_rwy*cos_rwy_hdg;
482 H_pilot_rwy = D_pilot_above_rwy;
484 /* Calculate Euler rates */
488 Sin_theta = sin(Theta);
489 Cos_theta = cos(Theta);
496 Psi_dot = (Q_total*Sin_phi + R_total*Cos_phi)/Cos_theta;
498 Theta_dot = Q_total*Cos_phi - R_total*Sin_phi;
499 Phi_dot = P_total + Psi_dot*Sin_theta;
504 /*************************************************************************/