From e340190916ee02f3eace5aded0ac76d1e14ce43d Mon Sep 17 00:00:00 2001 From: curt Date: Wed, 8 Dec 1999 19:48:54 +0000 Subject: [PATCH] Updates from Tony. --- src/FDM/LaRCsim/c172_aero.c | 11 +-- src/FDM/LaRCsim/c172_aero.h | 5 +- src/FDM/LaRCsim/c172_gear.c | 84 ++++++++++++------- src/FDM/LaRCsim/c172_init.c | 4 + src/FDM/LaRCsim/c172_main.c | 158 ++++++++++++++++++++++++++++++++---- 5 files changed, 209 insertions(+), 53 deletions(-) diff --git a/src/FDM/LaRCsim/c172_aero.c b/src/FDM/LaRCsim/c172_aero.c index c6ffe32a1..0e0fae67c 100644 --- a/src/FDM/LaRCsim/c172_aero.c +++ b/src/FDM/LaRCsim/c172_aero.c @@ -239,14 +239,15 @@ void aero( SCALAR dt, int Initialize ) { { Weight=1500; } - if(Dx_cg > 0.5586) - { Dx_cg = 0.5586; } - else if(Dx_cg < -0.4655) - { Dx_cg = -0.4655; } + if(Dx_cg > 0.43) + { Dx_cg = 0.43; } + else if(Dx_cg < -0.6) + { Dx_cg = -0.6; } - Cg=Dx_cg/cbar +0.25; + Cg=0.25 - Dx_cg/cbar; Dz_cg=Zcg*cbar; + Dy_cg=0; if(Flap_handle < flap_ind[0]) diff --git a/src/FDM/LaRCsim/c172_aero.h b/src/FDM/LaRCsim/c172_aero.h index bba9e1c50..30543d5ae 100644 --- a/src/FDM/LaRCsim/c172_aero.h +++ b/src/FDM/LaRCsim/c172_aero.h @@ -66,10 +66,13 @@ SCALAR elevator, aileron, rudder; + SCALAR Flap_Position; - /* float Flap_Handle; */ + int Flaps_In_Transit; + + #endif diff --git a/src/FDM/LaRCsim/c172_gear.c b/src/FDM/LaRCsim/c172_gear.c index f7dd0045d..df3a44855 100644 --- a/src/FDM/LaRCsim/c172_gear.c +++ b/src/FDM/LaRCsim/c172_gear.c @@ -36,8 +36,8 @@ $Header$ $Log$ -Revision 1.11 1999/11/15 22:54:07 curt -Updates from Tony, mostly to landing gear. +Revision 1.12 1999/12/08 18:48:55 curt +Updates from Tony. ---------------------------------------------------------------------------- @@ -113,28 +113,31 @@ clear3( DATA v[] ) gear() { char rcsid[] = "$Id$"; -char gear_strings[3][12]={"nose","right main", "left main"}; +#define NUM_WHEELS 4 +char gear_strings[NUM_WHEELS][12]={"nose","right main", "left main", "tail skid"}; /* * Aircraft specific initializations and data goes here */ -#define NUM_WHEELS 3 static int num_wheels = NUM_WHEELS; /* number of wheels */ - static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations */ + static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations,full extension */ { - { 5, 0., 7.0 }, /*nose*/ /* in feet */ - { -2.0, 3.6, 6.5 }, /*right main*/ - { -2.0, -3.6, 6.5 } /*left main*/ + { 3.91, 0., 6.67 }, /*nose*/ /* in feet */ + { -1.47, 3.58, 6.71 }, /*right main*/ + { -1.47, -3.58, 6.71 }, /*left main*/ + { -15.67, 0, 2.42 } /*tail skid */ }; + static DATA gear_travel[NUM_WHEELS] = /*in Z-axis*/ + { -0.5, 2.5, 2.5, 0}; static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */ - { 1500., 5000., 5000. }; + { 1200., 900., 900., 10000. }; static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */ - { 1000., 1500., 1500. }; + { 200., 300., 300., 400. }; static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */ - { 0., 0., 0. }; /* 0 = none, 1 = full */ + { 0., 0., 0., 0. }; /* 0 = none, 1 = full */ static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */ - { 0., 0., 0.}; /* radians, +CW */ + { 0., 0., 0., 0}; /* radians, +CW */ /* * End of aircraft specific code */ @@ -159,9 +162,10 @@ char gear_strings[3][12]={"nose","right main", "left main"}; */ - static DATA sliding_mu = 0.5; - static DATA rolling_mu = 0.01; - static DATA max_brake_mu = 0.6; + static int it_rolls[NUM_WHEELS] = { 1,1,1,0}; + static DATA sliding_mu[NUM_WHEELS] = { 0.5, 0.5, 0.5, 0.3}; + static DATA rolling_mu[NUM_WHEELS] = { 0.01, 0.01, 0.01, 0.0}; + static DATA max_brake_mu[NUM_WHEELS] ={ 0.0, 0.6, 0.6, 0.0}; static DATA max_mu = 0.8; static DATA bkout_v = 0.1; static DATA skid_v = 1.0; @@ -172,9 +176,12 @@ char gear_strings[3][12]={"nose","right main", "left main"}; 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], temp3b[3], tempF[3], tempM[3]; DATA reaction_normal_force; /* wheel normal (to rwy) force */ DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */ @@ -207,17 +214,24 @@ char gear_strings[3][12]={"nose","right main", "left main"}; { /* printf("%s:\n",gear_strings[i]); */ + + /*========================================*/ /* Calculate wheel position w.r.t. runway */ /*========================================*/ - /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */ + + /* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */ + + + /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */ sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v ); /* then converting to local (North-East-Down) axes... */ multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v ); + /* Runway axes correction - third element is Altitude, not (-)Z... */ @@ -241,11 +255,11 @@ char gear_strings[3][12]={"nose","right main", "left main"}; /* transform into local axes */ - multtrans3x3by3( T_local_to_body_m, temp3a, temp3b ); + multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v ); /* plus contribution due to cg velocities */ - add3( temp3b, V_local_rel_ground_v, v_wheel_local_v ); + add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v ); clear3(f_wheel_local_v); reaction_normal_force=0; @@ -271,25 +285,36 @@ char gear_strings[3][12]={"nose","right main", "left main"}; + 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 = spring_constant[i]*HEIGHT_AGL_WHEEL + + reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2] - v_wheel_local_v[2]*spring_damping[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 */ - forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu; - abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward); - sideward_mu = sliding_mu; - 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.; + if(it_rolls[i]) + { + forward_mu = (max_brake_mu[i] - rolling_mu[i])*percent_brake[i] + rolling_mu[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 */ @@ -324,8 +349,9 @@ char gear_strings[3][12]={"nose","right main", "left main"}; - /* printf("\tN: %g,dZrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL); - printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear); + /* 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); */ diff --git a/src/FDM/LaRCsim/c172_init.c b/src/FDM/LaRCsim/c172_init.c index ab935ee87..63cce2308 100644 --- a/src/FDM/LaRCsim/c172_init.c +++ b/src/FDM/LaRCsim/c172_init.c @@ -73,7 +73,11 @@ void model_init( void ) { I_zz=1967; I_xz=0; + Flap_Position=Flap_handle; Flaps_In_Transit=0; + + + } diff --git a/src/FDM/LaRCsim/c172_main.c b/src/FDM/LaRCsim/c172_main.c index 9bf0c0d22..919dc793f 100644 --- a/src/FDM/LaRCsim/c172_main.c +++ b/src/FDM/LaRCsim/c172_main.c @@ -38,9 +38,6 @@ #include #include - - - void do_trims(int kmax,FILE *out,InitialConditions IC) { int bad_trim=0,i,j; @@ -216,14 +213,77 @@ int fgLaRCsimInit(double dt) { return(1); } - +int wave_stats(float *var,float *var_rate,int N,FILE *out) +{ + int Nc,i,Nmaxima; + float varmax,slope,intercept,time,ld,zeta,omegad,omegan; + float varmaxima[100],vm_times[100]; + /*adjust N so that any constant slope region at the end is cut off */ + i=N; + while((fabs(var_rate[N]-var_rate[i]) < 0.1) && (i >= 0)) + { + i--; + } + Nc=N-i; + slope=(var[N]-var[Nc])/(N*0.01 - Nc*0.01); + intercept=var[N]-slope*N*0.01; + printf("\tRotating constant decay out of data using:\n"); + printf("\tslope: %g, intercept: %g\n",slope,intercept); + printf("\tUsing first %d points for dynamic response analysis\n",Nc); + varmax=0; + Nmaxima=0;i=0; + while((i <= Nc) && (i <= 801)) + { + + fprintf(out,"%g\t%g",i*0.01,var[i]); + var[i]-=slope*i*0.01+intercept; + /* printf("%g\n",var[i]); */ + fprintf(out,"\t%g\n",var[i]); + if(var[i] > varmax) + { + varmax=var[i]; + time=i*0.01; + + } + if((var[i-1]*var[i] < 0) && (var[i] > 0)) + { + varmaxima[Nmaxima]=varmax; + vm_times[Nmaxima]=time; + printf("\t%6.2f: %8.4f\n",vm_times[Nmaxima],varmaxima[Nmaxima]); + varmax=0;Nmaxima++; + + } + + i++; + } + varmaxima[Nmaxima]=varmax; + vm_times[Nmaxima]=time; + Nmaxima++; + if(Nmaxima > 2) + { + ld=log(varmaxima[1]/varmaxima[2]); //logarithmic decrement + zeta=ld/sqrt(4*PI*PI +ld*ld); //damping ratio + omegad=1/(vm_times[2]-vm_times[1]); //damped natural frequency Hz + if(zeta < 1) + { + omegan=omegad/sqrt(1-zeta*zeta); //natural frequency Hz + } + printf("\tDamping Ratio: %g\n",zeta); + printf("\tDamped Freqency: %g Hz\n\tNatural Freqency: %g Hz\n",omegad,omegan); + } + else + printf("\tNot enough points to take log decrement\n"); +/* printf("w: %g, u: %g, q: %g\n",W_body,U_body,Q_body); + */ + return 1; +} // Run an iteration of the EOM (equations of motion) int main(int argc, char *argv[]) { double save_alt = 0.0; - int multiloop=1,k=0,i,j; + int multiloop=1,k=0,i,j,touchdown,N; double time=0,elev_trim,elev_trim_save,elevator,speed,cmcl; FILE *out; double hgain,hdiffgain,herr,herrprev,herr_diff,htarget; @@ -232,7 +292,8 @@ int main(int argc, char *argv[]) { SCALAR *control[7]; SCALAR *state[7]; float old_state,effectiveness,tol,delta_state,lctrim; - float newcm,lastcm,cmalpha; + float newcm,lastcm,cmalpha,td_vspeed,td_time,stop_time; + float h[801],hdot[801],altmin,lastAlt,theta[800],theta_dot[800]; if(argc < 6) { @@ -252,31 +313,92 @@ int main(int argc, char *argv[]) { IC.beta=0; IC.theta=strtod(argv[3],NULL); IC.use_gamma_tmg=0; - IC.phi=strtod(argv[4],NULL); + IC.phi=0; IC.psi=0; IC.weight=2400; IC.cg=0.25; - IC.flap_handle=0; + IC.flap_handle=30; IC.long_control=0; IC.rudder_pedal=0; - printf("IC.vc: %g\n",IC.vc); + ls_ForceAltitude(IC.altitude); fgLaRCsimInit(0.01); setIC(IC); + printf("Dx_cg: %g\n",Dx_cg); + V_down=strtod(argv[4],NULL);; ls_loop(0,-1); - printf("\nAltitude: %g\n\n",Altitude); - i=0; - while(i <= 1) + i=0;time=0; + IC.long_control=0; + altmin=Altitude; + printf("\tAltitude: %g, Theta: %g, V_down: %g\n\n",Altitude,Theta*RAD_TO_DEG,V_down); + + printf("%12s %10s %10s\n","Alpha (deg)","Alpha","Drag"); + for(i=-5;i<=22;i++) + { + IC.alpha=i; + setIC(IC); + ls_loop(0,-1); + printf("%12f %10f %10f\n",Alpha*RAD_TO_DEG,Alpha,cd); + } + + + + + /*out=fopen("drop.out","w"); + N=800;touchdown=0; + + while(i <= N) { - if(i > 0) - Brake_pct=1; - ls_update(1); - printf("\tAltitude: %g, Theta: %g, Phi: %g\n\n",Altitude,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); + ls_update(1); + printf("\tAltitude: %g, Theta: %g, V_down: %g\n\n",D_cg_above_rwy,Theta*RAD_TO_DEG,V_down); + fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground); + h[i]=D_cg_above_rwy;hdot[i]=V_down; + theta[i]=Theta; theta_dot[i]=Theta_dot; + if(D_cg_above_rwy < altmin) + altmin=D_cg_above_rwy; + if((F_Z_gear < -10) && (! touchdown)) + { + touchdown=1; + td_vspeed=V_down; + td_time=time; + } + time+=0.01; i++; } - printf("w: %g, u: %g, q: %g\n",W_body,U_body,Q_body); - + while(V_rel_ground > 1) + { + if(Brake_pct < 1) + { + Brake_pct+=0.02; + } + ls_update(1); + time=i*0.01; + fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground); + i++; + } + stop_time=time; + while((time-stop_time) < 5.0) + { + ls_update(1); + time=i*0.01; + fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground); + i++; + } + fclose(out); + + printf("Min Altitude: %g, Final Alitutde: %g, Delta: %g\n",altmin, h[N], D_cg_above_rwy-altmin); + printf("Vertical Speed at touchdown: %g, Time at touchdown: %g\n",td_vspeed,td_time); + printf("\nAltitude response:\n"); + out=fopen("alt.out","w"); + wave_stats(h,hdot,N,out); + fclose(out); + out=fopen("theta.out","w"); + printf("\nPitch Attitude response:\n"); + wave_stats(theta,theta_dot,N,out); + fclose(out);*/ + + /*printf("Flap_handle: %g, Flap_Position: %g\n",Flap_handle,Flap_Position); printf("k: %d, %g knots, %g lbs, %g %%MAC\n",k,V_calibrated_kts,Weight,Cg);