diff --git a/src/FDM/LaRCsim/c172_engine.c b/src/FDM/LaRCsim/c172_engine.c index 2b738dae1..e74983ea0 100644 --- a/src/FDM/LaRCsim/c172_engine.c +++ b/src/FDM/LaRCsim/c172_engine.c @@ -75,8 +75,8 @@ void engine( SCALAR dt, int init ) { /* 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]*268.42/0.83; - F_Z_engine = Throttle[3]*3.75/0.83; + F_X_engine = Throttle[3]*350/0.83; + F_Z_engine = Throttle[3]*4.9/0.83; Throttle_pct = Throttle[3]; } diff --git a/src/FDM/LaRCsim/c172_gear.c b/src/FDM/LaRCsim/c172_gear.c index 8f1031c8c..1f47bcfb0 100644 --- a/src/FDM/LaRCsim/c172_gear.c +++ b/src/FDM/LaRCsim/c172_gear.c @@ -37,8 +37,8 @@ $Header$ $Log$ -Revision 1.5 1999/08/07 17:18:45 curt -Updates to Tony's c172 model. +Revision 1.6 1999/08/08 15:12:33 curt +Updates from Tony. Revision 1.1.1.1 1999/04/05 21:32:45 curt Start of 0.6.x branch. diff --git a/src/FDM/LaRCsim/c172_main.c b/src/FDM/LaRCsim/c172_main.c index d52865847..3bf5ad971 100644 --- a/src/FDM/LaRCsim/c172_main.c +++ b/src/FDM/LaRCsim/c172_main.c @@ -28,7 +28,315 @@ #include #include #include +#include +/* #include */ +#include +#include +#include +#include + + +//simple "one-at-a-time" longitudinal trimming routine +typedef struct +{ + double latitude,longitude,altitude; + double vc,alpha,beta,gamma; + double theta,phi,psi; + int use_gamma_tmg; +}InitialConditions; + +// Units for setIC +// vc knots (calibrated airspeed, close to indicated) +// altitude ft +// all angles in degrees +// if use_gamma_tmg =1 then theta will be computed +// from theta=alpha+gamma and the value given will +// be ignored. Otherwise gamma is computed from +// gamma=theta-alpha +void setIC(InitialConditions IC) +{ + SCALAR vtfps,u,v,w,vt_east; + SCALAR vnu,vnv,vnw,vteu,vtev,vtew,vdu,vdv,vdw; + SCALAR alphar,betar,thetar,phir,psir,gammar; + SCALAR sigma,ps,Ts,a; + + Latitude=IC.latitude*DEG_TO_RAD; + Longitude=IC.longitude*DEG_TO_RAD; + Altitude=IC.altitude; + ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric); + + ls_atmos(IC.altitude,&sigma,&a,&Ts,&ps); + vtfps=sqrt(1/sigma*IC.vc*IC.vc)*1.68781; + alphar=IC.alpha*DEG_TO_RAD; + betar=IC.beta*DEG_TO_RAD; + gammar=IC.gamma*DEG_TO_RAD; + + + phir=IC.phi*DEG_TO_RAD; + psir=IC.psi*DEG_TO_RAD; + + if(IC.use_gamma_tmg == 1) + { + thetar=alphar+gammar; + } + else + { + thetar=IC.theta*DEG_TO_RAD; + gammar=thetar-alphar; + } + + u=vtfps*cos(alphar)*cos(betar); + v=vtfps*sin(betar); + w=vtfps*sin(alphar)*cos(betar); + + vnu=u*cos(thetar)*cos(psir); + vnv=v*(-sin(psir)*cos(phir)+sin(phir)*sin(thetar)*cos(psir)); + vnw=w*(sin(phir)*sin(psir)+cos(phir)*sin(thetar)*cos(psir)); + + V_north=vnu+vnv+vnw; + + vteu=u*cos(thetar)*sin(psir); + vtev=v*(cos(phir)*cos(psir)+sin(phir)*sin(thetar)*sin(psir)); + vtew=w*(-sin(phir)*cos(psir)+cos(phir)*sin(thetar)*sin(psir)); + + vt_east=vteu+vtev+vtew; + V_east=vt_east+ OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric); + + vdu=u*-sin(thetar); + vdv=v*cos(thetar)*sin(phir); + vdw=w*cos(thetar)*cos(phir); + + V_down=vdu+vdv+vdw; + + Theta=thetar; + Phi=phir; + Psi=psir; + +} + + +int trim_long(int kmax, InitialConditions IC) +{ + double elevator,alpha; + double tol=1E-3; + double a_tol=tol/10; + double alpha_step=0.001; + int k=0,i,j=0,jmax=75,sum=0; + ls_loop(0.0,-1); + do{ + //printf("k: %d\n",k); + while((fabs(W_dot_body) > tol) && (j < jmax)) + { + + IC.alpha+=W_dot_body*0.05; + if((IC.alpha < -5) || (IC.alpha > 21)) + j=jmax; + setIC(IC); + ls_loop(0.0,-1); +/* printf("IC.alpha: %g, Alpha: %g, wdot: %g\n",IC.alpha,Alpha*RAD_TO_DEG,W_dot_body); + */ j++; + } + sum+=j; +/* printf("\tTheta: %7.4f, Alpha: %7.4f, wdot: %10.6f, j: %d\n",Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,W_dot_body,j); + */ j=0; + while((fabs(U_dot_body) > tol) && (j < jmax)) + { + + Throttle_pct-=U_dot_body*0.005; + if((Throttle_pct < 0) || (Throttle_pct > 1)) + Throttle_pct=0.2; + setIC(IC); + ls_loop(0.0,-1); + j++; + } + sum+=j; +/* printf("\tThrottle_pct: %7.4f, udot: %10.6f, j: %d\n",Throttle_pct,U_dot_body,j); + */ j=0; + while((fabs(Q_dot_body) > a_tol) && (j < jmax)) + { + + Long_control+=Q_dot_body*0.001; + if((Long_control < -1) || (Long_control > 1)) + j=jmax; + setIC(IC); + ls_loop(0.0,-1); + j++; + } + sum+=j; + if(Long_control >= 0) + elevator=Long_control*23; + else + elevator=Long_control*28; +/* printf("\televator: %7.4f, qdot: %10.6f, j: %d\n",elevator,Q_dot_body,j); + */ k++;j=0; + }while(((fabs(W_dot_body) > tol) || (fabs(U_dot_body) > tol) || (fabs(Q_dot_body) > tol)) && (k < kmax)); + /* printf("Total Iterations: %d\n",sum); */ + return k; +} + +void do_trims(int kmax,FILE *out,InitialConditions IC) +{ + int k=0; + double speed,elevator; + out=fopen("trims.out","w"); + speed=55; + while(speed < 150) + { + IC.vc=speed; + Long_control=0;Theta=0;Throttle_pct=0.2; + + k=trim_long(kmax,IC); + if(Long_control <= 0) + elevator=Long_control*28; + else + elevator=Long_control*23; + + fprintf(out,"%g,%g,%g,%g,%g,%d\n",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Gamma_vert_rad,k); + printf("%g,%g,%g,%g,%d,%g,%g,%g\n",V_calibrated_kts,Alpha*RAD_TO_DEG,elevator,Throttle_pct,k,W_body,U_body,Q_body); + + if(k >= kmax) + { + printf("kmax exceeded at: %g knots\n",V_calibrated_kts); + printf("wdot: %g, udot: %g, qdot: %g\n\n",W_dot_body,U_dot_body,Q_dot_body); + } + speed+=10; + } + fclose(out); +} + +void do_stick_pull(int kmax, SCALAR tmax,FILE *out,InitialConditions IC) +{ + + SCALAR htarget,hgain,hdiffgain,herr,herr_diff,herrprev; + SCALAR theta_trim,elev_trim,time; + int k; + k=trim_long(kmax,IC); + printf("Trim:\n\tAlpha: %10.6f, elev: %10.6f, Throttle: %10.6f\n\twdot: %10.6f, qdot: %10.6f, udot: %10.6f\n",Alpha*RAD_TO_DEG,Long_control,Throttle_pct,W_dot_body,U_dot_body,Q_dot_body); + + + htarget=0; + + hgain=1; + hdiffgain=1; + elev_trim=Long_control; + out=fopen("stick_pull.out","w"); + herr=Q_body-htarget; + + //fly steady-level for 2 seconds, well, zero pitch rate anyway + while(time < 2.0) + { + herrprev=herr; + ls_update(1); + herr=Q_body-htarget; + herr_diff=herr-herrprev; + Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff); + time+=0.01; +/* printf("Time: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, Phi: %7.4f, Psi: %7.4f\n",time,Altitude,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,Phi,Psi); + printf("Mcg: %7.4f, Mrp: %7.4f, Maero: %7.4f, Meng: %7.4f, Mgear: %7.4f, Dx_cg: %7.4f, Dz_cg: %7.4f\n\n",M_m_cg,M_m_rp,M_m_aero,M_m_engine,M_m_gear,Dx_cg,Dz_cg); + */ fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_true_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG); + fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude); + } + + //begin untrimmed climb at theta_trim + 2 degrees + hgain=4; + hdiffgain=2; + theta_trim=Theta; + htarget=theta_trim; + herr=Theta-htarget; + while(time < tmax) + { + //ramp in the target theta + if(htarget < (theta_trim + 2*DEG_TO_RAD)) + { + htarget+= 0.01*DEG_TO_RAD; + } + herrprev=herr; + ls_update(1); + herr=Theta-htarget; + herr_diff=herr-herrprev; + Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff); + time+=0.01; +/* printf("Time: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, Phi: %7.4f, Psi: %7.4f\n",time,Altitude,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,Phi,Psi); + printf("Mcg: %7.4f, Mrp: %7.4f, Maero: %7.4f, Meng: %7.4f, Mgear: %7.4f, Dx_cg: %7.4f, Dz_cg: %7.4f\n\n",M_m_cg,M_m_rp,M_m_aero,M_m_engine,M_m_gear,Dx_cg,Dz_cg); + */ fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_true_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG); + fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude); + } + printf("%g,%g\n",theta_trim*RAD_TO_DEG,htarget*RAD_TO_DEG); + fclose(out); +} + +void do_takeoff(FILE *out) +{ + SCALAR htarget,hgain,hdiffgain,elev_trim,elev_trim_save,herr; + SCALAR time,herrprev,herr_diff; + + htarget=0; + + hgain=1; + hdiffgain=1; + elev_trim=Long_control; + elev_trim_save=elev_trim; + + + out=fopen("takeoff.out","w"); + herr=Q_body-htarget; + + //attempt to maintain zero pitch rate during the roll + while((V_calibrated_kts < 61) && (time < 30.0)) + { + /* herrprev=herr;*/ + ls_update(1); + /*herr=Q_body-htarget; + herr_diff=herr-herrprev; + Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff); */ + time+=0.01; + printf("Time: %7.4f, Vc: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, U: %7.4f, W: %7.4f\n",time,V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,U_body,W_body); +// printf("Mcg: %7.4f, Mrp: %7.4f, Maero: %7.4f, Meng: %7.4f, Mgear: %7.4f, Dx_cg: %7.4f, Dz_cg: %7.4f\n\n",M_m_cg,M_m_rp,M_m_aero,M_m_engine,M_m_gear,Dx_cg,Dz_cg); +// fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG); + fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude); + + } + //At Vr, ramp in 10% nose up elevator in 0.5 seconds + elev_trim_save=0; + printf("At Vr, rotate...\n"); + while((Q_body < 3.0*RAD_TO_DEG) && (time < 30.0)) + { + Long_control-=0.01; + ls_update(1); + printf("Time: %7.4f, Vc: %7.4f, Alpha: %7.4f, pelev: %7.4f, q: %7.4f, cm: %7.4f, U: %7.4f, W: %7.4f\n",time,V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,cm,U_body,W_body); + + fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG); + fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude); + time +=0.01; + + } + //Maintain 15 degrees theta for the climbout + htarget=15*DEG_TO_RAD; + herr=Theta-htarget; + hgain=10; + hdiffgain=1; + elev_trim=Long_control; + while(time < 30.0) + { + herrprev=herr; + ls_update(1); + herr=Theta-htarget; + herr_diff=herr-herrprev; + Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff); + time+=0.01; + printf("Time: %7.4f, Alt: %7.4f, Speed: %7.4f, Theta: %7.4f\n",time,Altitude,V_calibrated_kts,Theta*RAD_TO_DEG); + fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG); + fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude); + } + fclose(out); + printf("Speed: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, q: %7.4f, udot: %7.4f\n",V_true_kts,Altitude,Alpha*RAD_TO_DEG,Long_control,Q_body*RAD_TO_DEG,U_dot_body); + printf("F_down_total: %7.4f, F_Z_aero: %7.4f, F_X: %7.4f, M_m_cg: %7.4f\n\n",F_down+Mass*Gravity,F_Z_aero,F_X,M_m_cg); + + + + +} // Initialize the LaRCsim flight model, dt is the time increment for // each subsequent iteration through the EOM @@ -39,60 +347,61 @@ int fgLaRCsimInit(double dt) { } + // Run an iteration of the EOM (equations of motion) -int main() { +int main(int argc, char *argv[]) { double save_alt = 0.0; - int multiloop=1; - double time=0; + int multiloop=1,k=0,i; + double time=0,elev_trim,elev_trim_save,elevator,speed; + FILE *out; + double hgain,hdiffgain,herr,herrprev,herr_diff,htarget; + InitialConditions IC; - Altitude=1000; /*BFI as given by airnav*/ - Latitude=47.5299892; - Longitude=122.3019561; - Lat_geocentric=Latitude; - Lon_geocentric=Longitude; - Radius_to_vehicle=Altitude+EQUATORIAL_RADIUS; - Lat_control = 0; - Long_control = 0; - Long_trim = 0; - Rudder_pedal = 0; - Throttle_pct = 0.0; - Brake_pct = 1.0; - V_north=200; - V_east=0; - V_down=0; - - printf("Calling init...\n"); - fgLaRCsimInit(0.05); - - /* copy control positions into the LaRCsim structure */ - - - /* Inform LaRCsim of the local terrain altitude */ - Runway_altitude = 18.0; - printf("Entering Loop\n"); - printf("Speed: %7.4f, Lat: %7.4f, Long: %7.4f, Alt: %7.4f\n\n",V_true_kts,Latitude,Longitude,Altitude); - - while (time < 0.2) + if(argc < 6) { - time=time+0.05; - ls_update(multiloop); - printf("Speed: %7.4f, Fxeng: %7.4f, Fxaero: %7.4f, Fxgear: %7.4f Alt: %7.4f\n\n",V_true_kts,F_X_engine,F_X_aero,F_X_gear,Altitude); - - - - } - /* // printf("%d FG_Altitude = %.2f\n", i, FG_Altitude * 0.3048); - // printf("%d Altitude = %.2f\n", i, Altitude * 0.3048); + printf("Need args: $c172 speed alt alpha elev throttle\n"); + exit(1); + } + + IC.latitude=47.5299892; //BFI + IC.longitude=122.3019561; + Runway_altitude = 18.0; + IC.altitude=strtod(argv[2],NULL); + IC.vc=strtod(argv[1],NULL); + IC.alpha=strtod(argv[3],NULL); + IC.beta=0; + IC.gamma=strtod(argv[3],NULL); + IC.use_gamma_tmg=1; + IC.phi=0; + IC.psi=0; + Long_control=strtod(argv[4],NULL); + setIC(IC); + printf("Out setIC\n"); + ls_ForceAltitude(IC.altitude); + fgLaRCsimInit(0.01); + + /* printf("trim_long():\n"); + k=trim_long(200,IC); */ + + + printf("V_rel_wind: %8.2f, Alpha: %8.2f, Beta: %8.2f\n",V_rel_wind,Alpha*RAD_TO_DEG,Beta*RAD_TO_DEG); + printf("Theta: %8.2f, Gamma: %8.2f, Alpha_tmg: %8.2f\n",Theta*RAD_TO_DEG,Gamma_vert_rad*RAD_TO_DEG,Theta*RAD_TO_DEG-Gamma_vert_rad*RAD_TO_DEG); + printf("V_north: %8.2f, V_east_rel_ground: %8.2f, V_east: %8.2f, V_down: %8.2f\n",V_north,V_east_rel_ground,V_east,V_down); + printf("Long_control: %8.2f, Throttle_pct: %8.2f\n",Long_control,Throttle_pct); + printf("k: %d, udot: %8.4f, wdot: %8.4f, qdot: %8.5f\n",k,U_dot_body,W_dot_body,Q_dot_body); - // translate LaRCsim back to FG structure so that the - // autopilot (and the rest of the sim can use the updated - // values - //fgLaRCsim_2_FGInterface(f); */ - - + printf("\nls_update():\n"); + ls_update(1); + printf("V_rel_wind: %8.2f, Alpha: %8.2f, Beta: %8.2f\n",V_rel_wind,Alpha*RAD_TO_DEG,Beta*RAD_TO_DEG); + printf("Theta: %8.2f, Gamma: %8.2f, Alpha_tmg: %8.2f\n",Theta*RAD_TO_DEG,Gamma_vert_rad*RAD_TO_DEG,Theta*RAD_TO_DEG-Gamma_vert_rad*RAD_TO_DEG); + + /* Inform LaRCsim of the local terrain altitude */ + + + do_trims(200,out,IC); return 1; } @@ -411,3 +720,4 @@ int fgLaRCsim_2_FGInterface (FGInterface& f) { return 0; } */ +