fly/flightgear
1
0
Fork 0
Code Activity

c172 updates from Tony. Fix extra yaw when using ailerons. Flaps and elevator

Browse source 
tweaks.
This commit is contained in:
curt 1999-11-01 19:17:16 +00:00
parent 6c80732f54
commit 43143ef358
5 changed files with 331 additions and 654 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

36
src/FDM/LaRCsim/c172_aero.c
View file

@ -94,7 +94,7 @@
#include <stdio.h>
#define NCL 11
#define NCL 9
#define Ndf 4
#define DYN_ON_SPEED 33 /*20 knots*/
@ -150,13 +150,13 @@ void aero( SCALAR dt, int Initialize ) {
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 alpha_ind[NCL]={-0.087,0,0.14,0.21,0.24,0.26,0.28,0.31,0.35};
static SCALAR CLtable[NCL]={-0.22,0.25,1.02,1.252,1.354,1.44,1.466,1.298,0.97};
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 dCmf[Ndf]={0,-0.0654,-0.0981,-0.114};
static SCALAR flap_transit_rate=2.5;
@ -188,8 +188,8 @@ void aero( SCALAR dt, int Initialize ) {
Cma=-1.8;
Cmadot=-5.2;
Cmq=-12.4;
Cmob=-0.00;
Cmde=-1.00;
Cmob=-0.02;
Cmde=-1.28;
CLde=-Cmde / lbare; /* kinda backwards, huh? */
@ -202,7 +202,7 @@ void aero( SCALAR dt, int Initialize ) {
Cnbeta=0.065;
Cnp=-0.03;
Cnr=-0.099;
Cnda=-0.053;
Cnda=-0.0053;
Cndr=-0.0657;
Cybeta=-0.31;
@ -247,6 +247,7 @@ void aero( SCALAR dt, int Initialize ) {
Dz_cg=Zcg*cbar;
if(Flap_handle < flap_ind[0])
{
Flap_handle=flap_ind[0];
@ -343,8 +344,11 @@ void aero( SCALAR dt, int Initialize ) {
*/
/* sum coefficients */
CLwbh = interp(CLtable,alpha_ind,NCL,Alpha);
/* printf("CLwbh: %g\n",CLwbh);
*/
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);
@ -352,8 +356,9 @@ void aero( SCALAR dt, int Initialize ) {
/* printf("FP: %g\n",Flap_Position);
printf("CLo: %g\n",CLo);
printf("Cdo: %g\n",Cdo);
printf("Cmo: %g\n",Cmo);
*/
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;
@ -365,6 +370,7 @@ void aero( SCALAR dt, int Initialize ) {
/* 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*/
F_X_wind=-1*cd*qS;
F_Y_wind=cy*qS;
@ -372,6 +378,7 @@ void aero( SCALAR dt, int Initialize ) {
/* printf("V_rel_wind: %7.4f, Fxwind: %7.4f Fywind: %7.4f Fzwind: %7.4f\n",V_rel_wind,F_X_wind,F_Y_wind,F_Z_wind);
*/
/*calculate moments and body axis forces */
@ -387,11 +394,12 @@ void aero( SCALAR dt, int Initialize ) {
M_n_aero = cn*qSb;
/* 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,Weight);
*/
/* 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);
*/
}

9
src/FDM/LaRCsim/c172_aero.h
View file

@ -1,5 +1,12 @@
/*c172_aero.h*/
#ifndef __C172_AERO_H
#define __C172_AERO_H
#include <FDM/LaRCsim/ls_types.h>
/*global declarations of aero model parameters*/
SCALAR CLadot;
@ -64,3 +71,5 @@
/* float Flap_Handle; */
int Flaps_In_Transit;
#endif

5
src/FDM/LaRCsim/c172_gear.c
View file

@ -36,8 +36,9 @@
$Header$
$Log$
Revision 1.8 1999/08/24 21:17:05 curt
Updates from Tony.
Revision 1.9 1999/11/01 18:17:16 curt
c172 updates from Tony. Fix extra yaw when using ailerons. Flaps and elevator
tweaks.
----------------------------------------------------------------------------

2
src/FDM/LaRCsim/c172_init.c
View file

@ -72,8 +72,8 @@ void model_init( void ) {
I_yy=1346;
I_zz=1967;
I_xz=0;
Flap_Position=Flap_handle;
Flaps_In_Transit=0;
}

929
src/FDM/LaRCsim/c172_main.c
View file

@ -31,330 +31,308 @@
#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>
//simple "one-at-a-time" longitudinal trimming routine
typedef struct
{
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
// all angles in degrees
// weight lbs
// cg %MAC
// 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;
Mass=IC.weight*INVG;
Dx_cg=(IC.cg-0.25)*4.9;
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;
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);
}
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=40,sum=0,trim_failed=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;
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))
{
IC.throttle-=U_dot_body*0.01;
setIC(IC);
ls_loop(0.0,-1);
j++;
}
sum+=j;
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))
{
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(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 trim_ground(int kmax, InitialConditions IC)
{
double elevator,alpha,qdot_prev,alt_prev,step;
double tol=1E-3;
double a_tol=tol/10;
double alpha_step=0.001;
int k=0,i,j=0,jmax=40,sum=0,m=0;
Throttle_pct=0;
Brake_pct=1;
Theta=5*DEG_TO_RAD;
IC.altitude=Runway_altitude;
printf("udot: %g\n",U_dot_body);
setIC(IC);
printf("Altitude: %g, Runway_altitude: %g\n",Altitude,Runway_altitude);
qdot_prev=1.0E6;
ls_loop(0.0,-1);
do{
//printf("k: %d\n",k);
step=1;
printf("IC.altitude: %g, Altitude: %g, Runway_altitude: %g,wdot: %g,F_Z_gear: %g, M_m_gear: %g,F_Z: %g\n",IC.altitude,Altitude,Runway_altitude,W_dot_body,F_Z_gear,M_m_gear,F_Z);
m=0;
while((fabs(W_dot_body) > tol) && (m < 10))
{
j=0;
do{
alt_prev=IC.altitude;
IC.altitude+=step;
setIC(IC);
ls_loop(0.0,-1);
printf("IC.altitude: %g, Altitude: %g, Runway_altitude: %g,wdot: %g,F_Z: %g\n",IC.altitude,Altitude,Runway_altitude,W_dot_body,F_Z);
j++;
}while((W_dot_body < 0) && (j < jmax));
IC.altitude-=step;
step/=10;
printf("step: %g\n",step);
m++;
}
sum+=j;
printf("IC.altitude: %g, Altitude: %g, Runway_altitude: %g,wdot: %g,F_Z_gear: %g, M_m_gear: %g,F_Z: %g\n",IC.altitude,Altitude,Runway_altitude,W_dot_body,F_Z_gear,M_m_gear,F_Z);
j=0;
while((Q_dot_body <= qdot_prev) && (j < jmax))
{
qdot_prev=Q_dot_body;
IC.theta+=Q_dot_body;
setIC(IC);
ls_loop(0.0,-1);
j++;
printf("\tTheta: %7.4f, qdot: %10.6f, qdot_prev: %10.6f, j: %d\n",Theta*RAD_TO_DEG,Q_dot_body,qdot_prev,j);
}
IC.theta-=qdot_prev;
sum+=j;
printf("\tTheta: %7.4f, qdot: %10.6f, W_dot_body: %g\n",Theta,Q_dot_body,W_dot_body);
j=0;
if(W_dot_body > tol)
{
step=1;
while((W_dot_body > 0) && (j <jmax))
{
IC.altitude-=step;
setIC(IC);
ls_loop(0.0,-1);
j++;
}
}
k++;j=0;
}while(((fabs(W_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,i;
double speed,elevator,cmcl;
out=fopen("trims.oldCmde.out","w");
int k=0,i,j;
double speed,elevator,cmcl,maxspeed;
out=fopen("trims.out","w");
speed=55;
for(i=1;i<=5;i++)
for(j=0;j<=30;j+=10)
{
switch(i)
IC.flap_handle=j;
for(i=1;i<=5;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=2550;IC.cg=0.257;break;
case 5: IC.weight=2550;IC.cg=0.364;break;
}
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=2550;IC.cg=0.257;break;
case 5: IC.weight=2550;IC.cg=0.364;break;
}
speed=50;
while(speed <= 150)
{
IC.vc=speed;
Long_control=0;Theta=0;Throttle_pct=0.0;
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;
k=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(k < kmax)
{
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,%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\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;
}
}
k=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(k < kmax)
{
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("kmax exceeded 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);
}
speed+=10;
}
}
}
fclose(out);
}
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
// each subsequent iteration through the EOM
int fgLaRCsimInit(double dt) {
ls_toplevel_init(dt);
return(1);
}
// Run an iteration of the EOM (equations of motion)
int main(int argc, char *argv[]) {
double save_alt = 0.0;
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=0;
IC.beta=0;
IC.gamma=strtod(argv[3],NULL);
IC.use_gamma_tmg=1;
IC.phi=0;
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);
IC.altitude=1000;
setIC(IC);
ls_loop(0.0,-1);
IC.flap_handle=10;
setIC(IC);
ls_loop(0.0,-1);
IC.flap_handle=20;
setIC(IC);
ls_loop(0.0,-1);
IC.flap_handle=30;
setIC(IC);
ls_loop(0.0,-1);
/* find_trim_stall(200,out,IC);
IC.vc=120;
IC.altitude=8000;
IC.weight=2300;
IC.cg=0.25;
IC.flap_handle=0;
setIC(IC);
printIC(IC);
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);
IC.cg=0.155;
setIC(IC);
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);
IC.cg=0.364;
setIC(IC);
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);
*/
/* 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;
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)
{
@ -382,9 +360,9 @@ void do_stick_pull(int kmax, SCALAR tmax,FILE *out,InitialConditions IC)
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);
//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);
}
@ -407,9 +385,9 @@ void do_stick_pull(int kmax, SCALAR tmax,FILE *out,InitialConditions IC)
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);
//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);
@ -432,14 +410,14 @@ void do_takeoff(FILE *out)
out=fopen("takeoff.out","w");
herr=Q_body-htarget;
//attempt to maintain zero pitch rate during the roll
// attempt to maintain zero pitch rate during the roll
while((V_calibrated_kts < 61) && (time < 30.0))
{
/* herrprev=herr;*/
// herrprev=herr
ls_update(1);
/*herr=Q_body-htarget;
herr_diff=herr-herrprev;
Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff); */
// 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);
@ -486,325 +464,6 @@ void do_takeoff(FILE *out)
}
// Initialize the LaRCsim flight model, dt is the time increment for
// each subsequent iteration through the EOM
int fgLaRCsimInit(double dt) {
ls_toplevel_init(dt);
return(1);
}
// Run an iteration of the EOM (equations of motion)
int main(int argc, char *argv[]) {
double save_alt = 0.0;
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=0;
IC.beta=0;
IC.gamma=strtod(argv[3],NULL);
IC.use_gamma_tmg=1;
IC.phi=0;
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);
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)
{
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;
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;
}
/*// 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 );
}
*/