diff --git a/src/FDM/LaRCsim/c172_aero.c b/src/FDM/LaRCsim/c172_aero.c index 837cc1ef5..c6ffe32a1 100644 --- a/src/FDM/LaRCsim/c172_aero.c +++ b/src/FDM/LaRCsim/c172_aero.c @@ -147,6 +147,7 @@ void aero( SCALAR dt, int Initialize ) { static int init = 0; static int flap_dir=0; static SCALAR lastFlapHandle=0; + static SCALAR Ai; static SCALAR trim_inc = 0.0002; @@ -180,9 +181,8 @@ void aero( SCALAR dt, int Initialize ) { CLob=0; - - /* original */ /* Cdob=0.031; */ - Cdob=0.046; + Ai=1.24; + Cdob=0.036; Cda=0.13; /*Not used*/ Cdde=0.06; @@ -263,11 +263,13 @@ void aero( SCALAR dt, int Initialize ) { { - if((Flap_handle != lastFlapHandle) && (dt > 0)) { + if((Flap_handle != lastFlapHandle) && (dt > 0)) + { Flaps_In_Transit=1; - } else if(dt <= 0) { + + } + else if(dt <= 0) Flap_Position=Flap_handle; - } lastFlapHandle=Flap_handle; if((Flaps_In_Transit) && (dt > 0)) @@ -317,7 +319,7 @@ void aero( SCALAR dt, int Initialize ) { /*calculate rate derivative nondimensionalization (is that a word?) factors */ /*hack to avoid divide by zero*/ - /*the dynamic terms might be negligible at low ground speeds anyway*/ + /*the dynamic terms are negligible at low ground speeds anyway*/ /* printf("Vinf: %g, Span: %g\n",V_rel_wind,b); */ @@ -357,12 +359,14 @@ void aero( SCALAR dt, int Initialize ) { /* printf("FP: %g\n",Flap_Position); printf("CLo: %g\n",CLo); printf("Cdo: %g\n",Cdo); - printf("Cmo: %g\n",Cmo); */ + printf("Cmo: %g\n",Cmo); */ + + CL = CLo + CLwbh + (CLadot*Alpha_dot + CLq*Theta_dot)*cbar_2V + CLde*elevator; - cd = Cdo + rPiARe*CL*CL + Cdde*elevator; + cd = Cdo + rPiARe*Ai*Ai*CL*CL + Cdde*elevator; cy = Cybeta*Beta + (Cyp*P_body + Cyr*R_body)*b_2V + Cyda*aileron + Cydr*rudder; cm = Cmo + Cma*Alpha + (Cmq*Q_body + Cmadot*Alpha_dot)*cbar_2V + Cmde*(elevator); diff --git a/src/FDM/LaRCsim/c172_engine.c b/src/FDM/LaRCsim/c172_engine.c index a1b1ae83a..261395c75 100644 --- a/src/FDM/LaRCsim/c172_engine.c +++ b/src/FDM/LaRCsim/c172_engine.c @@ -70,18 +70,26 @@ $Header$ extern SIM_CONTROL sim_control_; void engine( SCALAR dt, int init ) { - /* if (init) { */ + + float v,h,pa; + float bhp=160; + Throttle[3] = Throttle_pct; - /* } */ - /* F_X_engine = Throttle[3]*813.4/0.2; */ /* original code */ - /* F_Z_engine = Throttle[3]*11.36/0.2; */ /* original code */ - F_X_engine = Throttle[3]*400/0.83; - F_Z_engine = Throttle[3]*4.9/0.83; + + v=V_rel_wind; + h=Altitude; + if(V_rel_wind < 10) + v=10; + if(Altitude < 0) + h=0; + pa=(0.00144*v + 0.546)*(1 - 1.6E-5*h)*bhp; + if(pa < 0) + pa=0; + F_X_engine= Throttle[3]*(pa*550)/v; M_m_engine = F_X_engine*0.734*cbar; /* 0.734 - estimated (WAGged) location of thrust line in the z-axis*/ - Throttle_pct = Throttle[3]; } diff --git a/src/FDM/LaRCsim/c172_gear.c b/src/FDM/LaRCsim/c172_gear.c index 11a2b3bdd..f7dd0045d 100644 --- a/src/FDM/LaRCsim/c172_gear.c +++ b/src/FDM/LaRCsim/c172_gear.c @@ -36,12 +36,8 @@ $Header$ $Log$ -Revision 1.10 1999/11/03 16:46:24 curt -Patches from Tony to enable brakes. - -Revision 1.9 1999/11/01 18:17:16 curt -c172 updates from Tony. Fix extra yaw when using ailerons. Flaps and elevator -tweaks. +Revision 1.11 1999/11/15 22:54:07 curt +Updates from Tony, mostly to landing gear. ---------------------------------------------------------------------------- @@ -71,6 +67,8 @@ tweaks. #include "ls_generic.h" #include "ls_cockpit.h" +#define HEIGHT_AGL_WHEEL d_wheel_rwy_local_v[2] + sub3( DATA v1[], DATA v2[], DATA result[] ) { @@ -115,7 +113,7 @@ clear3( DATA v[] ) gear() { char rcsid[] = "$Id$"; - +char gear_strings[3][12]={"nose","right main", "left main"}; /* * Aircraft specific initializations and data goes here */ @@ -125,14 +123,14 @@ char rcsid[] = "$Id$"; static int num_wheels = NUM_WHEELS; /* number of wheels */ static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations */ { - { 10., 0., 4. }, /* in feet */ - { -1., 3., 4. }, - { -1., -3., 4. } + { 5, 0., 7.0 }, /*nose*/ /* in feet */ + { -2.0, 3.6, 6.5 }, /*right main*/ + { -2.0, -3.6, 6.5 } /*left main*/ }; static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */ { 1500., 5000., 5000. }; static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */ - { 100., 150., 150. }; + { 1000., 1500., 1500. }; static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */ { 0., 0., 0. }; /* 0 = none, 1 = full */ static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */ @@ -202,117 +200,134 @@ char rcsid[] = "$Id$"; percent_brake[1] = Brake_pct; /* replace with cockpit brake handle connection code */ percent_brake[2] = percent_brake[1]; - caster_angle_rad[0] = 0.03*Rudder_pedal; + caster_angle_rad[0] = 0.52*Rudder_pedal; - for (i=0;i 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.; + 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... */ + + d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */ + + /* Add wheel offset to cg location in local axes */ + + add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v ); + + /* remove Runway axes correction so right hand rule applies */ + + d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */ + + /*============================*/ + /* Calculate wheel velocities */ + /*============================*/ + + /* contribution due to angular rates */ + + cross3( Omega_body_v, d_wheel_cg_body_v, temp3a ); + + /* transform into local axes */ + + multtrans3x3by3( T_local_to_body_m, temp3a, temp3b ); + + /* plus contribution due to cg velocities */ + + add3( temp3b, V_local_rel_ground_v, v_wheel_local_v ); + + clear3(f_wheel_local_v); + reaction_normal_force=0; + if( HEIGHT_AGL_WHEEL < 0. ) + /*the wheel is underground -- which implies ground contact + so calculate reaction forces */ + { + /*===========================================*/ + /* Calculate forces & moments for this wheel */ + /*===========================================*/ + + /* Add any anticipation, or frame lead/prediction, here... */ + + /* no lead used at present */ + + /* 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 = spring_constant[i]*HEIGHT_AGL_WHEEL + - v_wheel_local_v[2]*spring_damping[i]; + 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.; + + /* 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\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("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */ - /* 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; - - /* 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 ); - } } diff --git a/src/FDM/LaRCsim/c172_main.c b/src/FDM/LaRCsim/c172_main.c index 4c874958f..9bf0c0d22 100644 --- a/src/FDM/LaRCsim/c172_main.c +++ b/src/FDM/LaRCsim/c172_main.c @@ -43,22 +43,22 @@ void do_trims(int kmax,FILE *out,InitialConditions IC) { - int k=0,i,j; + int bad_trim=0,i,j; double speed,elevator,cmcl,maxspeed; out=fopen("trims.out","w"); speed=55; - for(j=0;j<=30;j+=10) + for(j=0;j<=0;j+=10) { IC.flap_handle=j; - for(i=1;i<=5;i++) + for(i=4;i<=4;i++) { switch(i) { case 1: IC.weight=1500;IC.cg=0.155;break; case 2: IC.weight=1500;IC.cg=0.364;break; case 3: IC.weight=1950;IC.cg=0.155;break; - case 4: IC.weight=2550;IC.cg=0.257;break; + case 4: IC.weight=2400;IC.cg=0.257;break; case 5: IC.weight=2550;IC.cg=0.364;break; } @@ -70,7 +70,7 @@ void do_trims(int kmax,FILE *out,InitialConditions IC) IC.vc=speed; Long_control=0;Theta=0;Throttle_pct=0.0; - k=trim_long(kmax,IC); + bad_trim=trim_long(kmax,IC); if(Long_control <= 0) elevator=Long_control*28; else @@ -79,15 +79,15 @@ void do_trims(int kmax,FILE *out,InitialConditions IC) { cmcl=cm / CL; } - if(k < kmax) + if(!bad_trim) { - fprintf(out,"%g,%g,%g,%g,%g,%d",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Flap_Position,k); + fprintf(out,"%g,%g,%g,%g,%g",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Flap_Position); fprintf(out,",%g,%g,%g,%g,%g\n",CL,cm,cmcl,Weight,Cg); /* printf("%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n",V_calibrated_kts,Alpha*RAD_TO_DEG,elevator,CL,cm,Cmo,Cma,Cmde,Mass*32.174,Dx_cg); */ } else { - printf("kmax exceeded at: %g knots, %g lbs, %g %%MAC, Flaps: %g\n",V_calibrated_kts,Weight,Cg,Flap_Position); + printf("kmax exceeded at: %g knots, %g lbs, %g %%MAC, Flaps: %g\n",V_true_kts,Weight,Cg,Flap_Position); printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body); printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control); } @@ -96,7 +96,38 @@ void do_trims(int kmax,FILE *out,InitialConditions IC) } } fclose(out); -} +} + +find_max_alt(int kmax,InitialConditions IC) +{ + int bad_trim=0,i=0; + float min=0,max=30000; + IC.use_gamma_tmg=1; + IC.gamma=0; + IC.vc=73; + IC.altitude==1000; + while(!bad_trim) + { + bad_trim=trim_long(200,IC); + IC.altitude+=1000; + } + while((fabs(max-min) > 100) && (i < 50)) + { + + IC.altitude=(max-min)/2 + min; + printf("\nIC.altitude: %g, max: %g, min: %g, bad_trim: %d\n",IC.altitude,max,min,bad_trim); + printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control); + + bad_trim=trim_long(200,IC); + + if(bad_trim == 1 ) + max=IC.altitude; + else + min=IC.altitude; + i++; + } +} + void find_trim_stall(int kmax,FILE *out,InitialConditions IC) { @@ -213,79 +244,41 @@ int main(int argc, char *argv[]) { IC.latitude=47.5299892; //BFI IC.longitude=122.3019561; Runway_altitude = 18.0; + IC.altitude=strtod(argv[2],NULL); + printf("h: %g, argv[2]: %s\n",IC.altitude,argv[2]); IC.vc=strtod(argv[1],NULL); IC.alpha=0; IC.beta=0; - IC.gamma=strtod(argv[3],NULL); - IC.use_gamma_tmg=1; - IC.phi=0; - IC.psi=10; - IC.weight=2300; + IC.theta=strtod(argv[3],NULL); + IC.use_gamma_tmg=0; + IC.phi=strtod(argv[4],NULL); + IC.psi=0; + IC.weight=2400; IC.cg=0.25; IC.flap_handle=0; - IC.long_control=strtod(argv[4],NULL); + IC.long_control=0; IC.rudder_pedal=0; printf("IC.vc: %g\n",IC.vc); ls_ForceAltitude(IC.altitude); fgLaRCsimInit(0.01); - printf("\nLong_control: %g\n\n",Long_control); - - - IC.cg=0.155; - IC.alpha=-5; - setIC(IC);ls_loop(0.0,-1); - newcm=CLwbh*(IC.cg - 0.557); - lastcm=newcm; - out=fopen("cmcl.out","w"); - while(IC.alpha < 22) - { - IC.alpha+=1; - setIC(IC);ls_loop(0.0,-1); - newcm=CLwbh*(IC.cg - 0.557); - cmalpha=newcm-lastcm; - printf("alpha: %4.0f, CL: %5.2f, Cm: %5.2f, Cma: %7.4f\n",Alpha*RAD_TO_DEG,CLwbh,newcm,cmalpha); - fprintf(out,"%g %g\n",newcm,CLwbh); - lastcm=newcm; - } - fclose(out); - /* find_trim_stall(200,out,IC); - - IC.vc=120; - IC.altitude=8000; - IC.weight=2300; - IC.cg=0.25; - IC.flap_handle=0; - - - setIC(IC); - printIC(IC); - k=trim_long(100,IC); - - 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); - printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body); - printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha,Throttle_pct,Long_control); - - printf("Cme: %g, elevator: %g, Cmde: %g\n",elevator*Cmde,elevator,Cmde); - - IC.cg=0.155; - setIC(IC); - k=trim_long(100,IC); - - 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); - printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body); - printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha,Throttle_pct,Long_control); - - printf("Cme: %g, elevator: %g, Cmde: %g\n",elevator*Cmde,elevator,Cmde); - - IC.cg=0.364; setIC(IC); - k=trim_long(100,IC); + ls_loop(0,-1); + printf("\nAltitude: %g\n\n",Altitude); + i=0; + while(i <= 1) + { + 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); + i++; + } + printf("w: %g, u: %g, q: %g\n",W_body,U_body,Q_body); + - printf("Flap_handle: %g, Flap_Position: %g\n",Flap_handle,Flap_Position); + /*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); printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body); printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha,Throttle_pct,Long_control); @@ -297,7 +290,7 @@ int main(int argc, char *argv[]) { - /* do_trims(400,out,IC); */ + /* ls_loop(0.0,-1);