1 /**********************************************************************
3 FILENAME: uiuc_gear.cpp
5 ----------------------------------------------------------------------
7 DESCRIPTION: determine the gear forces and moments
9 ----------------------------------------------------------------------
13 ----------------------------------------------------------------------
15 REFERENCES: based on c172_gear by Tony Peden and others
17 ----------------------------------------------------------------------
19 HISTORY: 03/09/2001 initial release
21 ----------------------------------------------------------------------
23 AUTHOR(S): David Megginson <david@megginson.com
25 ----------------------------------------------------------------------
29 ----------------------------------------------------------------------
33 ----------------------------------------------------------------------
37 ----------------------------------------------------------------------
39 CALLED BY: uiuc_wrapper.cpp
41 ----------------------------------------------------------------------
45 ----------------------------------------------------------------------
47 COPYRIGHT: (c) 2001 by David Megginson
49 This program is free software; you can redistribute it and/or
50 modify it under the terms of the GNU General Public License
51 as published by the Free Software Foundation.
53 This program is distributed in the hope that it will be useful,
54 but WITHOUT ANY WARRANTY; without even the implied warranty of
55 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
56 GNU General Public License for more details.
58 You should have received a copy of the GNU General Public License
59 along with this program; if not, write to the Free Software
60 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
62 **********************************************************************/
68 #include <simgear/compiler.h>
69 #include <simgear/misc/sg_path.hxx>
70 #include <Aircraft/aircraft.hxx>
71 #include <Main/fg_props.hxx>
73 #include "uiuc_gear.h"
80 #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2]
83 static void sub3( DATA v1[], DATA v2[], DATA result[] )
85 result[0] = v1[0] - v2[0];
86 result[1] = v1[1] - v2[1];
87 result[2] = v1[2] - v2[2];
90 static void add3( DATA v1[], DATA v2[], DATA result[] )
92 result[0] = v1[0] + v2[0];
93 result[1] = v1[1] + v2[1];
94 result[2] = v1[2] + v2[2];
97 static void cross3( DATA v1[], DATA v2[], DATA result[] )
99 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
100 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
101 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
104 static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
106 result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
107 result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
108 result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
111 static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
113 result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
114 result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
115 result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
118 static void clear3( DATA v[] )
120 v[0] = 0.; v[1] = 0.; v[2] = 0.;
127 * Aircraft specific initializations and data goes here
130 static DATA percent_brake[MAX_GEAR] = /* percent applied braking */
134 0., 0., 0., 0. }; /* 0 = none, 1 = full */
135 static DATA caster_angle_rad[MAX_GEAR] = /* steerable tires - in */
139 0., 0., 0., 0. }; /* radians, +CW */
141 * End of aircraft specific code
145 * Constants & coefficients for tyres on tarmac - ref [1]
148 /* skid function looks like:
154 * sliding_mu | / +------
157 * +--+------------------------>
164 static int it_rolls[MAX_GEAR] =
169 static DATA sliding_mu[MAX_GEAR] =
170 { 0.5, 0.5, 0.5, 0.3,
173 0.3, 0.3, 0.3, 0.3 };
174 static DATA max_brake_mu[MAX_GEAR] =
175 { 0.0, 0.6, 0.6, 0.0,
178 0.0, 0.0, 0.0, 0.0 };
179 static DATA max_mu = 0.8;
180 static DATA bkout_v = 0.1;
181 static DATA skid_v = 1.0;
183 * Local data variables
186 DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
187 DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
188 DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
189 DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
190 // DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
191 DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
192 DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
193 // DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
194 // DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/
199 DATA reaction_normal_force; /* wheel normal (to rwy) force */
200 DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
201 DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
202 DATA forward_mu, sideward_mu; /* friction coefficients */
203 DATA beta_mu; /* breakout friction slope */
204 DATA forward_wheel_force, sideward_wheel_force;
206 int i; /* per wheel loop counter */
209 * Execution starts here
212 beta_mu = max_mu/(skid_v-bkout_v);
213 clear3( F_gear_v ); /* Initialize sum of forces... */
214 clear3( M_gear_v ); /* ...and moments */
217 * Put aircraft specific executable code here
220 percent_brake[1] = Brake_pct[0];
221 percent_brake[2] = Brake_pct[1];
223 caster_angle_rad[0] =
224 (0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
227 for (i=0;i<MAX_GEAR;i++) /* Loop for each wheel */
229 // Execute only if the gear has been defined
238 /*========================================*/
239 /* Calculate wheel position w.r.t. runway */
240 /*========================================*/
242 /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
244 /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
246 sub3( D_gear_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
248 /* then converting to local (North-East-Down) axes... */
250 multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
253 /* Runway axes correction - third element is Altitude, not (-)Z... */
255 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
257 /* Add wheel offset to cg location in local axes */
259 add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
261 /* remove Runway axes correction so right hand rule applies */
263 d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
265 /*============================*/
266 /* Calculate wheel velocities */
267 /*============================*/
269 /* contribution due to angular rates */
271 cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
273 /* transform into local axes */
275 multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
277 /* plus contribution due to cg velocities */
279 add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
281 clear3(f_wheel_local_v);
282 reaction_normal_force=0;
283 static const SGPropertyNode * gear_wow
284 = fgGetNode("/gear/gear[0]/wow", false);
285 static const SGPropertyNode * gear_wow1
286 = fgGetNode("/gear/gear[1]/wow", false);
287 static const SGPropertyNode * gear_wow2
288 = fgGetNode("/gear/gear[2]/wow", false);
289 fgSetBool("/gear/gear[0]/wow", false);
290 fgSetBool("/gear/gear[1]/wow", false);
291 fgSetBool("/gear/gear[2]/wow", false);
292 if( HEIGHT_AGL_WHEEL < 0. )
293 /*the wheel is underground -- which implies ground contact
294 so calculate reaction forces */
296 //set the property - weight on wheels
299 // fgSetBool("/gear/gear[0]/wow", true);
303 // fgSetBool("/gear/gear[1]/wow", true);
307 // fgSetBool("/gear/gear[2]/wow", true);
310 /*===========================================*/
311 /* Calculate forces & moments for this wheel */
312 /*===========================================*/
314 /* Add any anticipation, or frame lead/prediction, here... */
316 /* no lead used at present */
318 /* Calculate sideward and forward velocities of the wheel
319 in the runway plane */
321 cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
322 sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
324 v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
325 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
326 v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
327 - v_wheel_local_v[0]*sin_wheel_hdg_angle;
330 /* Calculate normal load force (simple spring constant) */
332 reaction_normal_force = 0.;
334 reaction_normal_force = kgear[i]*d_wheel_rwy_local_v[2]
335 - v_wheel_local_v[2]*cgear[i];
336 /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
338 if (reaction_normal_force > 0.) reaction_normal_force = 0.;
339 /* to prevent damping component from swamping spring component */
342 /* Calculate friction coefficients */
346 forward_mu = (max_brake_mu[i] - muGear[i])*percent_brake[i] + muGear[i];
347 abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
348 sideward_mu = sliding_mu[i];
349 if (abs_v_wheel_sideward < skid_v)
350 sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
351 if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
355 forward_mu=sliding_mu[i];
356 sideward_mu=sliding_mu[i];
359 /* Calculate foreward and sideward reaction forces */
361 forward_wheel_force = forward_mu*reaction_normal_force;
362 sideward_wheel_force = sideward_mu*reaction_normal_force;
363 if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
364 if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
365 /* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
367 /* Rotate into local (N-E-D) axes */
369 f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
370 - sideward_wheel_force*sin_wheel_hdg_angle;
371 f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
372 + sideward_wheel_force*cos_wheel_hdg_angle;
373 f_wheel_local_v[2] = reaction_normal_force;
375 /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
376 mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
378 /* Calculate moments from force and offsets in body axes */
380 cross3( d_wheel_cg_body_v, tempF, tempM );
382 /* Sum forces and moments across all wheels */
384 fgSetBool("/gear/gear[1]/wow", true);
387 add3( tempF, F_gear_v, F_gear_v );
388 add3( tempM, M_gear_v, M_gear_v );
395 /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
397 /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
398 printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
404 // end uiuc_engine.cpp