#define NCL 11
+#define Ndf 4
#define DYN_ON_SPEED 33 /*20 knots*/
}
-
void aero( SCALAR dt, int Initialize ) {
static int init = 0;
-
+ static int flap_dir=0;
+ static SCALAR lastFlapHandle=0;
static SCALAR trim_inc = 0.0002;
static SCALAR alpha_ind[NCL]={-0.087,0,0.175,0.209,0.24,0.262,0.278,0.303,0.314,0.332,0.367};
static SCALAR CLtable[NCL]={-0.14,0.31,1.21,1.376,1.51249,1.591,1.63,1.60878,1.53712,1.376,1.142};
+
+ static SCALAR flap_ind[Ndf]={0,10,20,30};
+ static SCALAR dCLf[Ndf]={0,0.20,0.30,0.35};
+ static SCALAR dCdf[Ndf]={0,0.0021,0.0085,0.0191};
+ static SCALAR dCmf[Ndf]={0,-0.186,-0.28,-0.325};
+
+ static SCALAR flap_transit_rate=2.5;
+
+
+
/* printf("Initialize= %d\n",Initialize); */
/* printf("Initializing aero model...Initialize= %d\n", Initialize);
- */ CLadot=1.7;
+ */
+ /*nondimensionalization quantities*/
+ /*units here are ft and lbs */
+ cbar=4.9; /*mean aero chord ft*/
+ b=35.8; /*wing span ft */
+ Sw=174; /*wing planform surface area ft^2*/
+ rPiARe=0.054; /*reciprocal of Pi*AR*e*/
+ lbare=3.682; /*elevator moment arm / MAC*/
+
+ CLadot=1.7;
CLq=3.9;
- CLde=0.43;
- CLo=0;
+
+ CLob=0;
- Cdo=0.031;
+ Cdob=0.031;
Cda=0.13; /*Not used*/
Cdde=0.06;
- Cma=-0.89;
+ Cma=-1.8;
Cmadot=-5.2;
Cmq=-12.4;
- Cmo=-0.015;
- Cmde=-1.28;
+ Cmob=-0.00;
+ Cmde=-1.00;
+
+ CLde=-Cmde / lbare; /* kinda backwards, huh? */
Clbeta=-0.089;
Clp=-0.47;
Clr=0.096;
- Clda=-0.178;
+ Clda=-0.09;
Cldr=0.0147;
Cnbeta=0.065;
Cyda=0.0;
Cydr=0.187;
- /*nondimensionalization quantities*/
- /*units here are ft and lbs */
- cbar=4.9; /*mean aero chord ft*/
- b=35.8; /*wing span ft */
- Sw=174; /*wing planform surface area ft^2*/
- rPiARe=0.054; /*reciprocal of Pi*AR*e*/
+
MaxTakeoffWeight=2550;
EmptyWeight=1500;
Dz_cg=Zcg*cbar;
-
-
-
+ if(Flap_handle < flap_ind[0])
+ {
+ Flap_handle=flap_ind[0];
+ Flap_Position=flap_ind[0];
+ }
+ else if(Flap_handle > flap_ind[3])
+ {
+ Flap_handle=flap_ind[3];
+ Flap_Position=flap_ind[3];
+ }
+ else
+ {
+
+
+ if((Flap_handle != lastFlapHandle) && (dt > 0))
+ Flaps_In_Transit=1;
+ else if(dt <= 0)
+ Flap_Position=Flap_handle;
+
+ lastFlapHandle=Flap_handle;
+ if((Flaps_In_Transit) && (dt > 0))
+ {
+ if(Flap_Position < 10)
+ flap_transit_rate = 2.5;
+ else
+ flap_transit_rate=5;
+
+ if(Flaps_In_Transit)
+ {
+ if(Flap_Position < Flap_handle)
+ flap_dir=1;
+ else
+ flap_dir=-1;
+
+ if(fabs(Flap_Position - Flap_handle) > dt*flap_transit_rate)
+ Flap_Position+=flap_dir*flap_transit_rate*dt;
+
+ if(fabs(Flap_Position - Flap_handle) < dt*flap_transit_rate)
+ {
+ Flaps_In_Transit=0;
+ Flap_Position=Flap_handle;
+ }
+ }
+ }
+ }
long_trim=0;
if(Aft_trim) long_trim = long_trim - trim_inc;
/*hack to avoid divide by zero*/
/*the dynamic terms might be negligible at low ground speeds anyway*/
+/* printf("Vinf: %g, Span: %g\n",V_rel_wind,b);
+ */
if(V_rel_wind > DYN_ON_SPEED)
{
cbar_2V=cbar/(2*V_rel_wind);
/* printf("aero: Wb: %7.4f, Ub: %7.4f, Alpha: %7.4f, elev: %7.4f, ail: %7.4f, rud: %7.4f, long_trim: %7.4f\n",W_body,U_body,Alpha*RAD_TO_DEG,elevator*RAD_TO_DEG,aileron*RAD_TO_DEG,rudder*RAD_TO_DEG,long_trim*RAD_TO_DEG);
- */ //printf("Theta: %7.4f, Gamma: %7.4f, Beta: %7.4f, Phi: %7.4f, Psi: %7.4f\n",Theta*RAD_TO_DEG,Gamma_vert_rad*RAD_TO_DEG,Beta*RAD_TO_DEG,Phi*RAD_TO_DEG,Psi*RAD_TO_DEG);
+ printf("aero: Theta: %7.4f, Gamma: %7.4f, Beta: %7.4f, Phi: %7.4f, Psi: %7.4f\n",Theta*RAD_TO_DEG,Gamma_vert_rad*RAD_TO_DEG,Beta*RAD_TO_DEG,Phi*RAD_TO_DEG,Psi*RAD_TO_DEG);
+ */
/* sum coefficients */
CLwbh = interp(CLtable,alpha_ind,NCL,Alpha);
+ CLo = CLob + interp(dCLf,flap_ind,Ndf,Flap_Position);
+ Cdo = Cdob + interp(dCdf,flap_ind,Ndf,Flap_Position);
+ Cmo = Cmob + interp(dCmf,flap_ind,Ndf,Flap_Position);
+
+ /* printf("FP: %g\n",Flap_Position);
+ printf("CLo: %g\n",CLo);
+ printf("Cdo: %g\n",Cdo);
+ 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;
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+long_trim);
+ cm = Cmo + Cma*Alpha + (Cmq*Q_body + Cmadot*Alpha_dot)*cbar_2V + Cmde*(elevator);
cn = Cnbeta*Beta + (Cnp*P_body + Cnr*R_body)*b_2V + Cnda*aileron + Cndr*rudder;
croll=Clbeta*Beta + (Clp*P_body + Clr*R_body)*b_2V + Clda*aileron + Cldr*rudder;
/* printf("aero: CL: %7.4f, Cd: %7.4f, Cm: %7.4f, Cy: %7.4f, Cn: %7.4f, Cl: %7.4f\n",CL,cd,cm,cy,cn,croll);
- */ /*calculate wind axes forces*/
+ */
+ /*calculate wind axes forces*/
F_X_wind=-1*cd*qS;
F_Y_wind=cy*qS;
F_Z_wind=-1*CL*qS;
/* printf("I_yy: %7.4f, qScbar: %7.4f, qbar: %7.4f, Sw: %7.4f, cbar: %7.4f, 0.5*rho*V^2: %7.4f\n",I_yy,qScbar,Dynamic_pressure,Sw,cbar,0.5*0.0023081*V_rel_wind*V_rel_wind);
*/
-/* printf("Fxaero: %7.4f Fyaero: %7.4f Fzaero: %7.4f Weight: %7.4f\n",F_X_aero,F_Y_aero,F_Z_aero,W);
- *//* printf("Maero: %7.4f Naero: %7.4f Raero: %7.4f\n",M_m_aero,M_n_aero,M_l_aero);
+/* printf("Fxaero: %7.4f Fyaero: %7.4f Fzaero: %7.4f Weight: %7.4f\n",F_X_aero,F_Y_aero,F_Z_aero,Weight);
+ */
+/* printf("Maero: %7.4f Naero: %7.4f Raero: %7.4f\n",M_m_aero,M_n_aero,M_l_aero);
*/
}
//simple "one-at-a-time" longitudinal trimming routine
typedef struct
{
- double latitude,longitude,altitude;
- double vc,alpha,beta,gamma;
- double theta,phi,psi;
- double weight,cg;
+ SCALAR latitude,longitude,altitude;
+ SCALAR vc,alpha,beta,gamma;
+ SCALAR theta,phi,psi;
+ SCALAR weight,cg;
+ SCALAR throttle,long_control,lat_control,rudder_pedal,flap_handle;
int use_gamma_tmg;
}InitialConditions;
+void initIC(InitialConditions *IC)
+{
+ IC->latitude=IC->longitude=IC->altitude=0;
+ IC->vc=IC->alpha=IC->beta=IC->gamma=0;
+ IC->theta=IC->phi=IC->psi=0;
+ IC->weight=IC->cg=0;
+ IC->throttle=IC->long_control=IC->lat_control=IC->rudder_pedal=IC->flap_handle=0;
+}
+
+void checkLimits(float *control, SCALAR min, SCALAR max)
+{
+ if(*control < min)
+ *control=min;
+ else if(*control > max)
+ *control=max;
+}
+
// Units for setIC
// vc knots (calibrated airspeed, close to indicated)
// altitude ft
Theta=thetar;
Phi=phir;
Psi=psir;
+
+ Throttle_pct=IC.throttle;
+ checkLimits(&Throttle_pct,0,1);
+
+ Long_control=IC.long_control;
+ checkLimits(&Long_control,-1,1);
+
+ Lat_control=IC.lat_control;
+ checkLimits(&Lat_control,-1,1);
+
+ Rudder_pedal=IC.rudder_pedal;
+ checkLimits(&Rudder_pedal,-1,1);
+
+ Flap_Handle=IC.flap_handle;
+ checkLimits(&Flap_Handle,0,30);
+
}
double tol=1E-3;
double a_tol=tol/10;
double alpha_step=0.001;
- int k=0,i,j=0,jmax=10,sum=0;
+ int k=0,i,j=0,jmax=40,sum=0,trim_failed=0;
ls_loop(0.0,-1);
do{
//printf("k: %d\n",k);
*/ 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;
+ if(trim_failed)
+ printf("\tAlpha: %7.4f, wdot: %10.6f, j: %d\n",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;
+ IC.throttle-=U_dot_body*0.01;
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;
+ if(trim_failed)
+ 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);
+ IC.long_control+=Q_dot_body*0.01;
+ setIC(IC);
ls_loop(0.0,-1);
j++;
}
+ if(trim_failed)
+ printf("\tLong_control: %7.4f, qdot: %10.6f, j: %d\n",Long_control,Q_dot_body,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));
+ if(k == kmax-2)
+ {
+ if((fabs(W_dot_body) > tol) || (fabs(U_dot_body) > tol) || (fabs(Q_dot_body) > a_tol));
+ {
+ trim_failed=1;
+ jmax=kmax;
+ printf("\nTrim failed at: %6.1f knots, %g lbs, %5.3f %MAC\n",V_calibrated_kts,Weight,Cg);
+ IC.alpha=0;IC.throttle=0;IC.long_control=0;
+ setIC(IC);
+ ls_loop(0.0,-1);
+ }
+ }
+ k++;j=0;
+ }while(((fabs(W_dot_body) > tol) || (fabs(U_dot_body) > tol) || (fabs(Q_dot_body) > a_tol)) && (k < kmax));
/* printf("Total Iterations: %d\n",sum); */
return k;
}
{
int k=0,i;
double speed,elevator,cmcl;
- out=fopen("trims.out","w");
+ out=fopen("trims.oldCmde.out","w");
speed=55;
for(i=1;i<=5;i++)
*/ }
else
{
- printf("kmax exceeded at: %g knots, %g lbs, %g %%MAC\n",V_calibrated_kts,Weight,Cg);
- printf("wdot: %g, udot: %g, qdot: %g\n\n",W_dot_body,U_dot_body,Q_dot_body);
-
- }
+/* printf("kmax exceeded at: %g knots, %g lbs, %g %%MAC\n",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*RAD_TO_DEG,Throttle_pct,Long_control);
+ */ }
speed+=10;
}
}
double save_alt = 0.0;
- int multiloop=1,k=0,i;
+ int multiloop=1,k=0,i,j;
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;
+
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;
IC.altitude=strtod(argv[2],NULL);
IC.vc=strtod(argv[1],NULL);
- IC.alpha=10;
+ IC.alpha=0;
IC.beta=0;
- IC.theta=strtod(argv[3],NULL);
- IC.use_gamma_tmg=0;
+ IC.gamma=strtod(argv[3],NULL);
+ IC.use_gamma_tmg=1;
IC.phi=0;
- IC.psi=0;
- IC.weight=1500;
- IC.cg=0.155;
- Long_control=strtod(argv[4],NULL);
- setIC(IC);
- printf("Out setIC\n");
+ IC.psi=10;
+ IC.weight=2300;
+ IC.cg=0.25;
+ IC.flap_handle=0;
+ IC.long_control=strtod(argv[4],NULL);
+ 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);
+
+ setIC(IC);
+ printf("V_down: %g, V_north: %g V_east: %g\n",V_down,V_north,V_east);
+
+ ls_loop(0.0,-1);
+ printf("V_down: %g, V_north: %g V_east: %g\n",V_down,V_north,V_east);
+ 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);
- while(IC.alpha < 30.0)
+ 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);
+
+
+ i=0;
+ while(i < 20)
{
- setIC(IC);
- ls_loop(0.0,-1);
- printf("CL: %g ,Alpha: %g\n",CL,IC.alpha);
- IC.alpha+=1.0;
+ ls_update(1);
+ i++;
}
+ Flap_Handle=10;
+ while((Flap_Position < 5) && (i < 500))
+ {
+ printf("Flap_Handle: %2.0f, Flap_Position: %5.2f",Flap_Handle,Flap_Position);
+ printf(" Flaps_In_Transit: %d\n", Flaps_In_Transit);
+/* printf(" CLo: %7.4f, Cdo: %7.4f, Cmo: %7.4f\n",CLo,Cdo,Cmo);
+ */
+ ls_update(1);
+ i++;
+ }
+ Flap_Handle=0;
+ while((Flap_Position > 0) || (i < 500))
+ {
+ printf("Flap_Handle: %2.0f, Flap_Position: %5.2f",Flap_Handle,Flap_Position);
+ printf(" Flaps_In_Transit: %d\n", Flaps_In_Transit);
+/* printf(" CLo: %7.4f, Cdo: %7.4f, Cmo: %7.4f\n",CLo,Cdo,Cmo);
+ */
+ ls_update(1);
+ i++;
+ }
+
+
+ /* do_trims(400,out,IC); */
+
+ /* 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;
+
- /*trim_ground(10,IC);*/
- /* printf("%g,%g\n",Theta,Gamma_vert_rad);
- printf("trim_long():\n");
- k=trim_long(200,IC);
- Throttle_pct=Throttle_pct-0.2;
- printf("%g,%g\n",Theta,Gamma_vert_rad);
- out=fopen("dive.out","w");
- time=0;
- while(time < 30.0)
+ for(i=1;i<=6;i++)
{
- ls_update(1);
+ 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;
- cmcl=cm/CL;
- fprintf(out,"%g,%g,%g,%g,%g,%d",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Gamma_vert_rad,k);
- fprintf(out,",%g,%g,%g\n",CL,cm,cmcl);
- time+=0.01;
- }
- fclose(out);
- 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);
-
- 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);
- */
+ }
+ printf("\n");
+ setIC(IC);
+ ls_loop(0.0,-1);
+ } */
- /* Inform LaRCsim of the local terrain altitude */
-
-
-
- return 1;
+ return 1;
}
projects / flightgear.git / commitdiff