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.3 2001/07/30 20:53:54 curt
40 Various MSVC tweaks and warning fixes.
42 Revision 1.2 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.1.1.1 1999/06/17 18:07:34 curt
61 Revision 1.1.1.1 1999/04/05 21:32:45 curt
62 Start of 0.6.x branch.
65 ----------------------------------------------------------------------------
69 ----------------------------------------------------------------------------
73 ----------------------------------------------------------------------------
77 ----------------------------------------------------------------------------
81 ----------------------------------------------------------------------------
85 --------------------------------------------------------------------------*/
88 #include "ls_constants.h"
89 #include "ls_generic.h"
90 #include "ls_cockpit.h"
93 static void sub3( DATA v1[], DATA v2[], DATA result[] )
95 result[0] = v1[0] - v2[0];
96 result[1] = v1[1] - v2[1];
97 result[2] = v1[2] - v2[2];
100 static void add3( DATA v1[], DATA v2[], DATA result[] )
102 result[0] = v1[0] + v2[0];
103 result[1] = v1[1] + v2[1];
104 result[2] = v1[2] + v2[2];
107 static void cross3( DATA v1[], DATA v2[], DATA result[] )
109 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
110 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
111 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
114 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
116 result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
117 result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
118 result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
121 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
123 result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
124 result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
125 result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
128 static void clear3( DATA v[] )
130 v[0] = 0.; v[1] = 0.; v[2] = 0.;
135 char rcsid[] = "$Id$";
138 * Aircraft specific initializations and data goes here
143 static int num_wheels = NUM_WHEELS; /* number of wheels */
144 static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations */
146 { 10., 0., 4. }, /* in feet */
150 static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
151 { 1500., 5000., 5000. };
152 static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
153 { 100., 150., 150. };
154 static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
155 { 0., 0., 0. }; /* 0 = none, 1 = full */
156 static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
157 { 0., 0., 0.}; /* radians, +CW */
159 * End of aircraft specific code
163 * Constants & coefficients for tyres on tarmac - ref [1]
166 /* skid function looks like:
172 * sliding_mu | / +------
175 * +--+------------------------>
182 static DATA sliding_mu = 0.5;
183 static DATA rolling_mu = 0.01;
184 static DATA max_brake_mu = 0.6;
185 static DATA max_mu = 0.8;
186 static DATA bkout_v = 0.1;
187 static DATA skid_v = 1.0;
189 * Local data variables
192 DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
193 DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
194 DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
195 // DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
196 DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
197 DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
198 DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
199 DATA reaction_normal_force; /* wheel normal (to rwy) force */
200 DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
201 DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
202 DATA forward_mu, sideward_mu; /* friction coefficients */
203 DATA beta_mu; /* breakout friction slope */
204 DATA forward_wheel_force, sideward_wheel_force;
206 int i; /* per wheel loop counter */
209 * Execution starts here
212 beta_mu = max_mu/(skid_v-bkout_v);
213 clear3( F_gear_v ); /* Initialize sum of forces... */
214 clear3( M_gear_v ); /* ...and moments */
217 * Put aircraft specific executable code here
220 percent_brake[1] = 0.; /* replace with cockpit brake handle connection code */
221 percent_brake[2] = percent_brake[1];
223 caster_angle_rad[0] = 0.03*Rudder_pedal;
225 for (i=0;i<num_wheels;i++) /* Loop for each wheel */
227 /*========================================*/
228 /* Calculate wheel position w.r.t. runway */
229 /*========================================*/
231 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
233 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
235 /* then converting to local (North-East-Down) axes... */
237 multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
239 /* Runway axes correction - third element is Altitude, not (-)Z... */
241 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
243 /* Add wheel offset to cg location in local axes */
245 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
247 /* remove Runway axes correction so right hand rule applies */
249 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
251 /*============================*/
252 /* Calculate wheel velocities */
253 /*============================*/
255 /* contribution due to angular rates */
257 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
259 /* transform into local axes */
261 multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
263 /* plus contribution due to cg velocities */
265 add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
268 /*===========================================*/
269 /* Calculate forces & moments for this wheel */
270 /*===========================================*/
272 /* Add any anticipation, or frame lead/prediction, here... */
274 /* no lead used at present */
276 /* Calculate sideward and forward velocities of the wheel
277 in the runway plane */
279 cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
280 sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
282 v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
283 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
284 v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
285 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
287 /* Calculate normal load force (simple spring constant) */
289 reaction_normal_force = 0.;
290 if( d_wheel_rwy_local_v[2] < 0. )
292 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
293 - v_wheel_local_v[2]*spring_damping[i];
294 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
295 /* to prevent damping component from swamping spring component */
298 /* Calculate friction coefficients */
300 forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
301 abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
302 sideward_mu = sliding_mu;
303 if (abs_v_wheel_sideward < skid_v)
304 sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
305 if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
307 /* Calculate foreward and sideward reaction forces */
309 forward_wheel_force = forward_mu*reaction_normal_force;
310 sideward_wheel_force = sideward_mu*reaction_normal_force;
311 if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
312 if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
314 /* Rotate into local (N-E-D) axes */
316 f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
317 - sideward_wheel_force*sin_wheel_hdg_angle;
318 f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
319 + sideward_wheel_force*cos_wheel_hdg_angle;
320 f_wheel_local_v[2] = reaction_normal_force;
322 /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
324 mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
326 /* Calculate moments from force and offsets in body axes */
328 cross3( d_wheel_cg_body_v, tempF, tempM );
330 /* Sum forces and moments across all wheels */
332 add3( tempF, F_gear_v, F_gear_v );
333 add3( tempM, M_gear_v, M_gear_v );