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.1 2002/09/10 01:14:01 curt
42 Revision 1.20 2001/07/30 20:53:54 curt
43 Various MSVC tweaks and warning fixes.
45 Revision 1.19 2001/03/02 21:37:01 curt
46 Added a first pass at a C++ sound manager class.
48 Revision 1.18 2000/12/13 22:02:02 curt
49 MacOS changes contributed by Darrell Walisser (12/13/2000)
51 Revision 1.17 2000/09/14 15:36:25 curt
52 Tweaks to ground steering sensitivity.
54 Revision 1.16 2000/09/13 19:51:09 curt
55 MacOS changes by Darrell Walisser.
57 Revision 1.15 2000/06/12 18:52:37 curt
58 Added differential braking (Alex and David).
60 Revision 1.14 2000/04/10 18:09:41 curt
61 David Megginson made a few (mostly minor) mods to the LaRCsim files, and
62 it's now possible to choose the LaRCsim model at runtime, as in
68 fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
70 I did this so that I could play with the UIUC stuff without losing
71 Tony's C172 with its flaps, etc. I did my best to respect the design
72 of the LaRCsim code by staying in C, making only minimal changes, and
73 not introducing any dependencies on the rest of FlightGear. The
74 modified files are attached.
76 Revision 1.13 1999/12/13 20:43:41 curt
80 ----------------------------------------------------------------------------
84 ----------------------------------------------------------------------------
88 ----------------------------------------------------------------------------
92 ----------------------------------------------------------------------------
96 ----------------------------------------------------------------------------
100 --------------------------------------------------------------------------*/
102 #include "ls_types.h"
103 #include "ls_constants.h"
104 #include "ls_generic.h"
105 #include "ls_cockpit.h"
107 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
110 static void sub3( DATA v1[], DATA v2[], DATA result[] )
112 result[0] = v1[0] - v2[0];
113 result[1] = v1[1] - v2[1];
114 result[2] = v1[2] - v2[2];
117 static void add3( DATA v1[], DATA v2[], DATA result[] )
119 result[0] = v1[0] + v2[0];
120 result[1] = v1[1] + v2[1];
121 result[2] = v1[2] + v2[2];
124 static void cross3( DATA v1[], DATA v2[], DATA result[] )
126 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
127 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
128 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
131 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
133 result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
134 result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
135 result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
138 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
140 result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
141 result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
142 result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
145 static void clear3( DATA v[] )
147 v[0] = 0.; v[1] = 0.; v[2] = 0.;
152 char rcsid[] = "$Id$";
154 // char gear_strings[NUM_WHEELS][12]={"nose","right main", "left main", "tail skid"};
156 * Aircraft specific initializations and data goes here
160 static int num_wheels = NUM_WHEELS; /* number of wheels */
161 static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations,full extension */
163 { 3.91, 0., 6.67 }, /*nose*/ /* in feet */
164 { -1.47, 3.58, 6.71 }, /*right main*/
165 { -1.47, -3.58, 6.71 }, /*left main*/
166 { -15.67, 0, 2.42 } /*tail skid */
168 // static DATA gear_travel[NUM_WHEELS] = /*in Z-axis*/
169 // { -0.5, 2.5, 2.5, 0};
170 static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
171 { 1200., 900., 900., 10000. };
172 static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
173 { 200., 300., 300., 400. };
174 static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
175 { 0., 0., 0., 0. }; /* 0 = none, 1 = full */
176 static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
177 { 0., 0., 0., 0}; /* radians, +CW */
179 * End of aircraft specific code
183 * Constants & coefficients for tyres on tarmac - ref [1]
186 /* skid function looks like:
192 * sliding_mu | / +------
195 * +--+------------------------>
202 static int it_rolls[NUM_WHEELS] = { 1,1,1,0};
203 static DATA sliding_mu[NUM_WHEELS] = { 0.5, 0.5, 0.5, 0.3};
204 static DATA rolling_mu[NUM_WHEELS] = { 0.01, 0.01, 0.01, 0.0};
205 static DATA max_brake_mu[NUM_WHEELS] ={ 0.0, 0.6, 0.6, 0.0};
206 static DATA max_mu = 0.8;
207 static DATA bkout_v = 0.1;
208 static DATA skid_v = 1.0;
210 * Local data variables
213 DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
214 DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
215 DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
216 DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
217 // DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
218 DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
219 DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
220 // DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
221 // DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/
226 DATA reaction_normal_force; /* wheel normal (to rwy) force */
227 DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
228 DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
229 DATA forward_mu, sideward_mu; /* friction coefficients */
230 DATA beta_mu; /* breakout friction slope */
231 DATA forward_wheel_force, sideward_wheel_force;
233 int i; /* per wheel loop counter */
236 * Execution starts here
239 beta_mu = max_mu/(skid_v-bkout_v);
240 clear3( F_gear_v ); /* Initialize sum of forces... */
241 clear3( M_gear_v ); /* ...and moments */
244 * Put aircraft specific executable code here
247 percent_brake[1] = Brake_pct[0];
248 percent_brake[2] = Brake_pct[1];
250 caster_angle_rad[0] =
251 (0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
254 for (i=0;i<num_wheels;i++) /* Loop for each wheel */
256 /* printf("%s:\n",gear_strings[i]); */
260 /*========================================*/
261 /* Calculate wheel position w.r.t. runway */
262 /*========================================*/
265 /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
268 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
270 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
272 /* then converting to local (North-East-Down) axes... */
274 multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
277 /* Runway axes correction - third element is Altitude, not (-)Z... */
279 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
281 /* Add wheel offset to cg location in local axes */
283 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
285 /* remove Runway axes correction so right hand rule applies */
287 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
289 /*============================*/
290 /* Calculate wheel velocities */
291 /*============================*/
293 /* contribution due to angular rates */
295 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
297 /* transform into local axes */
299 multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
301 /* plus contribution due to cg velocities */
303 add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
305 clear3(f_wheel_local_v);
306 reaction_normal_force=0;
307 if( HEIGHT_AGL_WHEEL < 0. )
308 /*the wheel is underground -- which implies ground contact
309 so calculate reaction forces */
311 /*===========================================*/
312 /* Calculate forces & moments for this wheel */
313 /*===========================================*/
315 /* Add any anticipation, or frame lead/prediction, here... */
317 /* no lead used at present */
319 /* Calculate sideward and forward velocities of the wheel
320 in the runway plane */
322 cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
323 sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
325 v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
326 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
327 v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
328 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
331 /* Calculate normal load force (simple spring constant) */
333 reaction_normal_force = 0.;
335 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
336 - v_wheel_local_v[2]*spring_damping[i];
337 /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
339 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
340 /* to prevent damping component from swamping spring component */
343 /* Calculate friction coefficients */
347 forward_mu = (max_brake_mu[i] - rolling_mu[i])*percent_brake[i] + rolling_mu[i];
348 abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
349 sideward_mu = sliding_mu[i];
350 if (abs_v_wheel_sideward < skid_v)
351 sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
352 if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
356 forward_mu=sliding_mu[i];
357 sideward_mu=sliding_mu[i];
360 /* Calculate foreward and sideward reaction forces */
362 forward_wheel_force = forward_mu*reaction_normal_force;
363 sideward_wheel_force = sideward_mu*reaction_normal_force;
364 if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
365 if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
366 /* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
368 /* Rotate into local (N-E-D) axes */
370 f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
371 - sideward_wheel_force*sin_wheel_hdg_angle;
372 f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
373 + sideward_wheel_force*cos_wheel_hdg_angle;
374 f_wheel_local_v[2] = reaction_normal_force;
376 /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
377 mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
379 /* Calculate moments from force and offsets in body axes */
381 cross3( d_wheel_cg_body_v, tempF, tempM );
383 /* Sum forces and moments across all wheels */
385 add3( tempF, F_gear_v, F_gear_v );
386 add3( tempM, M_gear_v, M_gear_v );
393 /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
395 /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
396 printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */