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.15 2000/06/12 18:52:37 curt
40 Added differential braking (Alex and David).
42 Revision 1.14 2000/04/10 18:09:41 curt
43 David Megginson made a few (mostly minor) mods to the LaRCsim files, and
44 it's now possible to choose the LaRCsim model at runtime, as in
50 fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
52 I did this so that I could play with the UIUC stuff without losing
53 Tony's C172 with its flaps, etc. I did my best to respect the design
54 of the LaRCsim code by staying in C, making only minimal changes, and
55 not introducing any dependencies on the rest of FlightGear. The
56 modified files are attached.
58 Revision 1.13 1999/12/13 20:43:41 curt
62 ----------------------------------------------------------------------------
66 ----------------------------------------------------------------------------
70 ----------------------------------------------------------------------------
74 ----------------------------------------------------------------------------
78 ----------------------------------------------------------------------------
82 --------------------------------------------------------------------------*/
85 #include "ls_constants.h"
86 #include "ls_generic.h"
87 #include "ls_cockpit.h"
89 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
92 static sub3( DATA v1[], DATA v2[], DATA result[] )
94 result[0] = v1[0] - v2[0];
95 result[1] = v1[1] - v2[1];
96 result[2] = v1[2] - v2[2];
99 static add3( DATA v1[], DATA v2[], DATA result[] )
101 result[0] = v1[0] + v2[0];
102 result[1] = v1[1] + v2[1];
103 result[2] = v1[2] + v2[2];
106 static cross3( DATA v1[], DATA v2[], DATA result[] )
108 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
109 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
110 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
113 static multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
115 result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
116 result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
117 result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
120 static mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
122 result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
123 result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
124 result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
127 static clear3( DATA v[] )
129 v[0] = 0.; v[1] = 0.; v[2] = 0.;
134 char rcsid[] = "$Id$";
136 char gear_strings[NUM_WHEELS][12]={"nose","right main", "left main", "tail skid"};
138 * Aircraft specific initializations and data goes here
142 static int num_wheels = NUM_WHEELS; /* number of wheels */
143 static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations,full extension */
145 { 3.91, 0., 6.67 }, /*nose*/ /* in feet */
146 { -1.47, 3.58, 6.71 }, /*right main*/
147 { -1.47, -3.58, 6.71 }, /*left main*/
148 { -15.67, 0, 2.42 } /*tail skid */
150 static DATA gear_travel[NUM_WHEELS] = /*in Z-axis*/
151 { -0.5, 2.5, 2.5, 0};
152 static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
153 { 1200., 900., 900., 10000. };
154 static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
155 { 200., 300., 300., 400. };
156 static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
157 { 0., 0., 0., 0. }; /* 0 = none, 1 = full */
158 static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
159 { 0., 0., 0., 0}; /* radians, +CW */
161 * End of aircraft specific code
165 * Constants & coefficients for tyres on tarmac - ref [1]
168 /* skid function looks like:
174 * sliding_mu | / +------
177 * +--+------------------------>
184 static int it_rolls[NUM_WHEELS] = { 1,1,1,0};
185 static DATA sliding_mu[NUM_WHEELS] = { 0.5, 0.5, 0.5, 0.3};
186 static DATA rolling_mu[NUM_WHEELS] = { 0.01, 0.01, 0.01, 0.0};
187 static DATA max_brake_mu[NUM_WHEELS] ={ 0.0, 0.6, 0.6, 0.0};
188 static DATA max_mu = 0.8;
189 static DATA bkout_v = 0.1;
190 static DATA skid_v = 1.0;
192 * Local data variables
195 DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
196 DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
197 DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
198 DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
199 DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
200 DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
201 DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
202 DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
203 DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/
204 DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
205 DATA reaction_normal_force; /* wheel normal (to rwy) force */
206 DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
207 DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
208 DATA forward_mu, sideward_mu; /* friction coefficients */
209 DATA beta_mu; /* breakout friction slope */
210 DATA forward_wheel_force, sideward_wheel_force;
212 int i; /* per wheel loop counter */
215 * Execution starts here
218 beta_mu = max_mu/(skid_v-bkout_v);
219 clear3( F_gear_v ); /* Initialize sum of forces... */
220 clear3( M_gear_v ); /* ...and moments */
223 * Put aircraft specific executable code here
226 percent_brake[1] = Brake_pct[0];
227 percent_brake[2] = Brake_pct[1];
229 caster_angle_rad[0] = 0.52*Rudder_pedal;
232 for (i=0;i<num_wheels;i++) /* Loop for each wheel */
234 /* printf("%s:\n",gear_strings[i]); */
238 /*========================================*/
239 /* Calculate wheel position w.r.t. runway */
240 /*========================================*/
243 /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
246 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
248 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
250 /* then converting to local (North-East-Down) axes... */
252 multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
255 /* Runway axes correction - third element is Altitude, not (-)Z... */
257 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
259 /* Add wheel offset to cg location in local axes */
261 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
263 /* remove Runway axes correction so right hand rule applies */
265 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
267 /*============================*/
268 /* Calculate wheel velocities */
269 /*============================*/
271 /* contribution due to angular rates */
273 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
275 /* transform into local axes */
277 multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
279 /* plus contribution due to cg velocities */
281 add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
283 clear3(f_wheel_local_v);
284 reaction_normal_force=0;
285 if( HEIGHT_AGL_WHEEL < 0. )
286 /*the wheel is underground -- which implies ground contact
287 so calculate reaction forces */
289 /*===========================================*/
290 /* Calculate forces & moments for this wheel */
291 /*===========================================*/
293 /* Add any anticipation, or frame lead/prediction, here... */
295 /* no lead used at present */
297 /* Calculate sideward and forward velocities of the wheel
298 in the runway plane */
300 cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
301 sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
303 v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
304 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
305 v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
306 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
309 /* Calculate normal load force (simple spring constant) */
311 reaction_normal_force = 0.;
313 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
314 - v_wheel_local_v[2]*spring_damping[i];
315 /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
317 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
318 /* to prevent damping component from swamping spring component */
321 /* Calculate friction coefficients */
325 forward_mu = (max_brake_mu[i] - rolling_mu[i])*percent_brake[i] + rolling_mu[i];
326 abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
327 sideward_mu = sliding_mu[i];
328 if (abs_v_wheel_sideward < skid_v)
329 sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
330 if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
334 forward_mu=sliding_mu[i];
335 sideward_mu=sliding_mu[i];
338 /* Calculate foreward and sideward reaction forces */
340 forward_wheel_force = forward_mu*reaction_normal_force;
341 sideward_wheel_force = sideward_mu*reaction_normal_force;
342 if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
343 if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
344 /* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
346 /* Rotate into local (N-E-D) axes */
348 f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
349 - sideward_wheel_force*sin_wheel_hdg_angle;
350 f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
351 + sideward_wheel_force*cos_wheel_hdg_angle;
352 f_wheel_local_v[2] = reaction_normal_force;
354 /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
355 mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
357 /* Calculate moments from force and offsets in body axes */
359 cross3( d_wheel_cg_body_v, tempF, tempM );
361 /* Sum forces and moments across all wheels */
363 add3( tempF, F_gear_v, F_gear_v );
364 add3( tempM, M_gear_v, M_gear_v );
371 /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
373 /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
374 printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */