fly/flightgear
1
0
Fork 0
Code Activity

Updated Tony's c172 model code.

Browse source
This commit is contained in:
curt 1999-08-19 21:24:03 +00:00
parent 926849a2d3
commit 2eaa10e1d0
5 changed files with 615 additions and 344 deletions

210
Simulator/FDM/LaRCsim/c172_aero.c
View file

@ -88,6 +88,8 @@
#include "ls_cockpit.h"
#include "ls_constants.h"
#include "ls_types.h"
#include "c172_aero.h"
#include <math.h>
#include <stdio.h>
@ -104,7 +106,7 @@
extern COCKPIT cockpit_;
FILE *out;
SCALAR interp(SCALAR *y_table, SCALAR *x_table, int Ntable, SCALAR x)
{
@ -121,9 +123,11 @@ SCALAR interp(SCALAR *y_table, SCALAR *x_table, int Ntable, SCALAR x)
}
else if(x >= x_table[Ntable-1])
{
y=y_table[Ntable-1];
/* printf("x larger than x_table[N]: %g %g %d\n",x,x_table[NCL-1],Ntable-1); */
}
slope=(y_table[Ntable-1]-y_table[Ntable-2])/(x_table[Ntable-1]-x_table[Ntable-2]);
y=slope*(x-x_table[Ntable-1]) +y_table[Ntable-1];
/* printf("x larger than x_table[N]: %g %g %d\n",x,x_table[NCL-1],Ntable-1);
*/ }
else /*x is within the table, interpolate linearly to find y value*/
{
@ -135,97 +139,67 @@ SCALAR interp(SCALAR *y_table, SCALAR *x_table, int Ntable, SCALAR x)
return y;
}
void record()
{
fprintf(out,"%g,%g,%g,%g,%g,%g,%g,%g,%g,",Long_control,Lat_control,Rudder_pedal,Aft_trim,Fwd_trim,V_rel_wind,Dynamic_pressure,P_body,R_body);
fprintf(out,"%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,",Alpha,Cos_alpha,Sin_alpha,Alpha_dot,Q_body,Theta_dot,Sin_theta,Cos_theta,Beta,Cos_beta,Sin_beta);
fprintf(out,"%g,%g,%g,%g,%g,%g,%g,%g\n",Sin_phi,Cos_phi,F_X_aero,F_Y_aero,F_Z_aero,M_l_aero,M_m_aero,M_n_aero);
fflush(out);
}
void aero( SCALAR dt, int Initialize ) {
static int init = 0;
static SCALAR trim_inc = 0.0002;
SCALAR long_trim;
SCALAR elevator, aileron, rudder;
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};
/* printf("Initialize= %d\n",Initialize); */
/* printf("Initializing aero model...Initialize= %d\n", Initialize);
*/ CLadot=1.7;
CLq=3.9;
CLde=0.43;
CLo=0;
/*Note that CLo,Cdo,Cmo will likely change with flap setting so
they may not be declared static in the future */
Cdo=0.031;
Cda=0.13; /*Not used*/
Cdde=0.06;
Cma=-0.89;
Cmadot=-5.2;
Cmq=-12.4;
Cmo=-0.015;
Cmde=-1.28;
static SCALAR CLadot=1.7;
static SCALAR CLq=3.9;
static SCALAR CLde=0.43;
static SCALAR CLo=0;
Clbeta=-0.089;
Clp=-0.47;
Clr=0.096;
Clda=-0.178;
Cldr=0.0147;
Cnbeta=0.065;
Cnp=-0.03;
Cnr=-0.099;
Cnda=-0.053;
Cndr=-0.0657;
static SCALAR Cdo=0.031;
static SCALAR Cda=0.13; /*Not used*/
static SCALAR Cdde=0.06;
static SCALAR Cma=-0.89;
static SCALAR Cmadot=-5.2;
static SCALAR Cmq=-12.4;
static SCALAR Cmo=-0.062;
static SCALAR Cmde=-1.28;
static SCALAR Clbeta=-0.089;
static SCALAR Clp=-0.47;
static SCALAR Clr=0.096;
static SCALAR Clda=0.178;
static SCALAR Cldr=0.0147;
static SCALAR Cnbeta=0.065;
static SCALAR Cnp=-0.03;
static SCALAR Cnr=-0.099;
static SCALAR Cnda=-0.053;
static SCALAR Cndr=-0.0657;
static SCALAR Cybeta=-0.31;
static SCALAR Cyp=-0.037;
static SCALAR Cyr=0.21;
static SCALAR Cyda=0.0;
static SCALAR Cydr=0.187;
Cybeta=-0.31;
Cyp=-0.037;
Cyr=0.21;
Cyda=0.0;
Cydr=0.187;
/*nondimensionalization quantities*/
/*units here are ft and lbs */
static SCALAR cbar=4.9; /*mean aero chord ft*/
static SCALAR b=35.8; /*wing span ft */
static SCALAR Sw=174; /*wing planform surface area ft^2*/
static SCALAR rPiARe=0.054; /*reciprocal of Pi*AR*e*/
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*/
SCALAR W=Mass/INVG;
MaxTakeoffWeight=2550;
EmptyWeight=1500;
SCALAR CLwbh,CL,cm,cd,cn,cy,croll,cbar_2V,b_2V,qS,qScbar,qSb,ps,rs;
SCALAR F_X_wind,F_Y_wind,F_Z_wind,W_X,W_Y,W_Z;
if (Initialize != 0)
{
out=fopen("flight.csv","w");
/* Initialize aero coefficients */
}
record();
Zcg=0.51;
/*
LaRCsim uses:
@ -238,38 +212,47 @@ void aero( SCALAR dt, int Initialize ) {
rudder > 0 => ANL
*/
/*do weight & balance here since there is no better place*/
Weight=Mass / INVG;
if(Weight > 2550)
{ Weight=2550; }
else if(Weight < 1500)
{ Weight=1500; }
if(Dx_cg > 0.5586)
{ Dx_cg = 0.5586; }
else if(Dx_cg < -0.4655)
{ Dx_cg = -0.4655; }
Cg=Dx_cg/cbar +0.25;
Dz_cg=Zcg*cbar;
long_trim=0;
if(Aft_trim) long_trim = long_trim - trim_inc;
if(Fwd_trim) long_trim = long_trim + trim_inc;
/*scale pct control to degrees deflection*/
/* printf("Long_control: %7.4f, long_trim: %7.4f,DEG_TO_RAD: %7.4f, RAD_TO_DEG: %7.4f\n",Long_control,long_trim,DEG_TO_RAD,RAD_TO_DEG);
*/ /*scale pct control to degrees deflection*/
if ((Long_control+long_trim) <= 0)
elevator=(Long_control+long_trim)*-28*DEG_TO_RAD;
elevator=(Long_control+long_trim)*28*DEG_TO_RAD;
else
elevator=(Long_control+long_trim)*23*DEG_TO_RAD;
aileron = Lat_control*17.5*DEG_TO_RAD;
rudder = Rudder_pedal*16*DEG_TO_RAD;
/*check control surface travel limits*/
/* if((elevator+long_trim) > 23)
elevator=23;
else if((elevator+long_trim) < -28)
elevator=-23; */
aileron = -1*Lat_control*17.5*DEG_TO_RAD;
rudder = -1*Rudder_pedal*16*DEG_TO_RAD;
/*
The aileron travel limits are 20 deg. TEU and 15 deg TED
but since we don't distinguish between left and right we'll
use the average here (17.5 deg)
*/
/* if(fabs(aileron) > 17.5)
aileron = 17.5;
if(fabs(rudder) > 16)
rudder = 16; */
/*calculate rate derivative nondimensionalization (is that a word?) factors */
/*hack to avoid divide by zero*/
@ -286,50 +269,55 @@ void aero( SCALAR dt, int Initialize ) {
b_2V=0;
}
/*calcuate the qS nondimensionalization factors*/
qS=Dynamic_pressure*Sw;
qScbar=qS*cbar;
qSb=qS*b;
/*transform the aircraft rotation rates*/
ps=-P_body*Cos_alpha + R_body*Sin_alpha;
rs=-P_body*Sin_alpha + R_body*Cos_alpha;
/* 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);
/* sum coefficients */
CLwbh = interp(CLtable,alpha_ind,NCL,Alpha);
CL = CLo + CLwbh + (CLadot*Alpha_dot + CLq*Theta_dot)*cbar_2V + CLde*elevator;
cd = Cdo + rPiARe*CL*CL + Cdde*elevator;
cy = Cybeta*Beta + (Cyp*ps + Cyr*rs)*b_2V + Cyda*aileron + Cydr*rudder;
cy = Cybeta*Beta + (Cyp*P_body + Cyr*R_body)*b_2V + Cyda*aileron + Cydr*rudder;
cm = Cmo + Cma*Alpha + (Cmq*Theta_dot + Cmadot*Alpha_dot)*cbar_2V + Cmde*(elevator+long_trim);
cn = Cnbeta*Beta + (Cnp*ps + Cnr*rs)*b_2V + Cnda*aileron + Cndr*rudder;
croll=Clbeta*Beta + (Clp*ps + Clr*rs)*b_2V + Clda*aileron + Cldr*rudder;
cm = Cmo + Cma*Alpha + (Cmq*Q_body + Cmadot*Alpha_dot)*cbar_2V + Cmde*(elevator+long_trim);
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;
/*calculate wind axes forces*/
/* 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;
F_Z_wind=-1*CL*qS;
/* 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 */
/*find body-axis components of weight*/
/*with earth axis to body axis transform */
W_X=-1*W*Sin_theta;
W_Y=W*Sin_phi*Cos_theta;
W_Z=W*Cos_phi*Cos_theta;
/* requires ugly wind-axes to body-axes transform */
F_X_aero = W_X + F_X_wind*Cos_alpha*Cos_beta - F_Y_wind*Cos_alpha*Sin_beta - F_Z_wind*Sin_alpha;
F_Y_aero = W_Y + F_X_wind*Sin_beta + F_Z_wind*Cos_beta;
F_Z_aero = W_Z*NZ + F_X_wind*Sin_alpha*Cos_beta - F_Y_wind*Sin_alpha*Sin_beta + F_Z_wind*Cos_alpha;
F_X_aero = F_X_wind*Cos_alpha*Cos_beta - F_Y_wind*Cos_alpha*Sin_beta - F_Z_wind*Sin_alpha;
F_Y_aero = F_X_wind*Sin_beta + F_Y_wind*Cos_beta;
F_Z_aero = F_X_wind*Sin_alpha*Cos_beta - F_Y_wind*Sin_alpha*Sin_beta + F_Z_wind*Cos_alpha;
/*no axes transform here */
M_l_aero = I_xx*croll*qSb;
M_m_aero = I_yy*cm*qScbar;
M_n_aero = I_zz*cn*qSb;
M_l_aero = croll*qSb;
M_m_aero = cm*qScbar;
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,W);
*//* printf("Maero: %7.4f Naero: %7.4f Raero: %7.4f\n",M_m_aero,M_n_aero,M_l_aero);
*/
}

7
Simulator/FDM/LaRCsim/c172_engine.c
View file

@ -65,6 +65,7 @@ $Header$
#include "ls_generic.h"
#include "ls_sim_control.h"
#include "ls_cockpit.h"
#include "c172_aero.h"
extern SIM_CONTROL sim_control_;
@ -75,8 +76,10 @@ void engine( SCALAR dt, int init ) {
/* F_X_engine = Throttle[3]*813.4/0.2; */ /* original code */
/* F_Z_engine = Throttle[3]*11.36/0.2; */ /* original code */
F_X_engine = Throttle[3]*813.4/0.83;
F_Z_engine = Throttle[3]*11.36/0.83;
F_X_engine = Throttle[3]*350/0.83;
F_Z_engine = Throttle[3]*4.9/0.83;
M_m_engine = F_X_engine*0.734*cbar;
/* 0.734 - estimated (WAGged) location of thrust line in the z-axis*/
Throttle_pct = Throttle[3];
}

50
Simulator/FDM/LaRCsim/c172_gear.c
View file

@ -12,8 +12,7 @@
----------------------------------------------------------------------------
GENEALOGY: Renamed navion_gear.c originally created 931012 by E. B. Jackson
GENEALOGY: Created 931012 by E. B. Jackson
----------------------------------------------------------------------------
@ -37,33 +36,8 @@
$Header$
$Log$
Revision 1.1 1999/06/15 20:05:27 curt
Added c172 model from Tony Peden.
Revision 1.1.1.1 1999/04/05 21:32:45 curt
Start of 0.6.x branch.
Revision 1.6 1998/10/17 01:34:16 curt
C++ ifying ...
Revision 1.5 1998/09/29 02:03:00 curt
Added a brake + autopilot mods.
Revision 1.4 1998/08/06 12:46:40 curt
Header change.
Revision 1.3 1998/02/03 23:20:18 curt
Lots of little tweaks to fix various consistency problems discovered by
Solaris' CC. Fixed a bug in fg_debug.c with how the fgPrintf() wrapper
passed arguments along to the real printf(). Also incorporated HUD changes
by Michele America.
Revision 1.2 1998/01/19 18:40:29 curt
Tons of little changes to clean up the code and to remove fatal errors
when building with the c++ compiler.
Revision 1.1 1997/05/29 00:10:02 curt
Initial Flight Gear revision.
Revision 1.2 1999/08/19 21:24:03 curt
Updated Tony's c172 model code.
----------------------------------------------------------------------------
@ -94,47 +68,48 @@ Initial Flight Gear revision.
#include "ls_cockpit.h"
void sub3( DATA v1[], DATA v2[], DATA result[] )
sub3( DATA v1[], DATA v2[], DATA result[] )
{
result[0] = v1[0] - v2[0];
result[1] = v1[1] - v2[1];
result[2] = v1[2] - v2[2];
}
void add3( DATA v1[], DATA v2[], DATA result[] )
add3( DATA v1[], DATA v2[], DATA result[] )
{
result[0] = v1[0] + v2[0];
result[1] = v1[1] + v2[1];
result[2] = v1[2] + v2[2];
}
void cross3( DATA v1[], DATA v2[], DATA result[] )
cross3( DATA v1[], DATA v2[], DATA result[] )
{
result[0] = v1[1]*v2[2] - v1[2]*v2[1];
result[1] = v1[2]*v2[0] - v1[0]*v2[2];
result[2] = v1[0]*v2[1] - v1[1]*v2[0];
}
void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
{
result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
}
void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
{
result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
}
void clear3( DATA v[] )
clear3( DATA v[] )
{
v[0] = 0.; v[1] = 0.; v[2] = 0.;
}
void gear( SCALAR dt, int Initialize ) {
gear()
{
char rcsid[] = "$Id$";
/*
@ -220,8 +195,7 @@ char rcsid[] = "$Id$";
* Put aircraft specific executable code here
*/
/* replace with cockpit brake handle connection code */
percent_brake[1] = Brake_pct;
percent_brake[1] = 0.; /* replace with cockpit brake handle connection code */
percent_brake[2] = percent_brake[1];
caster_angle_rad[0] = 0.03*Rudder_pedal;

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

@ -63,7 +63,7 @@
void model_init( void ) {
Throttle[3] = 0.2; Rudder_pedal = 0; Lat_control = 0; Long_control = 0;
Throttle[3] = 0.2;
Dx_pilot = 0; Dy_pilot = 0; Dz_pilot = 0;
Mass=2300*INVG;

660
Simulator/FDM/LaRCsim/c172_main.c
View file

@ -28,7 +28,423 @@
#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;
double weight,cg;
int use_gamma_tmg;
}InitialConditions;
// 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;
}
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=10,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;
}
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.out","w");
speed=55;
for(i=1;i<=5;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=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;
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\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
@ -39,57 +455,82 @@ int fgLaRCsimInit(double dt) {
}
// 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,cmcl;
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);
}
printf("Calling init...\n");
fgLaRCsimInit(0.05);
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.beta=0;
IC.theta=strtod(argv[3],NULL);
IC.use_gamma_tmg=0;
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");
ls_ForceAltitude(IC.altitude);
fgLaRCsimInit(0.01);
/* copy control positions into the LaRCsim structure */
while(IC.alpha < 30.0)
{
setIC(IC);
ls_loop(0.0,-1);
printf("CL: %g ,Alpha: %g\n",CL,IC.alpha);
IC.alpha+=1.0;
}
/*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)
{
ls_update(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);
*/
/* 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); */
@ -272,142 +713,7 @@ int FGInterface_2_LaRCsim (FGInterface& f) {
return( 0 );
}
*/
// 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 );
// 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;
} */