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:02 curt
42 Revision 1.4 2001/07/30 20:53:54 curt
43 Various MSVC tweaks and warning fixes.
45 Revision 1.3 2000/06/12 18:52:37 curt
46 Added differential braking (Alex and David).
48 Revision 1.2 2000/04/10 18:09:41 curt
49 David Megginson made a few (mostly minor) mods to the LaRCsim files, and
50 it's now possible to choose the LaRCsim model at runtime, as in
56 fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
58 I did this so that I could play with the UIUC stuff without losing
59 Tony's C172 with its flaps, etc. I did my best to respect the design
60 of the LaRCsim code by staying in C, making only minimal changes, and
61 not introducing any dependencies on the rest of FlightGear. The
62 modified files are attached.
64 Revision 1.1.1.1 1999/06/17 18:07:34 curt
67 Revision 1.1.1.1 1999/04/05 21:32:45 curt
68 Start of 0.6.x branch.
70 Revision 1.6 1998/10/17 01:34:16 curt
73 Revision 1.5 1998/09/29 02:03:00 curt
74 Added a brake + autopilot mods.
76 Revision 1.4 1998/08/06 12:46:40 curt
79 Revision 1.3 1998/02/03 23:20:18 curt
80 Lots of little tweaks to fix various consistency problems discovered by
81 Solaris' CC. Fixed a bug in fg_debug.c with how the fgPrintf() wrapper
82 passed arguments along to the real printf(). Also incorporated HUD changes
85 Revision 1.2 1998/01/19 18:40:29 curt
86 Tons of little changes to clean up the code and to remove fatal errors
87 when building with the c++ compiler.
89 Revision 1.1 1997/05/29 00:10:02 curt
90 Initial Flight Gear revision.
93 ----------------------------------------------------------------------------
97 ----------------------------------------------------------------------------
101 ----------------------------------------------------------------------------
105 ----------------------------------------------------------------------------
109 ----------------------------------------------------------------------------
113 --------------------------------------------------------------------------*/
115 #include "ls_types.h"
116 #include "ls_constants.h"
117 #include "ls_generic.h"
118 #include "ls_cockpit.h"
121 static void sub3( DATA v1[], DATA v2[], DATA result[] )
123 result[0] = v1[0] - v2[0];
124 result[1] = v1[1] - v2[1];
125 result[2] = v1[2] - v2[2];
128 static void add3( DATA v1[], DATA v2[], DATA result[] )
130 result[0] = v1[0] + v2[0];
131 result[1] = v1[1] + v2[1];
132 result[2] = v1[2] + v2[2];
135 static void cross3( DATA v1[], DATA v2[], DATA result[] )
137 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
138 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
139 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
142 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
144 result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
145 result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
146 result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
149 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
151 result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
152 result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
153 result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
156 static void clear3( DATA v[] )
158 v[0] = 0.; v[1] = 0.; v[2] = 0.;
161 void navion_gear( SCALAR dt, int Initialize ) {
162 char rcsid[] = "$Id$";
165 * Aircraft specific initializations and data goes here
170 static int num_wheels = NUM_WHEELS; /* number of wheels */
171 static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations */
173 { 10., 0., 4. }, /* in feet */
177 static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
178 { 1500., 5000., 5000. };
179 static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
180 { 100., 150., 150. };
181 static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
182 { 0., 0., 0. }; /* 0 = none, 1 = full */
183 static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
184 { 0., 0., 0.}; /* radians, +CW */
186 * End of aircraft specific code
190 * Constants & coefficients for tyres on tarmac - ref [1]
193 /* skid function looks like:
199 * sliding_mu | / +------
202 * +--+------------------------>
209 static DATA sliding_mu = 0.5;
210 static DATA rolling_mu = 0.01;
211 static DATA max_brake_mu = 0.6;
212 static DATA max_mu = 0.8;
213 static DATA bkout_v = 0.1;
214 static DATA skid_v = 1.0;
216 * Local data variables
219 DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
220 DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
221 DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
222 // DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
223 DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
224 DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
225 DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
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] = 0.03*Rudder_pedal;
252 for (i=0;i<num_wheels;i++) /* Loop for each wheel */
254 /*========================================*/
255 /* Calculate wheel position w.r.t. runway */
256 /*========================================*/
258 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
260 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
262 /* then converting to local (North-East-Down) axes... */
264 multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
266 /* Runway axes correction - third element is Altitude, not (-)Z... */
268 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
270 /* Add wheel offset to cg location in local axes */
272 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
274 /* remove Runway axes correction so right hand rule applies */
276 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
278 /*============================*/
279 /* Calculate wheel velocities */
280 /*============================*/
282 /* contribution due to angular rates */
284 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
286 /* transform into local axes */
288 multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
290 /* plus contribution due to cg velocities */
292 add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
295 /*===========================================*/
296 /* Calculate forces & moments for this wheel */
297 /*===========================================*/
299 /* Add any anticipation, or frame lead/prediction, here... */
301 /* no lead used at present */
303 /* Calculate sideward and forward velocities of the wheel
304 in the runway plane */
306 cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
307 sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
309 v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
310 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
311 v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
312 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
314 /* Calculate normal load force (simple spring constant) */
316 reaction_normal_force = 0.;
317 if( d_wheel_rwy_local_v[2] < 0. )
319 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
320 - v_wheel_local_v[2]*spring_damping[i];
321 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
322 /* to prevent damping component from swamping spring component */
325 /* Calculate friction coefficients */
327 forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
328 abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
329 sideward_mu = sliding_mu;
330 if (abs_v_wheel_sideward < skid_v)
331 sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
332 if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
334 /* Calculate foreward and sideward reaction forces */
336 forward_wheel_force = forward_mu*reaction_normal_force;
337 sideward_wheel_force = sideward_mu*reaction_normal_force;
338 if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
339 if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
341 /* Rotate into local (N-E-D) axes */
343 f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
344 - sideward_wheel_force*sin_wheel_hdg_angle;
345 f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
346 + sideward_wheel_force*cos_wheel_hdg_angle;
347 f_wheel_local_v[2] = reaction_normal_force;
349 /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
351 mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
353 /* Calculate moments from force and offsets in body axes */
355 cross3( d_wheel_cg_body_v, tempF, tempM );
357 /* Sum forces and moments across all wheels */
359 add3( tempF, F_gear_v, F_gear_v );
360 add3( tempM, M_gear_v, M_gear_v );