#include <FDM/LaRCsim/ls_generic.h>
#include <FDM/LaRCsim/ls_interface.h>
#include <FDM/LaRCsim/ls_constants.h>
+#include <FDM/LaRCsim/atmos_62.h>
+/* #include <FDM/LaRCsim/ls_trim_fs.h> */
+#include <FDM/LaRCsim/c172_aero.h>
+#include <FDM/LaRCsim/ic.h>
+
+#include <math.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+void do_trims(int kmax,FILE *out,InitialConditions IC)
+{
+ int bad_trim=0,i,j;
+ double speed,elevator,cmcl,maxspeed;
+ out=fopen("trims.out","w");
+ speed=55;
+
+ for(j=0;j<=0;j+=10)
+ {
+ IC.flap_handle=j;
+ 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=2400;IC.cg=0.257;break;
+ case 5: IC.weight=2550;IC.cg=0.364;break;
+ }
+
+ speed=40;
+ if(j > 0) { maxspeed = 90; }
+ else { maxspeed = 170; }
+ while(speed <= maxspeed)
+ {
+ IC.vc=speed;
+ Long_control=0;Theta=0;Throttle_pct=0.0;
+
+ bad_trim=trim_long(kmax,IC);
+ if(Long_control <= 0)
+ elevator=Long_control*28;
+ else
+ elevator=Long_control*23;
+ if(fabs(CL) > 1E-3)
+ {
+ cmcl=cm / CL;
+ }
+ if(!bad_trim)
+ {
+ 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_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);
+ }
+ speed+=10;
+ }
+ }
+ }
+ 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)
+{
+ int k=0,i,j;
+ int failf;
+ char axis[10];
+ double speed,elevator,cmcl,speed_inc,lastgood;
+ out=fopen("trim_stall.summary","w");
+ speed=90;
+ speed_inc=10;
+ //failf=malloc(sizeof(int));
+
+ for(j=0;j<=30;j+=10)
+ {
+ IC.flap_handle=j;
+ for(i=1;i<=6;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=2400;IC.cg=0.155;break;
+ case 4: IC.weight=2400;IC.cg=0.364;break;
+ case 5: IC.weight=2550;IC.cg=0.257;break;
+ case 6: IC.weight=2550;IC.cg=0.364;break;
+ }
+
+ speed=90;
+ speed_inc=10;
+ while(speed_inc >= 0.5)
+ {
+ IC.vc=speed;
+ Long_control=0;Theta=0;Throttle_pct=0.0;
+ failf=trim_longfr(kmax,IC);
+ if(Long_control <= 0)
+ elevator=Long_control*28;
+ else
+ elevator=Long_control*23;
+ if(fabs(CL) > 1E-3)
+ {
+ cmcl=cm / CL;
+ }
+ if(failf == 0)
+ {
+ lastgood=speed;
+ axis[0]='\0';
+ //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\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("trim failed at: %g knots, %g lbs, %g %%MAC, Flaps: %g\n",V_calibrated_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);
+ printf("Speed increment: %g\n",speed_inc);
+ speed+=speed_inc;
+ speed_inc/=2;
+ }
+ speed-=speed_inc;
+
+
+ }
+ printf("failf %d\n",failf);
+ if(failf == 1)
+ strcpy(axis,"lift");
+ else if(failf == 2)
+ strcpy(axis,"thrust");
+ else if(failf == 3)
+ strcpy(axis,"pitch");
+ fprintf(out,"Last good speed: %g, Flaps: %g, Weight: %g, CG: %g, failed axis: %s\n",lastgood,Flap_handle,Weight,Cg,axis);
+
+
+ }
+ }
+ fclose(out);
+ //free(failf);
+}
// Initialize the LaRCsim flight model, dt is the time increment for
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*LS_PI*LS_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 main(int argc, char *argv[]) {
double save_alt = 0.0;
- int multiloop=1;
- double time=0;
-
- 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;
+ 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;
+ double lastVt,vtdots,vtdott;
+ InitialConditions IC;
+ SCALAR *control[7];
+ SCALAR *state[7];
+ float old_state,effectiveness,tol,delta_state,lctrim;
+ float newcm,lastcm,cmalpha,td_vspeed,td_time,stop_time;
+ float h[801],hdot[801],altmin,lastAlt,theta[800],theta_dot[800];
- printf("Calling init...\n");
- fgLaRCsimInit(0.05);
+ if(argc < 6)
+ {
+ printf("Need args: $c172 speed alt alpha elev throttle\n");
+ exit(1);
+ }
+ initIC(&IC);
+
+ IC.latitude=47.5299892; //BFI
+ IC.longitude=122.3019561;
+ Runway_altitude = 18.0;
- /* copy control positions into the LaRCsim structure */
+ 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.theta=strtod(argv[3],NULL);
+ IC.use_gamma_tmg=0;
+ IC.phi=0;
+ IC.psi=0;
+ IC.weight=2400;
+ IC.cg=0.25;
+ IC.flap_handle=10;
+ IC.long_control=0;
+ IC.rudder_pedal=0;
-
- /* 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);
+
+ 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);
+ 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);
- while (time < 0.2)
+ while(time < 5.0)
{
- 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("Time: %g, Flap_handle: %g, Flap_position: %g, Transit: %d\n",time,Flap_handle,Flap_Position,Flaps_In_Transit);
+ if(time > 2.5)
+ Flap_handle=20;
+ else if (time > 0.5)
+ Flap_handle=20;
+ ls_update(1);
+ time+=0.01;
}
- /* // printf("%d FG_Altitude = %.2f\n", i, FG_Altitude * 0.3048);
- // printf("%d Altitude = %.2f\n", i, Altitude * 0.3048);
-
- // 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); */
+
+
+
+ /*out=fopen("drop.out","w");
+ N=800;touchdown=0;
+
+ while(i <= N)
+ {
+ 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++;
+ }
+ 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);*/
-
- return 1;
-}
+ /*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);
+ */
-/*// Convert from the FGInterface struct to the LaRCsim generic_ struct
-int FGInterface_2_LaRCsim (FGInterface& f) {
-
- Mass = f.get_Mass();
- I_xx = f.get_I_xx();
- I_yy = f.get_I_yy();
- I_zz = f.get_I_zz();
- I_xz = f.get_I_xz();
- // Dx_pilot = f.get_Dx_pilot();
- // Dy_pilot = f.get_Dy_pilot();
- // Dz_pilot = f.get_Dz_pilot();
- Dx_cg = f.get_Dx_cg();
- Dy_cg = f.get_Dy_cg();
- Dz_cg = f.get_Dz_cg();
- // F_X = f.get_F_X();
- // F_Y = f.get_F_Y();
- // F_Z = f.get_F_Z();
- // F_north = f.get_F_north();
- // F_east = f.get_F_east();
- // F_down = f.get_F_down();
- // F_X_aero = f.get_F_X_aero();
- // F_Y_aero = f.get_F_Y_aero();
- // F_Z_aero = f.get_F_Z_aero();
- // F_X_engine = f.get_F_X_engine();
- // F_Y_engine = f.get_F_Y_engine();
- // F_Z_engine = f.get_F_Z_engine();
- // F_X_gear = f.get_F_X_gear();
- // F_Y_gear = f.get_F_Y_gear();
- // F_Z_gear = f.get_F_Z_gear();
- // M_l_rp = f.get_M_l_rp();
- // M_m_rp = f.get_M_m_rp();
- // M_n_rp = f.get_M_n_rp();
- // M_l_cg = f.get_M_l_cg();
- // M_m_cg = f.get_M_m_cg();
- // M_n_cg = f.get_M_n_cg();
- // M_l_aero = f.get_M_l_aero();
- // M_m_aero = f.get_M_m_aero();
- // M_n_aero = f.get_M_n_aero();
- // M_l_engine = f.get_M_l_engine();
- // M_m_engine = f.get_M_m_engine();
- // M_n_engine = f.get_M_n_engine();
- // M_l_gear = f.get_M_l_gear();
- // M_m_gear = f.get_M_m_gear();
- // M_n_gear = f.get_M_n_gear();
- // V_dot_north = f.get_V_dot_north();
- // V_dot_east = f.get_V_dot_east();
- // V_dot_down = f.get_V_dot_down();
- // U_dot_body = f.get_U_dot_body();
- // V_dot_body = f.get_V_dot_body();
- // W_dot_body = f.get_W_dot_body();
- // A_X_cg = f.get_A_X_cg();
- // A_Y_cg = f.get_A_Y_cg();
- // A_Z_cg = f.get_A_Z_cg();
- // A_X_pilot = f.get_A_X_pilot();
- // A_Y_pilot = f.get_A_Y_pilot();
- // A_Z_pilot = f.get_A_Z_pilot();
- // N_X_cg = f.get_N_X_cg();
- // N_Y_cg = f.get_N_Y_cg();
- // N_Z_cg = f.get_N_Z_cg();
- // N_X_pilot = f.get_N_X_pilot();
- // N_Y_pilot = f.get_N_Y_pilot();
- // N_Z_pilot = f.get_N_Z_pilot();
- // P_dot_body = f.get_P_dot_body();
- // Q_dot_body = f.get_Q_dot_body();
- // R_dot_body = f.get_R_dot_body();
- V_north = f.get_V_north();
- V_east = f.get_V_east();
- V_down = f.get_V_down();
- // V_north_rel_ground = f.get_V_north_rel_ground();
- // V_east_rel_ground = f.get_V_east_rel_ground();
- // V_down_rel_ground = f.get_V_down_rel_ground();
- // V_north_airmass = f.get_V_north_airmass();
- // V_east_airmass = f.get_V_east_airmass();
- // V_down_airmass = f.get_V_down_airmass();
- // V_north_rel_airmass = f.get_V_north_rel_airmass();
- // V_east_rel_airmass = f.get_V_east_rel_airmass();
- // V_down_rel_airmass = f.get_V_down_rel_airmass();
- // U_gust = f.get_U_gust();
- // V_gust = f.get_V_gust();
- // W_gust = f.get_W_gust();
- // U_body = f.get_U_body();
- // V_body = f.get_V_body();
- // W_body = f.get_W_body();
- // V_rel_wind = f.get_V_rel_wind();
- // V_true_kts = f.get_V_true_kts();
- // V_rel_ground = f.get_V_rel_ground();
- // V_inertial = f.get_V_inertial();
- // V_ground_speed = f.get_V_ground_speed();
- // V_equiv = f.get_V_equiv();
- // V_equiv_kts = f.get_V_equiv_kts();
- // V_calibrated = f.get_V_calibrated();
- // V_calibrated_kts = f.get_V_calibrated_kts();
- P_body = f.get_P_body();
- Q_body = f.get_Q_body();
- R_body = f.get_R_body();
- // P_local = f.get_P_local();
- // Q_local = f.get_Q_local();
- // R_local = f.get_R_local();
- // P_total = f.get_P_total();
- // Q_total = f.get_Q_total();
- // R_total = f.get_R_total();
- // Phi_dot = f.get_Phi_dot();
- // Theta_dot = f.get_Theta_dot();
- // Psi_dot = f.get_Psi_dot();
- // Latitude_dot = f.get_Latitude_dot();
- // Longitude_dot = f.get_Longitude_dot();
- // Radius_dot = f.get_Radius_dot();
- Lat_geocentric = f.get_Lat_geocentric();
- Lon_geocentric = f.get_Lon_geocentric();
- Radius_to_vehicle = f.get_Radius_to_vehicle();
- Latitude = f.get_Latitude();
- Longitude = f.get_Longitude();
- Altitude = f.get_Altitude();
- Phi = f.get_Phi();
- Theta = f.get_Theta();
- Psi = f.get_Psi();
- // T_local_to_body_11 = f.get_T_local_to_body_11();
- // T_local_to_body_12 = f.get_T_local_to_body_12();
- // T_local_to_body_13 = f.get_T_local_to_body_13();
- // T_local_to_body_21 = f.get_T_local_to_body_21();
- // T_local_to_body_22 = f.get_T_local_to_body_22();
- // T_local_to_body_23 = f.get_T_local_to_body_23();
- // T_local_to_body_31 = f.get_T_local_to_body_31();
- // T_local_to_body_32 = f.get_T_local_to_body_32();
- // T_local_to_body_33 = f.get_T_local_to_body_33();
- // Gravity = f.get_Gravity();
- // Centrifugal_relief = f.get_Centrifugal_relief();
- // Alpha = f.get_Alpha();
- // Beta = f.get_Beta();
- // Alpha_dot = f.get_Alpha_dot();
- // Beta_dot = f.get_Beta_dot();
- // Cos_alpha = f.get_Cos_alpha();
- // Sin_alpha = f.get_Sin_alpha();
- // Cos_beta = f.get_Cos_beta();
- // Sin_beta = f.get_Sin_beta();
- // Cos_phi = f.get_Cos_phi();
- // Sin_phi = f.get_Sin_phi();
- // Cos_theta = f.get_Cos_theta();
- // Sin_theta = f.get_Sin_theta();
- // Cos_psi = f.get_Cos_psi();
- // Sin_psi = f.get_Sin_psi();
- // Gamma_vert_rad = f.get_Gamma_vert_rad();
- // Gamma_horiz_rad = f.get_Gamma_horiz_rad();
- // Sigma = f.get_Sigma();
- // Density = f.get_Density();
- // V_sound = f.get_V_sound();
- // Mach_number = f.get_Mach_number();
- // Static_pressure = f.get_Static_pressure();
- // Total_pressure = f.get_Total_pressure();
- // Impact_pressure = f.get_Impact_pressure();
- // Dynamic_pressure = f.get_Dynamic_pressure();
- // Static_temperature = f.get_Static_temperature();
- // Total_temperature = f.get_Total_temperature();
- Sea_level_radius = f.get_Sea_level_radius();
- Earth_position_angle = f.get_Earth_position_angle();
- Runway_altitude = f.get_Runway_altitude();
- // Runway_latitude = f.get_Runway_latitude();
- // Runway_longitude = f.get_Runway_longitude();
- // Runway_heading = f.get_Runway_heading();
- // Radius_to_rwy = f.get_Radius_to_rwy();
- // D_cg_north_of_rwy = f.get_D_cg_north_of_rwy();
- // D_cg_east_of_rwy = f.get_D_cg_east_of_rwy();
- // D_cg_above_rwy = f.get_D_cg_above_rwy();
- // X_cg_rwy = f.get_X_cg_rwy();
- // Y_cg_rwy = f.get_Y_cg_rwy();
- // H_cg_rwy = f.get_H_cg_rwy();
- // D_pilot_north_of_rwy = f.get_D_pilot_north_of_rwy();
- // D_pilot_east_of_rwy = f.get_D_pilot_east_of_rwy();
- // D_pilot_above_rwy = f.get_D_pilot_above_rwy();
- // X_pilot_rwy = f.get_X_pilot_rwy();
- // Y_pilot_rwy = f.get_Y_pilot_rwy();
- // H_pilot_rwy = f.get_H_pilot_rwy();
-
- return( 0 );
+
+
+
+
+
+
+ /* ls_loop(0.0,-1);
+
+ control[1]=&IC.long_control;
+ control[2]=&IC.throttle;
+ control[3]=&IC.alpha;
+ control[4]=&IC.beta;
+ control[5]=&IC.phi;
+ control[6]=&IC.lat_control;
+
+ state[1]=&Q_dot_body;state[2]=&U_dot_body;state[3]=&W_dot_body;
+ state[4]=&R_dot_body;state[5]=&V_dot_body;state[6]=&P_dot_body;
+
+
+ for(i=1;i<=6;i++)
+ {
+ old_state=*state[i];
+ tol=1E-4;
+ for(j=1;j<=6;j++)
+ {
+ *control[j]+=0.1;
+ setIC(IC);
+ ls_loop(0.0,-1);
+ delta_state=*state[i]-old_state;
+ effectiveness=(delta_state)/ 0.1;
+ if(delta_state < tol)
+ effectiveness = 0;
+ printf("%8.4f,",delta_state);
+ *control[j]-=0.1;
+
+ }
+ printf("\n");
+ setIC(IC);
+ ls_loop(0.0,-1);
+ } */
+
+ return 1;
}
+/*
+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);
-// Convert from the LaRCsim generic_ struct to the FGInterface struct
-int fgLaRCsim_2_FGInterface (FGInterface& f) {
-
- // Mass properties and geometry values
- f.set_Inertias( Mass, I_xx, I_yy, I_zz, I_xz );
- // f.set_Pilot_Location( Dx_pilot, Dy_pilot, Dz_pilot );
- f.set_CG_Position( Dx_cg, Dy_cg, Dz_cg );
-
- // Forces
- // f.set_Forces_Body_Total( F_X, F_Y, F_Z );
- // f.set_Forces_Local_Total( F_north, F_east, F_down );
- // f.set_Forces_Aero( F_X_aero, F_Y_aero, F_Z_aero );
- // f.set_Forces_Engine( F_X_engine, F_Y_engine, F_Z_engine );
- // f.set_Forces_Gear( F_X_gear, F_Y_gear, F_Z_gear );
-
- // Moments
- // f.set_Moments_Total_RP( M_l_rp, M_m_rp, M_n_rp );
- // f.set_Moments_Total_CG( M_l_cg, M_m_cg, M_n_cg );
- // f.set_Moments_Aero( M_l_aero, M_m_aero, M_n_aero );
- // f.set_Moments_Engine( M_l_engine, M_m_engine, M_n_engine );
- // f.set_Moments_Gear( M_l_gear, M_m_gear, M_n_gear );
-
- // Accelerations
- // f.set_Accels_Local( V_dot_north, V_dot_east, V_dot_down );
- // f.set_Accels_Body( U_dot_body, V_dot_body, W_dot_body );
- // f.set_Accels_CG_Body( A_X_cg, A_Y_cg, A_Z_cg );
- // f.set_Accels_Pilot_Body( A_X_pilot, A_Y_pilot, A_Z_pilot );
- // f.set_Accels_CG_Body_N( N_X_cg, N_Y_cg, N_Z_cg );
- // f.set_Accels_Pilot_Body_N( N_X_pilot, N_Y_pilot, N_Z_pilot );
- // f.set_Accels_Omega( P_dot_body, Q_dot_body, R_dot_body );
-
- // Velocities
- f.set_Velocities_Local( V_north, V_east, V_down );
- // f.set_Velocities_Ground( V_north_rel_ground, V_east_rel_ground,
- // V_down_rel_ground );
- // f.set_Velocities_Local_Airmass( V_north_airmass, V_east_airmass,
- // V_down_airmass );
- // f.set_Velocities_Local_Rel_Airmass( V_north_rel_airmass,
- // V_east_rel_airmass, V_down_rel_airmass );
- // f.set_Velocities_Gust( U_gust, V_gust, W_gust );
- // f.set_Velocities_Wind_Body( U_body, V_body, W_body );
-
- // f.set_V_rel_wind( V_rel_wind );
- // f.set_V_true_kts( V_true_kts );
- // f.set_V_rel_ground( V_rel_ground );
- // f.set_V_inertial( V_inertial );
- // f.set_V_ground_speed( V_ground_speed );
- // f.set_V_equiv( V_equiv );
- f.set_V_equiv_kts( V_equiv_kts );
- // f.set_V_calibrated( V_calibrated );
- // f.set_V_calibrated_kts( V_calibrated_kts );
-
- f.set_Omega_Body( P_body, Q_body, R_body );
- // f.set_Omega_Local( P_local, Q_local, R_local );
- // f.set_Omega_Total( P_total, Q_total, R_total );
+
+ 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);
+
+
+
- // f.set_Euler_Rates( Phi_dot, Theta_dot, Psi_dot );
- f.set_Geocentric_Rates( Latitude_dot, Longitude_dot, Radius_dot );
+}
+*/
+
- FG_LOG( FG_FLIGHT, FG_DEBUG, "lon = " << Longitude
- << " lat_geoc = " << Lat_geocentric << " lat_geod = " << Latitude
- << " alt = " << Altitude << " sl_radius = " << Sea_level_radius
- << " radius_to_vehicle = " << Radius_to_vehicle );
-
- // Positions
- f.set_Geocentric_Position( Lat_geocentric, Lon_geocentric,
- Radius_to_vehicle );
- f.set_Geodetic_Position( Latitude, Longitude, Altitude );
- f.set_Euler_Angles( Phi, Theta, Psi );
-
- // Miscellaneous quantities
- f.set_T_Local_to_Body(T_local_to_body_m);
- // f.set_Gravity( Gravity );
- // f.set_Centrifugal_relief( Centrifugal_relief );
-
- f.set_Alpha( Alpha );
- f.set_Beta( Beta );
- // f.set_Alpha_dot( Alpha_dot );
- // f.set_Beta_dot( Beta_dot );
-
- // f.set_Cos_alpha( Cos_alpha );
- // f.set_Sin_alpha( Sin_alpha );
- // f.set_Cos_beta( Cos_beta );
- // f.set_Sin_beta( Sin_beta );
-
- // f.set_Cos_phi( Cos_phi );
- // f.set_Sin_phi( Sin_phi );
- // f.set_Cos_theta( Cos_theta );
- // f.set_Sin_theta( Sin_theta );
- // f.set_Cos_psi( Cos_psi );
- // f.set_Sin_psi( Sin_psi );
-
- f.set_Gamma_vert_rad( Gamma_vert_rad );
- // f.set_Gamma_horiz_rad( Gamma_horiz_rad );
-
- // f.set_Sigma( Sigma );
- // f.set_Density( Density );
- // f.set_V_sound( V_sound );
- // f.set_Mach_number( Mach_number );
-
- // f.set_Static_pressure( Static_pressure );
- // f.set_Total_pressure( Total_pressure );
- // f.set_Impact_pressure( Impact_pressure );
- // f.set_Dynamic_pressure( Dynamic_pressure );
-
- // f.set_Static_temperature( Static_temperature );
- // f.set_Total_temperature( Total_temperature );
-
- f.set_Sea_level_radius( Sea_level_radius );
- f.set_Earth_position_angle( Earth_position_angle );
-
- f.set_Runway_altitude( Runway_altitude );
- // f.set_Runway_latitude( Runway_latitude );
- // f.set_Runway_longitude( Runway_longitude );
- // f.set_Runway_heading( Runway_heading );
- // f.set_Radius_to_rwy( Radius_to_rwy );
-
- // f.set_CG_Rwy_Local( D_cg_north_of_rwy, D_cg_east_of_rwy, D_cg_above_rwy);
- // f.set_CG_Rwy_Rwy( X_cg_rwy, Y_cg_rwy, H_cg_rwy );
- // f.set_Pilot_Rwy_Local( D_pilot_north_of_rwy, D_pilot_east_of_rwy,
- // D_pilot_above_rwy );
- // f.set_Pilot_Rwy_Rwy( X_pilot_rwy, Y_pilot_rwy, H_pilot_rwy );
-
- f.set_sin_lat_geocentric(Lat_geocentric);
- f.set_cos_lat_geocentric(Lat_geocentric);
- f.set_sin_cos_longitude(Longitude);
- f.set_sin_cos_latitude(Latitude);
-
- // printf("sin_lat_geo %f cos_lat_geo %f\n", sin_Lat_geoc, cos_Lat_geoc);
- // printf("sin_lat %f cos_lat %f\n",
- // f.get_sin_latitude(), f.get_cos_latitude());
- // printf("sin_lon %f cos_lon %f\n",
- // f.get_sin_longitude(), f.get_cos_longitude());
-
- return 0;
-} */
projects / flightgear.git / blobdiff