1 /***************************************************************************
5 ----------------------------------------------------------------------------
7 FUNCTION: Landing gear model for example simulation
9 ----------------------------------------------------------------------------
11 MODULE STATUS: developmental
13 ----------------------------------------------------------------------------
15 GENEALOGY: Created 931012 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 931218 Added navion.h header to allow connection with
32 aileron displacement for nosewheel steering. EBJ
33 940511 Connected nosewheel to rudder pedal; adjusted gain.
39 Revision 1.20 2001/07/30 20:53:54 curt
40 Various MSVC tweaks and warning fixes.
42 Revision 1.19 2001/03/02 21:37:01 curt
43 Added a first pass at a C++ sound manager class.
45 Revision 1.18 2000/12/13 22:02:02 curt
46 MacOS changes contributed by Darrell Walisser (12/13/2000)
48 Revision 1.17 2000/09/14 15:36:25 curt
49 Tweaks to ground steering sensitivity.
51 Revision 1.16 2000/09/13 19:51:09 curt
52 MacOS changes by Darrell Walisser.
54 Revision 1.15 2000/06/12 18:52:37 curt
55 Added differential braking (Alex and David).
57 Revision 1.14 2000/04/10 18:09:41 curt
58 David Megginson made a few (mostly minor) mods to the LaRCsim files, and
59 it's now possible to choose the LaRCsim model at runtime, as in
65 fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
67 I did this so that I could play with the UIUC stuff without losing
68 Tony's C172 with its flaps, etc. I did my best to respect the design
69 of the LaRCsim code by staying in C, making only minimal changes, and
70 not introducing any dependencies on the rest of FlightGear. The
71 modified files are attached.
73 Revision 1.13 1999/12/13 20:43:41 curt
77 ----------------------------------------------------------------------------
81 ----------------------------------------------------------------------------
85 ----------------------------------------------------------------------------
89 ----------------------------------------------------------------------------
93 ----------------------------------------------------------------------------
97 --------------------------------------------------------------------------*/
100 #include "ls_constants.h"
101 #include "ls_generic.h"
102 #include "ls_cockpit.h"
104 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
107 static void sub3( DATA v1[], DATA v2[], DATA result[] )
109 result[0] = v1[0] - v2[0];
110 result[1] = v1[1] - v2[1];
111 result[2] = v1[2] - v2[2];
114 static void add3( DATA v1[], DATA v2[], DATA result[] )
116 result[0] = v1[0] + v2[0];
117 result[1] = v1[1] + v2[1];
118 result[2] = v1[2] + v2[2];
121 static void cross3( DATA v1[], DATA v2[], DATA result[] )
123 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
124 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
125 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
128 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
130 result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
131 result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
132 result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
135 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
137 result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
138 result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
139 result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
142 static void clear3( DATA v[] )
144 v[0] = 0.; v[1] = 0.; v[2] = 0.;
149 char rcsid[] = "$Id$";
151 // char gear_strings[NUM_WHEELS][12]={"nose","right main", "left main", "tail skid"};
153 * Aircraft specific initializations and data goes here
157 static int num_wheels = NUM_WHEELS; /* number of wheels */
158 static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations,full extension */
160 { 3.91, 0., 6.67 }, /*nose*/ /* in feet */
161 { -1.47, 3.58, 6.71 }, /*right main*/
162 { -1.47, -3.58, 6.71 }, /*left main*/
163 { -15.67, 0, 2.42 } /*tail skid */
165 // static DATA gear_travel[NUM_WHEELS] = /*in Z-axis*/
166 // { -0.5, 2.5, 2.5, 0};
167 static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
168 { 1200., 900., 900., 10000. };
169 static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
170 { 200., 300., 300., 400. };
171 static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
172 { 0., 0., 0., 0. }; /* 0 = none, 1 = full */
173 static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
174 { 0., 0., 0., 0}; /* radians, +CW */
176 * End of aircraft specific code
180 * Constants & coefficients for tyres on tarmac - ref [1]
183 /* skid function looks like:
189 * sliding_mu | / +------
192 * +--+------------------------>
199 static int it_rolls[NUM_WHEELS] = { 1,1,1,0};
200 static DATA sliding_mu[NUM_WHEELS] = { 0.5, 0.5, 0.5, 0.3};
201 static DATA rolling_mu[NUM_WHEELS] = { 0.01, 0.01, 0.01, 0.0};
202 static DATA max_brake_mu[NUM_WHEELS] ={ 0.0, 0.6, 0.6, 0.0};
203 static DATA max_mu = 0.8;
204 static DATA bkout_v = 0.1;
205 static DATA skid_v = 1.0;
207 * Local data variables
210 DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
211 DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
212 DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
213 DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
214 // DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
215 DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
216 DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
217 // DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
218 // DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/
223 DATA reaction_normal_force; /* wheel normal (to rwy) force */
224 DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
225 DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
226 DATA forward_mu, sideward_mu; /* friction coefficients */
227 DATA beta_mu; /* breakout friction slope */
228 DATA forward_wheel_force, sideward_wheel_force;
230 int i; /* per wheel loop counter */
233 * Execution starts here
236 beta_mu = max_mu/(skid_v-bkout_v);
237 clear3( F_gear_v ); /* Initialize sum of forces... */
238 clear3( M_gear_v ); /* ...and moments */
241 * Put aircraft specific executable code here
244 percent_brake[1] = Brake_pct[0];
245 percent_brake[2] = Brake_pct[1];
247 caster_angle_rad[0] =
248 (0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
251 for (i=0;i<num_wheels;i++) /* Loop for each wheel */
253 /* printf("%s:\n",gear_strings[i]); */
257 /*========================================*/
258 /* Calculate wheel position w.r.t. runway */
259 /*========================================*/
262 /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
265 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
267 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
269 /* then converting to local (North-East-Down) axes... */
271 multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
274 /* Runway axes correction - third element is Altitude, not (-)Z... */
276 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
278 /* Add wheel offset to cg location in local axes */
280 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
282 /* remove Runway axes correction so right hand rule applies */
284 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
286 /*============================*/
287 /* Calculate wheel velocities */
288 /*============================*/
290 /* contribution due to angular rates */
292 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
294 /* transform into local axes */
296 multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
298 /* plus contribution due to cg velocities */
300 add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
302 clear3(f_wheel_local_v);
303 reaction_normal_force=0;
304 if( HEIGHT_AGL_WHEEL < 0. )
305 /*the wheel is underground -- which implies ground contact
306 so calculate reaction forces */
308 /*===========================================*/
309 /* Calculate forces & moments for this wheel */
310 /*===========================================*/
312 /* Add any anticipation, or frame lead/prediction, here... */
314 /* no lead used at present */
316 /* Calculate sideward and forward velocities of the wheel
317 in the runway plane */
319 cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
320 sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
322 v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
323 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
324 v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
325 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
328 /* Calculate normal load force (simple spring constant) */
330 reaction_normal_force = 0.;
332 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
333 - v_wheel_local_v[2]*spring_damping[i];
334 /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
336 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
337 /* to prevent damping component from swamping spring component */
340 /* Calculate friction coefficients */
344 forward_mu = (max_brake_mu[i] - rolling_mu[i])*percent_brake[i] + rolling_mu[i];
345 abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
346 sideward_mu = sliding_mu[i];
347 if (abs_v_wheel_sideward < skid_v)
348 sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
349 if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
353 forward_mu=sliding_mu[i];
354 sideward_mu=sliding_mu[i];
357 /* Calculate foreward and sideward reaction forces */
359 forward_wheel_force = forward_mu*reaction_normal_force;
360 sideward_wheel_force = sideward_mu*reaction_normal_force;
361 if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
362 if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
363 /* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
365 /* Rotate into local (N-E-D) axes */
367 f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
368 - sideward_wheel_force*sin_wheel_hdg_angle;
369 f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
370 + sideward_wheel_force*cos_wheel_hdg_angle;
371 f_wheel_local_v[2] = reaction_normal_force;
373 /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
374 mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
376 /* Calculate moments from force and offsets in body axes */
378 cross3( d_wheel_cg_body_v, tempF, tempM );
380 /* Sum forces and moments across all wheels */
382 add3( tempF, F_gear_v, F_gear_v );
383 add3( tempM, M_gear_v, M_gear_v );
390 /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
392 /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
393 printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */