1 /***************************************************************************
5 ----------------------------------------------------------------------------
7 FUNCTION: Landing gear model for example simulation
9 ----------------------------------------------------------------------------
11 MODULE STATUS: developmental
13 ----------------------------------------------------------------------------
15 GENEALOGY: Renamed navion_gear.c originally created 931012 by E. B. Jackson
18 ----------------------------------------------------------------------------
20 DESIGNED BY: E. B. Jackson
22 CODED BY: E. B. Jackson
24 MAINTAINED BY: E. B. Jackson
26 ----------------------------------------------------------------------------
32 931218 Added navion.h header to allow connection with
33 aileron displacement for nosewheel steering. EBJ
34 940511 Connected nosewheel to rudder pedal; adjusted gain.
40 Revision 1.2 1999/06/21 03:01:47 curt
41 Updated for both JSBsim and Tony Peden's c172 flight model.
43 Revision 1.1.1.1 1999/04/05 21:32:45 curt
44 Start of 0.6.x branch.
46 Revision 1.6 1998/10/17 01:34:16 curt
49 Revision 1.5 1998/09/29 02:03:00 curt
50 Added a brake + autopilot mods.
52 Revision 1.4 1998/08/06 12:46:40 curt
55 Revision 1.3 1998/02/03 23:20:18 curt
56 Lots of little tweaks to fix various consistency problems discovered by
57 Solaris' CC. Fixed a bug in fg_debug.c with how the fgPrintf() wrapper
58 passed arguments along to the real printf(). Also incorporated HUD changes
61 Revision 1.2 1998/01/19 18:40:29 curt
62 Tons of little changes to clean up the code and to remove fatal errors
63 when building with the c++ compiler.
65 Revision 1.1 1997/05/29 00:10:02 curt
66 Initial Flight Gear revision.
69 ----------------------------------------------------------------------------
73 ----------------------------------------------------------------------------
77 ----------------------------------------------------------------------------
81 ----------------------------------------------------------------------------
85 ----------------------------------------------------------------------------
89 --------------------------------------------------------------------------*/
92 #include "ls_constants.h"
93 #include "ls_generic.h"
94 #include "ls_cockpit.h"
97 void sub3( DATA v1[], DATA v2[], DATA result[] )
99 result[0] = v1[0] - v2[0];
100 result[1] = v1[1] - v2[1];
101 result[2] = v1[2] - v2[2];
104 void add3( DATA v1[], DATA v2[], DATA result[] )
106 result[0] = v1[0] + v2[0];
107 result[1] = v1[1] + v2[1];
108 result[2] = v1[2] + v2[2];
111 void cross3( DATA v1[], DATA v2[], DATA result[] )
113 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
114 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
115 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
118 void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
120 result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
121 result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
122 result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
125 void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
127 result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
128 result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
129 result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
132 void clear3( DATA v[] )
134 v[0] = 0.; v[1] = 0.; v[2] = 0.;
137 void gear( SCALAR dt, int Initialize ) {
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 /* replace with cockpit brake handle connection code */
224 percent_brake[1] = Brake_pct;
225 percent_brake[2] = percent_brake[1];
227 caster_angle_rad[0] = 0.03*Rudder_pedal;
229 for (i=0;i<num_wheels;i++) /* Loop for each wheel */
231 /*========================================*/
232 /* Calculate wheel position w.r.t. runway */
233 /*========================================*/
235 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
237 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
239 /* then converting to local (North-East-Down) axes... */
241 multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
243 /* Runway axes correction - third element is Altitude, not (-)Z... */
245 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
247 /* Add wheel offset to cg location in local axes */
249 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
251 /* remove Runway axes correction so right hand rule applies */
253 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
255 /*============================*/
256 /* Calculate wheel velocities */
257 /*============================*/
259 /* contribution due to angular rates */
261 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
263 /* transform into local axes */
265 multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
267 /* plus contribution due to cg velocities */
269 add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
272 /*===========================================*/
273 /* Calculate forces & moments for this wheel */
274 /*===========================================*/
276 /* Add any anticipation, or frame lead/prediction, here... */
278 /* no lead used at present */
280 /* Calculate sideward and forward velocities of the wheel
281 in the runway plane */
283 cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
284 sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
286 v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
287 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
288 v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
289 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
291 /* Calculate normal load force (simple spring constant) */
293 reaction_normal_force = 0.;
294 if( d_wheel_rwy_local_v[2] < 0. )
296 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
297 - v_wheel_local_v[2]*spring_damping[i];
298 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
299 /* to prevent damping component from swamping spring component */
302 /* Calculate friction coefficients */
304 forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
305 abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
306 sideward_mu = sliding_mu;
307 if (abs_v_wheel_sideward < skid_v)
308 sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
309 if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
311 /* Calculate foreward and sideward reaction forces */
313 forward_wheel_force = forward_mu*reaction_normal_force;
314 sideward_wheel_force = sideward_mu*reaction_normal_force;
315 if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
316 if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
318 /* Rotate into local (N-E-D) axes */
320 f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
321 - sideward_wheel_force*sin_wheel_hdg_angle;
322 f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
323 + sideward_wheel_force*cos_wheel_hdg_angle;
324 f_wheel_local_v[2] = reaction_normal_force;
326 /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
328 mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
330 /* Calculate moments from force and offsets in body axes */
332 cross3( d_wheel_cg_body_v, tempF, tempM );
334 /* Sum forces and moments across all wheels */
336 add3( tempF, F_gear_v, F_gear_v );
337 add3( tempM, M_gear_v, M_gear_v );