X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FUIUCModel%2Fuiuc_gear.cpp;h=31a87d0b98493960bba9e929fa683f3901c26fbf;hb=70b4f38ebccbafea664ed2310eaf990bd2b9edd8;hp=99df9b1cb872fa05dae07e751862124cedeee188;hpb=71f08e795dca21cac1293ce89abb62024d4a16cc;p=flightgear.git diff --git a/src/FDM/UIUCModel/uiuc_gear.cpp b/src/FDM/UIUCModel/uiuc_gear.cpp index 99df9b1cb..31a87d0b9 100644 --- a/src/FDM/UIUCModel/uiuc_gear.cpp +++ b/src/FDM/UIUCModel/uiuc_gear.cpp @@ -1,5 +1,5 @@ /********************************************************************** - + FILENAME: uiuc_gear.cpp ---------------------------------------------------------------------- @@ -57,314 +57,343 @@ You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software - Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, - USA or view http://www.gnu.org/copyleft/gpl.html. + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. **********************************************************************/ +#ifdef HAVE_CONFIG_H +# include +#endif + #include +#include +#include
#include "uiuc_gear.h" -SG_USING_STD(cerr); - - #define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2] static void sub3( DATA v1[], DATA v2[], DATA result[] ) { - result[0] = v1[0] - v2[0]; - result[1] = v1[1] - v2[1]; - result[2] = v1[2] - v2[2]; + result[0] = v1[0] - v2[0]; + result[1] = v1[1] - v2[1]; + result[2] = v1[2] - v2[2]; } static void add3( DATA v1[], DATA v2[], DATA result[] ) { - result[0] = v1[0] + v2[0]; - result[1] = v1[1] + v2[1]; - result[2] = v1[2] + v2[2]; + result[0] = v1[0] + v2[0]; + result[1] = v1[1] + v2[1]; + result[2] = v1[2] + v2[2]; } static void cross3( DATA v1[], DATA v2[], DATA result[] ) { - result[0] = v1[1]*v2[2] - v1[2]*v2[1]; - result[1] = v1[2]*v2[0] - v1[0]*v2[2]; - result[2] = v1[0]*v2[1] - v1[1]*v2[0]; + result[0] = v1[1]*v2[2] - v1[2]*v2[1]; + result[1] = v1[2]*v2[0] - v1[0]*v2[2]; + result[2] = v1[0]*v2[1] - v1[1]*v2[0]; } static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] ) { - result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2]; - result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2]; - result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2]; + result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2]; + result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2]; + result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2]; } static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] ) { - result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2]; - result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2]; - result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]; + result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2]; + result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2]; + result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]; } static void clear3( DATA v[] ) { - v[0] = 0.; v[1] = 0.; v[2] = 0.; + v[0] = 0.; v[1] = 0.; v[2] = 0.; } void uiuc_gear() { - + /* * Aircraft specific initializations and data goes here */ - - static DATA percent_brake[MAX_GEAR] = /* percent applied braking */ - { 0., 0., 0., 0., - 0., 0., 0., 0., - 0., 0., 0., 0., - 0., 0., 0., 0. }; /* 0 = none, 1 = full */ - static DATA caster_angle_rad[MAX_GEAR] = /* steerable tires - in */ - { 0., 0., 0., 0., - 0., 0., 0., 0., - 0., 0., 0., 0., - 0., 0., 0., 0. }; /* radians, +CW */ + + static DATA percent_brake[MAX_GEAR] = /* percent applied braking */ + { 0., 0., 0., 0., + 0., 0., 0., 0., + 0., 0., 0., 0., + 0., 0., 0., 0. }; /* 0 = none, 1 = full */ + static DATA caster_angle_rad[MAX_GEAR] = /* steerable tires - in */ + { 0., 0., 0., 0., + 0., 0., 0., 0., + 0., 0., 0., 0., + 0., 0., 0., 0. }; /* radians, +CW */ /* * End of aircraft specific code */ - + /* * Constants & coefficients for tyres on tarmac - ref [1] */ - - /* skid function looks like: - * - * mu ^ - * | - * max_mu | + - * | /| - * sliding_mu | / +------ - * | / - * | / - * +--+------------------------> - * | | | sideward V - * 0 bkout skid - * V V - */ + + /* skid function looks like: + * + * mu ^ + * | + * max_mu | + + * | /| + * sliding_mu | / +------ + * | / + * | / + * +--+------------------------> + * | | | sideward V + * 0 bkout skid + * V V + */ - static int it_rolls[MAX_GEAR] = - { 1, 1, 1, 0, - 0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0 }; - static DATA sliding_mu[MAX_GEAR] = - { 0.5, 0.5, 0.5, 0.3, - 0.3, 0.3, 0.3, 0.3, - 0.3, 0.3, 0.3, 0.3, - 0.3, 0.3, 0.3, 0.3 }; - static DATA max_brake_mu[MAX_GEAR] = - { 0.0, 0.6, 0.6, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0 }; - static DATA max_mu = 0.8; - static DATA bkout_v = 0.1; - static DATA skid_v = 1.0; + static int it_rolls[MAX_GEAR] = + { 1, 1, 1, 0, + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0 }; + static DATA sliding_mu[MAX_GEAR] = + { 0.5, 0.5, 0.5, 0.3, + 0.3, 0.3, 0.3, 0.3, + 0.3, 0.3, 0.3, 0.3, + 0.3, 0.3, 0.3, 0.3 }; + static DATA max_brake_mu[MAX_GEAR] = + { 0.0, 0.6, 0.6, 0.0, + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0 }; + static DATA max_mu = 0.8; + static DATA bkout_v = 0.1; + static DATA skid_v = 1.0; /* * Local data variables */ - - DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */ - DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */ - DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */ - DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/ - // DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */ - DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */ - DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */ - // DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/ - // DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/ - DATA temp3a[3]; - // DATA temp3b[3]; - DATA tempF[3]; - DATA tempM[3]; - DATA reaction_normal_force; /* wheel normal (to rwy) force */ - DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */ - DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward; - DATA forward_mu, sideward_mu; /* friction coefficients */ - DATA beta_mu; /* breakout friction slope */ - DATA forward_wheel_force, sideward_wheel_force; - - int i; /* per wheel loop counter */ - + + DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */ + DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */ + DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */ + DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/ + // DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */ + DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */ + DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */ + // DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/ + // DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/ + DATA temp3a[3]; + // DATA temp3b[3]; + DATA tempF[3]; + DATA tempM[3]; + DATA reaction_normal_force; /* wheel normal (to rwy) force */ + DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */ + DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward; + DATA forward_mu, sideward_mu; /* friction coefficients */ + DATA beta_mu; /* breakout friction slope */ + DATA forward_wheel_force, sideward_wheel_force; + + int i; /* per wheel loop counter */ + /* * Execution starts here */ - - beta_mu = max_mu/(skid_v-bkout_v); - clear3( F_gear_v ); /* Initialize sum of forces... */ - clear3( M_gear_v ); /* ...and moments */ - + + beta_mu = max_mu/(skid_v-bkout_v); + clear3( F_gear_v ); /* Initialize sum of forces... */ + clear3( M_gear_v ); /* ...and moments */ + /* * Put aircraft specific executable code here */ - - percent_brake[1] = Brake_pct[0]; - percent_brake[2] = Brake_pct[1]; - - caster_angle_rad[0] = - (0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal; - - - for (i=0;i 0.) reaction_normal_force = 0.; - /* to prevent damping component from swamping spring component */ - - - /* Calculate friction coefficients */ - - if(it_rolls[i]) - { - forward_mu = (max_brake_mu[i] - muGear[i])*percent_brake[i] + muGear[i]; - abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward); - sideward_mu = sliding_mu[i]; - if (abs_v_wheel_sideward < skid_v) - sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu; - if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.; - } - else - { - forward_mu=sliding_mu[i]; - sideward_mu=sliding_mu[i]; - } - - /* Calculate foreward and sideward reaction forces */ - - forward_wheel_force = forward_mu*reaction_normal_force; - sideward_wheel_force = sideward_mu*reaction_normal_force; - if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force; - if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force; -/* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force); - */ - /* Rotate into local (N-E-D) axes */ - - f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle - - sideward_wheel_force*sin_wheel_hdg_angle; - f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle - + sideward_wheel_force*cos_wheel_hdg_angle; - f_wheel_local_v[2] = reaction_normal_force; - - /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */ - mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF ); - - /* Calculate moments from force and offsets in body axes */ - - cross3( d_wheel_cg_body_v, tempF, tempM ); - - /* Sum forces and moments across all wheels */ - - add3( tempF, F_gear_v, F_gear_v ); - add3( tempM, M_gear_v, M_gear_v ); - - - } - - - - /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */ - - /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear); - printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */ - - + + /* Calculate sideward and forward velocities of the wheel + in the runway plane */ + + cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi); + sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi); + + v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle + + v_wheel_local_v[1]*sin_wheel_hdg_angle; + v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle + - v_wheel_local_v[0]*sin_wheel_hdg_angle; + + + /* Calculate normal load force (simple spring constant) */ + + reaction_normal_force = 0.; + + reaction_normal_force = kgear[i]*d_wheel_rwy_local_v[2] + - v_wheel_local_v[2]*cgear[i]; + /* printf("\treaction_normal_force: %g\n",reaction_normal_force); */ + + if (reaction_normal_force > 0.) reaction_normal_force = 0.; + /* to prevent damping component from swamping spring component */ + + + /* Calculate friction coefficients */ + + if(it_rolls[i]) + { + forward_mu = (max_brake_mu[i] - muGear[i])*percent_brake[i] + muGear[i]; + abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward); + sideward_mu = sliding_mu[i]; + if (abs_v_wheel_sideward < skid_v) + sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu; + if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.; + } + else + { + forward_mu=sliding_mu[i]; + sideward_mu=sliding_mu[i]; + } + + /* Calculate foreward and sideward reaction forces */ + + forward_wheel_force = forward_mu*reaction_normal_force; + sideward_wheel_force = sideward_mu*reaction_normal_force; + if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force; + if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force; + /* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force); + */ + /* Rotate into local (N-E-D) axes */ + + f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle + - sideward_wheel_force*sin_wheel_hdg_angle; + f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle + + sideward_wheel_force*cos_wheel_hdg_angle; + f_wheel_local_v[2] = reaction_normal_force; + + /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */ + mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF ); + + /* Calculate moments from force and offsets in body axes */ + + cross3( d_wheel_cg_body_v, tempF, tempM ); + + /* Sum forces and moments across all wheels */ + if (tempF[0] != 0.0 || tempF[1] != 0.0 || tempF[2] != 0.0) { + fgSetBool("/gear/gear[1]/wow", true); + } + + add3( tempF, F_gear_v, F_gear_v ); + add3( tempM, M_gear_v, M_gear_v ); + + } + } + + + + /* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */ + + /*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear); + printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */ + + } }