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.3 2001/07/30 20:53:54 curt
43 Various MSVC tweaks and warning fixes.
45 Revision 1.2 2000/04/10 18:09:41 curt
46 David Megginson made a few (mostly minor) mods to the LaRCsim files, and
47 it's now possible to choose the LaRCsim model at runtime, as in
53 fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
55 I did this so that I could play with the UIUC stuff without losing
56 Tony's C172 with its flaps, etc. I did my best to respect the design
57 of the LaRCsim code by staying in C, making only minimal changes, and
58 not introducing any dependencies on the rest of FlightGear. The
59 modified files are attached.
61 Revision 1.1.1.1 1999/06/17 18:07:34 curt
64 Revision 1.1.1.1 1999/04/05 21:32:45 curt
65 Start of 0.6.x branch.
68 ----------------------------------------------------------------------------
72 ----------------------------------------------------------------------------
76 ----------------------------------------------------------------------------
80 ----------------------------------------------------------------------------
84 ----------------------------------------------------------------------------
88 --------------------------------------------------------------------------*/
91 #include "ls_constants.h"
92 #include "ls_generic.h"
93 #include "ls_cockpit.h"
96 static void sub3( DATA v1[], DATA v2[], DATA result[] )
98 result[0] = v1[0] - v2[0];
99 result[1] = v1[1] - v2[1];
100 result[2] = v1[2] - v2[2];
103 static void add3( DATA v1[], DATA v2[], DATA result[] )
105 result[0] = v1[0] + v2[0];
106 result[1] = v1[1] + v2[1];
107 result[2] = v1[2] + v2[2];
110 static void cross3( DATA v1[], DATA v2[], DATA result[] )
112 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
113 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
114 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
117 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
119 result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
120 result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
121 result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
124 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
126 result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
127 result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
128 result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
131 static void clear3( DATA v[] )
133 v[0] = 0.; v[1] = 0.; v[2] = 0.;
138 // char rcsid[] = "$Id$";
141 * Aircraft specific initializations and data goes here
146 static int num_wheels = NUM_WHEELS; /* number of wheels */
147 static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations */
149 { 10., 0., 4. }, /* in feet */
153 static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
154 { 1500., 5000., 5000. };
155 static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
156 { 100., 150., 150. };
157 static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
158 { 0., 0., 0. }; /* 0 = none, 1 = full */
159 static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
160 { 0., 0., 0.}; /* radians, +CW */
162 * End of aircraft specific code
166 * Constants & coefficients for tyres on tarmac - ref [1]
169 /* skid function looks like:
175 * sliding_mu | / +------
178 * +--+------------------------>
185 static DATA sliding_mu = 0.5;
186 static DATA rolling_mu = 0.01;
187 static DATA max_brake_mu = 0.6;
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_body_v[3]; /* wheel velocity, X-Y-Z */
199 DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
200 DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
201 DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
202 DATA reaction_normal_force; /* wheel normal (to rwy) force */
203 DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
204 DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
205 DATA forward_mu, sideward_mu; /* friction coefficients */
206 DATA beta_mu; /* breakout friction slope */
207 DATA forward_wheel_force, sideward_wheel_force;
209 int i; /* per wheel loop counter */
212 * Execution starts here
215 beta_mu = max_mu/(skid_v-bkout_v);
216 clear3( F_gear_v ); /* Initialize sum of forces... */
217 clear3( M_gear_v ); /* ...and moments */
220 * Put aircraft specific executable code here
223 percent_brake[1] = 0.; /* replace with cockpit brake handle connection code */
224 percent_brake[2] = percent_brake[1];
226 caster_angle_rad[0] = 0.03*Rudder_pedal;
228 for (i=0;i<num_wheels;i++) /* Loop for each wheel */
230 /*========================================*/
231 /* Calculate wheel position w.r.t. runway */
232 /*========================================*/
234 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
236 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
238 /* then converting to local (North-East-Down) axes... */
240 multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
242 /* Runway axes correction - third element is Altitude, not (-)Z... */
244 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
246 /* Add wheel offset to cg location in local axes */
248 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
250 /* remove Runway axes correction so right hand rule applies */
252 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
254 /*============================*/
255 /* Calculate wheel velocities */
256 /*============================*/
258 /* contribution due to angular rates */
260 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
262 /* transform into local axes */
264 multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
266 /* plus contribution due to cg velocities */
268 add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
271 /*===========================================*/
272 /* Calculate forces & moments for this wheel */
273 /*===========================================*/
275 /* Add any anticipation, or frame lead/prediction, here... */
277 /* no lead used at present */
279 /* Calculate sideward and forward velocities of the wheel
280 in the runway plane */
282 cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
283 sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
285 v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
286 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
287 v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
288 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
290 /* Calculate normal load force (simple spring constant) */
292 reaction_normal_force = 0.;
293 if( d_wheel_rwy_local_v[2] < 0. )
295 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
296 - v_wheel_local_v[2]*spring_damping[i];
297 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
298 /* to prevent damping component from swamping spring component */
301 /* Calculate friction coefficients */
303 forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
304 abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
305 sideward_mu = sliding_mu;
306 if (abs_v_wheel_sideward < skid_v)
307 sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
308 if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
310 /* Calculate foreward and sideward reaction forces */
312 forward_wheel_force = forward_mu*reaction_normal_force;
313 sideward_wheel_force = sideward_mu*reaction_normal_force;
314 if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
315 if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
317 /* Rotate into local (N-E-D) axes */
319 f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
320 - sideward_wheel_force*sin_wheel_hdg_angle;
321 f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
322 + sideward_wheel_force*cos_wheel_hdg_angle;
323 f_wheel_local_v[2] = reaction_normal_force;
325 /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
327 mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
329 /* Calculate moments from force and offsets in body axes */
331 cross3( d_wheel_cg_body_v, tempF, tempM );
333 /* Sum forces and moments across all wheels */
335 add3( tempF, F_gear_v, F_gear_v );
336 add3( tempM, M_gear_v, M_gear_v );