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.4 2001/07/30 20:53:54 curt
40 Various MSVC tweaks and warning fixes.
42 Revision 1.3 2000/06/12 18:52:37 curt
43 Added differential braking (Alex and David).
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.
67 Revision 1.6 1998/10/17 01:34:16 curt
70 Revision 1.5 1998/09/29 02:03:00 curt
71 Added a brake + autopilot mods.
73 Revision 1.4 1998/08/06 12:46:40 curt
76 Revision 1.3 1998/02/03 23:20:18 curt
77 Lots of little tweaks to fix various consistency problems discovered by
78 Solaris' CC. Fixed a bug in fg_debug.c with how the fgPrintf() wrapper
79 passed arguments along to the real printf(). Also incorporated HUD changes
82 Revision 1.2 1998/01/19 18:40:29 curt
83 Tons of little changes to clean up the code and to remove fatal errors
84 when building with the c++ compiler.
86 Revision 1.1 1997/05/29 00:10:02 curt
87 Initial Flight Gear revision.
90 ----------------------------------------------------------------------------
94 ----------------------------------------------------------------------------
98 ----------------------------------------------------------------------------
102 ----------------------------------------------------------------------------
106 ----------------------------------------------------------------------------
110 --------------------------------------------------------------------------*/
112 #include "ls_types.h"
113 #include "ls_constants.h"
114 #include "ls_generic.h"
115 #include "ls_cockpit.h"
118 static void sub3( DATA v1[], DATA v2[], DATA result[] )
120 result[0] = v1[0] - v2[0];
121 result[1] = v1[1] - v2[1];
122 result[2] = v1[2] - v2[2];
125 static void add3( DATA v1[], DATA v2[], DATA result[] )
127 result[0] = v1[0] + v2[0];
128 result[1] = v1[1] + v2[1];
129 result[2] = v1[2] + v2[2];
132 static void cross3( DATA v1[], DATA v2[], DATA result[] )
134 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
135 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
136 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
139 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
141 result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
142 result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
143 result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
146 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
148 result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
149 result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
150 result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
153 static void clear3( DATA v[] )
155 v[0] = 0.; v[1] = 0.; v[2] = 0.;
158 void navion_gear( SCALAR dt, int Initialize ) {
159 char rcsid[] = "$Id$";
162 * Aircraft specific initializations and data goes here
167 static int num_wheels = NUM_WHEELS; /* number of wheels */
168 static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations */
170 { 10., 0., 4. }, /* in feet */
174 static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
175 { 1500., 5000., 5000. };
176 static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
177 { 100., 150., 150. };
178 static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
179 { 0., 0., 0. }; /* 0 = none, 1 = full */
180 static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
181 { 0., 0., 0.}; /* radians, +CW */
183 * End of aircraft specific code
187 * Constants & coefficients for tyres on tarmac - ref [1]
190 /* skid function looks like:
196 * sliding_mu | / +------
199 * +--+------------------------>
206 static DATA sliding_mu = 0.5;
207 static DATA rolling_mu = 0.01;
208 static DATA max_brake_mu = 0.6;
209 static DATA max_mu = 0.8;
210 static DATA bkout_v = 0.1;
211 static DATA skid_v = 1.0;
213 * Local data variables
216 DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
217 DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
218 DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
219 // DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
220 DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
221 DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
222 DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
223 DATA reaction_normal_force; /* wheel normal (to rwy) force */
224 DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
225 DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
226 DATA forward_mu, sideward_mu; /* friction coefficients */
227 DATA beta_mu; /* breakout friction slope */
228 DATA forward_wheel_force, sideward_wheel_force;
230 int i; /* per wheel loop counter */
233 * Execution starts here
236 beta_mu = max_mu/(skid_v-bkout_v);
237 clear3( F_gear_v ); /* Initialize sum of forces... */
238 clear3( M_gear_v ); /* ...and moments */
241 * Put aircraft specific executable code here
244 percent_brake[1] = Brake_pct[0];
245 percent_brake[2] = Brake_pct[1];
247 caster_angle_rad[0] = 0.03*Rudder_pedal;
249 for (i=0;i<num_wheels;i++) /* Loop for each wheel */
251 /*========================================*/
252 /* Calculate wheel position w.r.t. runway */
253 /*========================================*/
255 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
257 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
259 /* then converting to local (North-East-Down) axes... */
261 multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
263 /* Runway axes correction - third element is Altitude, not (-)Z... */
265 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
267 /* Add wheel offset to cg location in local axes */
269 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
271 /* remove Runway axes correction so right hand rule applies */
273 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
275 /*============================*/
276 /* Calculate wheel velocities */
277 /*============================*/
279 /* contribution due to angular rates */
281 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
283 /* transform into local axes */
285 multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
287 /* plus contribution due to cg velocities */
289 add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
292 /*===========================================*/
293 /* Calculate forces & moments for this wheel */
294 /*===========================================*/
296 /* Add any anticipation, or frame lead/prediction, here... */
298 /* no lead used at present */
300 /* Calculate sideward and forward velocities of the wheel
301 in the runway plane */
303 cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
304 sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
306 v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
307 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
308 v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
309 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
311 /* Calculate normal load force (simple spring constant) */
313 reaction_normal_force = 0.;
314 if( d_wheel_rwy_local_v[2] < 0. )
316 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
317 - v_wheel_local_v[2]*spring_damping[i];
318 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
319 /* to prevent damping component from swamping spring component */
322 /* Calculate friction coefficients */
324 forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
325 abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
326 sideward_mu = sliding_mu;
327 if (abs_v_wheel_sideward < skid_v)
328 sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
329 if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
331 /* Calculate foreward and sideward reaction forces */
333 forward_wheel_force = forward_mu*reaction_normal_force;
334 sideward_wheel_force = sideward_mu*reaction_normal_force;
335 if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
336 if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
338 /* Rotate into local (N-E-D) axes */
340 f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
341 - sideward_wheel_force*sin_wheel_hdg_angle;
342 f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
343 + sideward_wheel_force*cos_wheel_hdg_angle;
344 f_wheel_local_v[2] = reaction_normal_force;
346 /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
348 mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
350 /* Calculate moments from force and offsets in body axes */
352 cross3( d_wheel_cg_body_v, tempF, tempM );
354 /* Sum forces and moments across all wheels */
356 add3( tempF, F_gear_v, F_gear_v );
357 add3( tempM, M_gear_v, M_gear_v );