#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 <math.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+
+//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
}
+
// 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;
+ if(argc < 6)
+ {
+ 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("Calling init...\n");
- fgLaRCsimInit(0.05);
- /* copy control positions into the LaRCsim structure */
+ 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);
+
/* 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)
- {
- 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);
- // 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); */
-
-
-
+ do_trims(200,out,IC);
return 1;
}
return 0;
} */
+
projects / flightgear.git / commitdiff