1 /***************************************************************************
2
3 TITLE: gear
4
5 ----------------------------------------------------------------------------
6
7 FUNCTION: Landing gear model for example simulation
8
9 ----------------------------------------------------------------------------
10
11 MODULE STATUS: developmental
12
13 ----------------------------------------------------------------------------
14
15 GENEALOGY: Created 931012 by E. B. Jackson
16
17 ----------------------------------------------------------------------------
18
19 DESIGNED BY: E. B. Jackson
20
21 CODED BY: E. B. Jackson
22
23 MAINTAINED BY: E. B. Jackson
24
25 ----------------------------------------------------------------------------
26
27 MODIFICATION HISTORY:
28
29 ----------------------------------------------------------------------------
30
31 REFERENCES:
32
33 ----------------------------------------------------------------------------
34
35 CALLED BY:
36
37 ----------------------------------------------------------------------------
38
39 CALLS TO:
40
41 ----------------------------------------------------------------------------
42
43 INPUTS:
44
45 ----------------------------------------------------------------------------
46
47 OUTPUTS:
48
49 --------------------------------------------------------------------------*/
50 #include <math.h>
51 #include "ls_types.h"
52 #include "ls_constants.h"
53 #include "ls_generic.h"
54 #include "ls_cockpit.h"
55
56 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
57
58
sub3(DATA v1[],DATA v2[],DATA result[])59 static void sub3( DATA v1[], DATA v2[], DATA result[] )
60 {
61 result[0] = v1[0] - v2[0];
62 result[1] = v1[1] - v2[1];
63 result[2] = v1[2] - v2[2];
64 }
65
add3(DATA v1[],DATA v2[],DATA result[])66 static void add3( DATA v1[], DATA v2[], DATA result[] )
67 {
68 result[0] = v1[0] + v2[0];
69 result[1] = v1[1] + v2[1];
70 result[2] = v1[2] + v2[2];
71 }
72
cross3(DATA v1[],DATA v2[],DATA result[])73 static void cross3( DATA v1[], DATA v2[], DATA result[] )
74 {
75 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
76 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
77 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
78 }
79
multtrans3x3by3(DATA m[][3],DATA v[],DATA result[])80 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
81 {
82 result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
83 result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
84 result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
85 }
86
mult3x3by3(DATA m[][3],DATA v[],DATA result[])87 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
88 {
89 result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
90 result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
91 result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
92 }
93
clear3(DATA v[])94 static void clear3( DATA v[] )
95 {
96 v[0] = 0.; v[1] = 0.; v[2] = 0.;
97 }
98
basic_gear()99 void basic_gear()
100 {
101 //char rcsid[] = "junk";
102 #define NUM_WHEELS 4
103
104 // char gear_strings[NUM_WHEELS][12]={"nose","right main", "left main", "tail skid"};
105 /*
106 * Aircraft specific initializations and data goes here
107 */
108
109
110 static int num_wheels = NUM_WHEELS; /* number of wheels */
111 static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations,full extension */
112 {
113 { .422, 0., .29 }, /*nose*/ /* in feet */
114 { 0.026, 0.006, .409 }, /*right main*/
115 { 0.026, -.006, .409 }, /*left main*/
116 { -1.32, 0, .17 } /*tail skid */
117 };
118 // static DATA gear_travel[NUM_WHEELS] = /*in Z-axis*/
119 // { -0.5, 2.5, 2.5, 0};
120 static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
121 { 2., .65, .65, 1. };
122 static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
123 { 1., .3, .3, .5 };
124 static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
125 { 0., 0., 0., 0. }; /* 0 = none, 1 = full */
126 static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
127 { 0., 0., 0., 0}; /* radians, +CW */
128 /*
129 * End of aircraft specific code
130 */
131
132 /*
133 * Constants & coefficients for tyres on tarmac - ref [1]
134 */
135
136 /* skid function looks like:
137 *
138 * mu ^
139 * |
140 * max_mu | +
141 * | /|
142 * sliding_mu | / +------
143 * | /
144 * | /
145 * +--+------------------------>
146 * | | | sideward V
147 * 0 bkout skid
148 * V V
149 */
150
151
152 static int it_rolls[NUM_WHEELS] = { 1,1,1,0};
153 static DATA sliding_mu[NUM_WHEELS] = { 0.5, 0.5, 0.5, 0.3};
154 static DATA rolling_mu[NUM_WHEELS] = { 0.01, 0.01, 0.01, 0.0};
155 static DATA max_brake_mu[NUM_WHEELS] ={ 0.0, 0.6, 0.6, 0.0};
156 static DATA max_mu = 0.8;
157 static DATA bkout_v = 0.1;
158 static DATA skid_v = 1.0;
159 /*
160 * Local data variables
161 */
162
163 DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
164 DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
165 DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
166 DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
167 // DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
168 DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
169 DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
170 // DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
171 // DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/
172 DATA temp3a[3];
173 // DATA temp3b[3];
174 DATA tempF[3];
175 DATA tempM[3];
176 DATA reaction_normal_force; /* wheel normal (to rwy) force */
177 DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
178 DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
179 DATA forward_mu, sideward_mu; /* friction coefficients */
180 DATA beta_mu; /* breakout friction slope */
181 DATA forward_wheel_force, sideward_wheel_force;
182
183 int i; /* per wheel loop counter */
184
185 /*
186 * Execution starts here
187 */
188
189 beta_mu = max_mu/(skid_v-bkout_v);
190 clear3( F_gear_v ); /* Initialize sum of forces... */
191 clear3( M_gear_v ); /* ...and moments */
192
193 /*
194 * Put aircraft specific executable code here
195 */
196
197 percent_brake[1] = Brake_pct[0];
198 percent_brake[2] = Brake_pct[1];
199
200 caster_angle_rad[0] =
201 (0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
202
203
204 for (i=0;i<num_wheels;i++) /* Loop for each wheel */
205 {
206 /* printf("%s:\n",gear_strings[i]); */
207
208
209
210 /*========================================*/
211 /* Calculate wheel position w.r.t. runway */
212 /*========================================*/
213
214
215 /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
216
217
218 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
219
220 sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
221
222 /* then converting to local (North-East-Down) axes... */
223
224 multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
225
226
227 /* Runway axes correction - third element is Altitude, not (-)Z... */
228
229 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
230
231 /* Add wheel offset to cg location in local axes */
232
233 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
234
235 /* remove Runway axes correction so right hand rule applies */
236
237 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
238
239 /*============================*/
240 /* Calculate wheel velocities */
241 /*============================*/
242
243 /* contribution due to angular rates */
244
245 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
246
247 /* transform into local axes */
248
249 multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
250
251 /* plus contribution due to cg velocities */
252
253 add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
254
255 clear3(f_wheel_local_v);
256 reaction_normal_force=0;
257 if( HEIGHT_AGL_WHEEL < 0. )
258 /*the wheel is underground -- which implies ground contact
259 so calculate reaction forces */
260 {
261 /*===========================================*/
262 /* Calculate forces & moments for this wheel */
263 /*===========================================*/
264
265 /* Add any anticipation, or frame lead/prediction, here... */
266
267 /* no lead used at present */
268
269 /* Calculate sideward and forward velocities of the wheel
270 in the runway plane */
271
272 cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
273 sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
274
275 v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
276 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
277 v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
278 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
279
280
281 /* Calculate normal load force (simple spring constant) */
282
283 reaction_normal_force = 0.;
284
285 reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
286 - v_wheel_local_v[2]*spring_damping[i];
287 /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
288
289 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
290 /* to prevent damping component from swamping spring component */
291
292
293 /* Calculate friction coefficients */
294
295 if(it_rolls[i])
296 {
297 forward_mu = (max_brake_mu[i] - rolling_mu[i])*percent_brake[i] + rolling_mu[i];
298 abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
299 sideward_mu = sliding_mu[i];
300 if (abs_v_wheel_sideward < skid_v)
301 sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
302 if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
303 }
304 else
305 {
306 forward_mu=sliding_mu[i];
307 sideward_mu=sliding_mu[i];
308 }
309
310 /* Calculate foreward and sideward reaction forces */
311
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;
316 /* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
317 */
318 /* Rotate into local (N-E-D) axes */
319
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;
325
326 /* 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 );
328
329 /* Calculate moments from force and offsets in body axes */
330
331 cross3( d_wheel_cg_body_v, tempF, tempM );
332
333 /* Sum forces and moments across all wheels */
334
335 add3( tempF, F_gear_v, F_gear_v );
336 add3( tempM, M_gear_v, M_gear_v );
337
338
339 }
340
341
342
343 /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
344 /* printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear); */
345 /* printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
346
347
348 }
349 }
350