Updated Tony's c172 model code.
This commit is contained in:
curt
parent
926849a2d3
commit
2eaa10e1d0
5 changed files with 615 additions and 344 deletions
Simulator/FDM/LaRCsim
|
|
@ -88,6 +88,8 @@
|
||||||
#include "ls_cockpit.h"
|
#include "ls_cockpit.h"
|
||||||
#include "ls_constants.h"
|
#include "ls_constants.h"
|
||||||
#include "ls_types.h"
|
#include "ls_types.h"
|
||||||
|
#include "c172_aero.h"
|
||||||
|
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
|
|
@ -104,7 +106,7 @@
|
||||||
|
|
||||||
|
|
||||||
extern COCKPIT cockpit_;
|
extern COCKPIT cockpit_;
|
||||||
FILE *out;
|
|
||||||
|
|
||||||
SCALAR interp(SCALAR *y_table, SCALAR *x_table, int Ntable, SCALAR x)
|
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])
|
else if(x >= x_table[Ntable-1])
|
||||||
{
|
{
|
||||||
y=y_table[Ntable-1];
|
slope=(y_table[Ntable-1]-y_table[Ntable-2])/(x_table[Ntable-1]-x_table[Ntable-2]);
|
||||||
/* printf("x larger than x_table[N]: %g %g %d\n",x,x_table[NCL-1],Ntable-1); */
|
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*/
|
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;
|
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 ) {
|
void aero( SCALAR dt, int Initialize ) {
|
||||||
|
|
||||||
|
|
||||||
static int init = 0;
|
static int init = 0;
|
||||||
|
|
||||||
|
|
||||||
static SCALAR trim_inc = 0.0002;
|
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 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 CLtable[NCL]={-0.14,0.31,1.21,1.376,1.51249,1.591,1.63,1.60878,1.53712,1.376,1.142};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/*Note that CLo,Cdo,Cmo will likely change with flap setting so
|
|
||||||
they may not be declared static in the future */
|
|
||||||
|
|
||||||
|
|
||||||
static SCALAR CLadot=1.7;
|
|
||||||
static SCALAR CLq=3.9;
|
|
||||||
static SCALAR CLde=0.43;
|
|
||||||
static SCALAR CLo=0;
|
|
||||||
|
|
||||||
|
|
||||||
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;
|
|
||||||
|
|
||||||
/*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*/
|
|
||||||
|
|
||||||
SCALAR W=Mass/INVG;
|
|
||||||
|
|
||||||
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)
|
/* printf("Initialize= %d\n",Initialize); */
|
||||||
{
|
/* printf("Initializing aero model...Initialize= %d\n", Initialize);
|
||||||
|
*/ CLadot=1.7;
|
||||||
|
CLq=3.9;
|
||||||
|
CLde=0.43;
|
||||||
|
CLo=0;
|
||||||
|
|
||||||
out=fopen("flight.csv","w");
|
|
||||||
/* Initialize aero coefficients */
|
|
||||||
|
|
||||||
|
Cdo=0.031;
|
||||||
}
|
Cda=0.13; /*Not used*/
|
||||||
|
Cdde=0.06;
|
||||||
record();
|
|
||||||
|
Cma=-0.89;
|
||||||
|
Cmadot=-5.2;
|
||||||
|
Cmq=-12.4;
|
||||||
|
Cmo=-0.015;
|
||||||
|
Cmde=-1.28;
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
Cybeta=-0.31;
|
||||||
|
Cyp=-0.037;
|
||||||
|
Cyr=0.21;
|
||||||
|
Cyda=0.0;
|
||||||
|
Cydr=0.187;
|
||||||
|
|
||||||
|
/*nondimensionalization quantities*/
|
||||||
|
/*units here are ft and lbs */
|
||||||
|
cbar=4.9; /*mean aero chord ft*/
|
||||||
|
b=35.8; /*wing span ft */
|
||||||
|
Sw=174; /*wing planform surface area ft^2*/
|
||||||
|
rPiARe=0.054; /*reciprocal of Pi*AR*e*/
|
||||||
|
|
||||||
|
MaxTakeoffWeight=2550;
|
||||||
|
EmptyWeight=1500;
|
||||||
|
|
||||||
|
Zcg=0.51;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
LaRCsim uses:
|
LaRCsim uses:
|
||||||
|
|
@ -238,38 +212,47 @@ void aero( SCALAR dt, int Initialize ) {
|
||||||
rudder > 0 => ANL
|
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(Aft_trim) long_trim = long_trim - trim_inc;
|
||||||
if(Fwd_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)
|
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
|
else
|
||||||
elevator=(Long_control+long_trim)*23*DEG_TO_RAD;
|
elevator=(Long_control+long_trim)*23*DEG_TO_RAD;
|
||||||
|
|
||||||
aileron = Lat_control*17.5*DEG_TO_RAD;
|
aileron = -1*Lat_control*17.5*DEG_TO_RAD;
|
||||||
rudder = Rudder_pedal*16*DEG_TO_RAD;
|
rudder = -1*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; */
|
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
The aileron travel limits are 20 deg. TEU and 15 deg TED
|
The aileron travel limits are 20 deg. TEU and 15 deg TED
|
||||||
but since we don't distinguish between left and right we'll
|
but since we don't distinguish between left and right we'll
|
||||||
use the average here (17.5 deg)
|
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 */
|
/*calculate rate derivative nondimensionalization (is that a word?) factors */
|
||||||
/*hack to avoid divide by zero*/
|
/*hack to avoid divide by zero*/
|
||||||
|
|
@ -286,50 +269,55 @@ void aero( SCALAR dt, int Initialize ) {
|
||||||
b_2V=0;
|
b_2V=0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*calcuate the qS nondimensionalization factors*/
|
/*calcuate the qS nondimensionalization factors*/
|
||||||
|
|
||||||
qS=Dynamic_pressure*Sw;
|
qS=Dynamic_pressure*Sw;
|
||||||
qScbar=qS*cbar;
|
qScbar=qS*cbar;
|
||||||
qSb=qS*b;
|
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 */
|
/* sum coefficients */
|
||||||
CLwbh = interp(CLtable,alpha_ind,NCL,Alpha);
|
CLwbh = interp(CLtable,alpha_ind,NCL,Alpha);
|
||||||
CL = CLo + CLwbh + (CLadot*Alpha_dot + CLq*Theta_dot)*cbar_2V + CLde*elevator;
|
CL = CLo + CLwbh + (CLadot*Alpha_dot + CLq*Theta_dot)*cbar_2V + CLde*elevator;
|
||||||
cd = Cdo + rPiARe*CL*CL + Cdde*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);
|
cm = Cmo + Cma*Alpha + (Cmq*Q_body + Cmadot*Alpha_dot)*cbar_2V + Cmde*(elevator+long_trim);
|
||||||
cn = Cnbeta*Beta + (Cnp*ps + Cnr*rs)*b_2V + Cnda*aileron + Cndr*rudder;
|
cn = Cnbeta*Beta + (Cnp*P_body + Cnr*R_body)*b_2V + Cnda*aileron + Cndr*rudder;
|
||||||
croll=Clbeta*Beta + (Clp*ps + Clr*rs)*b_2V + Clda*aileron + Cldr*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_X_wind=-1*cd*qS;
|
||||||
F_Y_wind=cy*qS;
|
F_Y_wind=cy*qS;
|
||||||
F_Z_wind=-1*CL*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 */
|
/*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 */
|
/* 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_X_aero = 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_Y_aero = F_X_wind*Sin_beta + F_Y_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_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 */
|
/*no axes transform here */
|
||||||
M_l_aero = I_xx*croll*qSb;
|
M_l_aero = croll*qSb;
|
||||||
M_m_aero = I_yy*cm*qScbar;
|
M_m_aero = cm*qScbar;
|
||||||
M_n_aero = I_zz*cn*qSb;
|
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);
|
||||||
|
*/
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -65,6 +65,7 @@ $Header$
|
||||||
#include "ls_generic.h"
|
#include "ls_generic.h"
|
||||||
#include "ls_sim_control.h"
|
#include "ls_sim_control.h"
|
||||||
#include "ls_cockpit.h"
|
#include "ls_cockpit.h"
|
||||||
|
#include "c172_aero.h"
|
||||||
|
|
||||||
extern SIM_CONTROL sim_control_;
|
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_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]*813.4/0.83;
|
F_X_engine = Throttle[3]*350/0.83;
|
||||||
F_Z_engine = Throttle[3]*11.36/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];
|
Throttle_pct = Throttle[3];
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -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$
|
$Header$
|
||||||
$Log$
|
$Log$
|
||||||
Revision 1.1 1999/06/15 20:05:27 curt
|
Revision 1.2 1999/08/19 21:24:03 curt
|
||||||
Added c172 model from Tony Peden.
|
Updated Tony's c172 model code.
|
||||||
|
|
||||||
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.
|
|
||||||
|
|
||||||
|
|
||||||
----------------------------------------------------------------------------
|
----------------------------------------------------------------------------
|
||||||
|
|
@ -94,47 +68,48 @@ Initial Flight Gear revision.
|
||||||
#include "ls_cockpit.h"
|
#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[0] = v1[0] - v2[0];
|
||||||
result[1] = v1[1] - v2[1];
|
result[1] = v1[1] - v2[1];
|
||||||
result[2] = v1[2] - v2[2];
|
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[0] = v1[0] + v2[0];
|
||||||
result[1] = v1[1] + v2[1];
|
result[1] = v1[1] + v2[1];
|
||||||
result[2] = v1[2] + v2[2];
|
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[0] = v1[1]*v2[2] - v1[2]*v2[1];
|
||||||
result[1] = v1[2]*v2[0] - v1[0]*v2[2];
|
result[1] = v1[2]*v2[0] - v1[0]*v2[2];
|
||||||
result[2] = v1[0]*v2[1] - v1[1]*v2[0];
|
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[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[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];
|
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[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[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];
|
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.;
|
v[0] = 0.; v[1] = 0.; v[2] = 0.;
|
||||||
}
|
}
|
||||||
|
|
||||||
void gear( SCALAR dt, int Initialize ) {
|
gear()
|
||||||
|
{
|
||||||
char rcsid[] = "$Id$";
|
char rcsid[] = "$Id$";
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
@ -220,8 +195,7 @@ char rcsid[] = "$Id$";
|
||||||
* Put aircraft specific executable code here
|
* Put aircraft specific executable code here
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* replace with cockpit brake handle connection code */
|
percent_brake[1] = 0.; /* replace with cockpit brake handle connection code */
|
||||||
percent_brake[1] = Brake_pct;
|
|
||||||
percent_brake[2] = percent_brake[1];
|
percent_brake[2] = percent_brake[1];
|
||||||
|
|
||||||
caster_angle_rad[0] = 0.03*Rudder_pedal;
|
caster_angle_rad[0] = 0.03*Rudder_pedal;
|
||||||
|
|
|
||||||
|
|
@ -63,7 +63,7 @@
|
||||||
|
|
||||||
void model_init( void ) {
|
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;
|
Dx_pilot = 0; Dy_pilot = 0; Dz_pilot = 0;
|
||||||
Mass=2300*INVG;
|
Mass=2300*INVG;
|
||||||
|
|
|
||||||
|
|
@ -28,7 +28,423 @@
|
||||||
#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;
|
||||||
|
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
|
// 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 +455,85 @@ 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,cmcl;
|
||||||
|
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;
|
|
||||||
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;
|
|
||||||
|
|
||||||
printf("Calling init...\n");
|
|
||||||
fgLaRCsimInit(0.05);
|
|
||||||
|
|
||||||
/* copy control positions into the LaRCsim structure */
|
|
||||||
|
|
||||||
|
|
||||||
/* 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;
|
printf("Need args: $c172 speed alt alpha elev throttle\n");
|
||||||
ls_update(multiloop);
|
exit(1);
|
||||||
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);
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
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;
|
||||||
}
|
}
|
||||||
/* // printf("%d FG_Altitude = %.2f\n", i, FG_Altitude * 0.3048);
|
fclose(out);
|
||||||
// printf("%d Altitude = %.2f\n", i, Altitude * 0.3048);
|
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 */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 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); */
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -272,142 +713,7 @@ int FGInterface_2_LaRCsim (FGInterface& f) {
|
||||||
|
|
||||||
return( 0 );
|
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;
|
|
||||||
} */
|
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Add table
Reference in a new issue