Updates from Tony.
This commit is contained in:
curt
parent
a5f19c7a7c
commit
21f888a43f
3 changed files with 360 additions and 50 deletions
|
|
@ -75,8 +75,8 @@ void engine( SCALAR dt, int init ) {
|
||||||
|
|
||||||
/* F_X_engine = Throttle[3]*813.4/0.2; */ /* original code */
|
/* F_X_engine = Throttle[3]*813.4/0.2; */ /* original code */
|
||||||
/* F_Z_engine = Throttle[3]*11.36/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_X_engine = Throttle[3]*350/0.83;
|
||||||
F_Z_engine = Throttle[3]*3.75/0.83;
|
F_Z_engine = Throttle[3]*4.9/0.83;
|
||||||
|
|
||||||
Throttle_pct = Throttle[3];
|
Throttle_pct = Throttle[3];
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -37,8 +37,8 @@
|
||||||
|
|
||||||
$Header$
|
$Header$
|
||||||
$Log$
|
$Log$
|
||||||
Revision 1.5 1999/08/07 17:18:45 curt
|
Revision 1.6 1999/08/08 15:12:33 curt
|
||||||
Updates to Tony's c172 model.
|
Updates from Tony.
|
||||||
|
|
||||||
Revision 1.1.1.1 1999/04/05 21:32:45 curt
|
Revision 1.1.1.1 1999/04/05 21:32:45 curt
|
||||||
Start of 0.6.x branch.
|
Start of 0.6.x branch.
|
||||||
|
|
|
||||||
|
|
@ -28,7 +28,315 @@
|
||||||
#include <FDM/LaRCsim/ls_generic.h>
|
#include <FDM/LaRCsim/ls_generic.h>
|
||||||
#include <FDM/LaRCsim/ls_interface.h>
|
#include <FDM/LaRCsim/ls_interface.h>
|
||||||
#include <FDM/LaRCsim/ls_constants.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
|
// Initialize the LaRCsim flight model, dt is the time increment for
|
||||||
// each subsequent iteration through the EOM
|
// each subsequent iteration through the EOM
|
||||||
|
|
@ -39,60 +347,61 @@ int fgLaRCsimInit(double dt) {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// Run an iteration of the EOM (equations of motion)
|
// Run an iteration of the EOM (equations of motion)
|
||||||
int main() {
|
int main(int argc, char *argv[]) {
|
||||||
|
|
||||||
|
|
||||||
double save_alt = 0.0;
|
double save_alt = 0.0;
|
||||||
int multiloop=1;
|
int multiloop=1,k=0,i;
|
||||||
double time=0;
|
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*/
|
if(argc < 6)
|
||||||
Latitude=47.5299892;
|
{
|
||||||
Longitude=122.3019561;
|
printf("Need args: $c172 speed alt alpha elev throttle\n");
|
||||||
Lat_geocentric=Latitude;
|
exit(1);
|
||||||
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");
|
IC.latitude=47.5299892; //BFI
|
||||||
fgLaRCsimInit(0.05);
|
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);
|
||||||
|
|
||||||
/* copy control positions into the LaRCsim structure */
|
/* 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);
|
||||||
|
|
||||||
|
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 */
|
/* 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 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -411,3 +720,4 @@ int fgLaRCsim_2_FGInterface (FGInterface& f) {
|
||||||
return 0;
|
return 0;
|
||||||
} */
|
} */
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue