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 2000/06/12 18:52:37 curt
40 Added differential braking (Alex and David).
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.
64 Revision 1.6 1998/10/17 01:34:16 curt
67 Revision 1.5 1998/09/29 02:03:00 curt
68 Added a brake + autopilot mods.
70 Revision 1.4 1998/08/06 12:46:40 curt
73 Revision 1.3 1998/02/03 23:20:18 curt
74 Lots of little tweaks to fix various consistency problems discovered by
75 Solaris' CC. Fixed a bug in fg_debug.c with how the fgPrintf() wrapper
76 passed arguments along to the real printf(). Also incorporated HUD changes
79 Revision 1.2 1998/01/19 18:40:29 curt
80 Tons of little changes to clean up the code and to remove fatal errors
81 when building with the c++ compiler.
83 Revision 1.1 1997/05/29 00:10:02 curt
84 Initial Flight Gear revision.
87 ----------------------------------------------------------------------------
91 ----------------------------------------------------------------------------
95 ----------------------------------------------------------------------------
99 ----------------------------------------------------------------------------
103 ----------------------------------------------------------------------------
107 --------------------------------------------------------------------------*/
109 #include "ls_types.h"
110 #include "ls_constants.h"
111 #include "ls_generic.h"
112 #include "ls_cockpit.h"
115 static void sub3( DATA v1[], DATA v2[], DATA result[] )
117 result[0] = v1[0] - v2[0];
118 result[1] = v1[1] - v2[1];
119 result[2] = v1[2] - v2[2];
122 static void add3( DATA v1[], DATA v2[], DATA result[] )
124 result[0] = v1[0] + v2[0];
125 result[1] = v1[1] + v2[1];
126 result[2] = v1[2] + v2[2];
129 static void cross3( DATA v1[], DATA v2[], DATA result[] )
131 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
132 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
133 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
136 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
138 result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
139 result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
140 result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
143 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
145 result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
146 result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
147 result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
150 static void clear3( DATA v[] )
152 v[0] = 0.; v[1] = 0.; v[2] = 0.;
155 void navion_gear( SCALAR dt, int Initialize ) {
156 char rcsid[] = "$Id$";
159 * Aircraft specific initializations and data goes here
164 static int num_wheels = NUM_WHEELS; /* number of wheels */
165 static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations */
167 { 10., 0., 4. }, /* in feet */
171 static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
172 { 1500., 5000., 5000. };
173 static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
174 { 100., 150., 150. };
175 static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
176 { 0., 0., 0. }; /* 0 = none, 1 = full */
177 static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
178 { 0., 0., 0.}; /* radians, +CW */
180 * End of aircraft specific code
184 * Constants & coefficients for tyres on tarmac - ref [1]
187 /* skid function looks like:
193 * sliding_mu | / +------
196 * +--+------------------------>
203 static DATA sliding_mu = 0.5;
204 static DATA rolling_mu = 0.01;
205 static DATA max_brake_mu = 0.6;
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_body_v[3]; /* wheel velocity, X-Y-Z */
217 DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
218 DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
219 DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
220 DATA reaction_normal_force; /* wheel normal (to rwy) force */
221 DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
222 DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
223 DATA forward_mu, sideward_mu; /* friction coefficients */
224 DATA beta_mu; /* breakout friction slope */
225 DATA forward_wheel_force, sideward_wheel_force;
227 int i; /* per wheel loop counter */
230 * Execution starts here
233 beta_mu = max_mu/(skid_v-bkout_v);
234 clear3( F_gear_v ); /* Initialize sum of forces... */
235 clear3( M_gear_v ); /* ...and moments */
238 * Put aircraft specific executable code here
241 percent_brake[1] = Brake_pct[0];
242 percent_brake[2] = Brake_pct[1];
244 caster_angle_rad[0] = 0.03*Rudder_pedal;
246 for (i=0;i<num_wheels;i++) /* Loop for each wheel */
248 /*========================================*/
249 /* Calculate wheel position w.r.t. runway */
250 /*========================================*/
252 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
254 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
256 /* then converting to local (North-East-Down) axes... */
258 multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
260 /* Runway axes correction - third element is Altitude, not (-)Z... */
262 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
264 /* Add wheel offset to cg location in local axes */
266 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
268 /* remove Runway axes correction so right hand rule applies */
270 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
272 /*============================*/
273 /* Calculate wheel velocities */
274 /*============================*/
276 /* contribution due to angular rates */
278 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
280 /* transform into local axes */
282 multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
284 /* plus contribution due to cg velocities */
286 add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
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;
308 /* Calculate normal load force (simple spring constant) */
310 reaction_normal_force = 0.;
311 if( d_wheel_rwy_local_v[2] < 0. )
313 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
314 - v_wheel_local_v[2]*spring_damping[i];
315 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
316 /* to prevent damping component from swamping spring component */
319 /* Calculate friction coefficients */
321 forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
322 abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
323 sideward_mu = sliding_mu;
324 if (abs_v_wheel_sideward < skid_v)
325 sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
326 if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
328 /* Calculate foreward and sideward reaction forces */
330 forward_wheel_force = forward_mu*reaction_normal_force;
331 sideward_wheel_force = sideward_mu*reaction_normal_force;
332 if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
333 if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
335 /* Rotate into local (N-E-D) axes */
337 f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
338 - sideward_wheel_force*sin_wheel_hdg_angle;
339 f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
340 + sideward_wheel_force*cos_wheel_hdg_angle;
341 f_wheel_local_v[2] = reaction_normal_force;
343 /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
345 mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
347 /* Calculate moments from force and offsets in body axes */
349 cross3( d_wheel_cg_body_v, tempF, tempM );
351 /* Sum forces and moments across all wheels */
353 add3( tempF, F_gear_v, F_gear_v );
354 add3( tempM, M_gear_v, M_gear_v );