fly/flightgear
1
0
Fork 0
Code Activity

Added c172 model from Tony Peden.

Browse source
This commit is contained in:
curt 1999-06-15 20:05:26 +00:00
parent b32c928653
commit 11a964d830
5 changed files with 1248 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

335
Simulator/FDM/LaRCsim/c172_aero.c Normal file
View file

@ -0,0 +1,335 @@
/***************************************************************************
TITLE: c172_aero
----------------------------------------------------------------------------
FUNCTION: aerodynamics model based on constant stability derivatives
----------------------------------------------------------------------------
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Based on data from:
Part 1 of Roskam's S&C text
The FAA type certificate data sheet for the 172
Various sources on the net
John D. Anderson's Intro to Flight text (NACA 2412 data)
UIUC's airfoil data web site
----------------------------------------------------------------------------
DESIGNED BY: Tony Peden
CODED BY: Tony Peden
MAINTAINED BY: Tony Peden
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
6/10/99 Initial test release
----------------------------------------------------------------------------
REFERENCES:
Aero Coeffs:
CL lift
Cd drag
Cm pitching moment
Cy sideforce
Cn yawing moment
Croll,Cl rolling moment (yeah, I know. Shoot me.)
Subscripts
o constant i.e. not a function of alpha or beta
a alpha
adot d(alpha)/dt
q pitch rate
qdot d(q)/dt
beta sideslip angle
p roll rate
r yaw rate
da aileron deflection
de elevator deflection
dr rudder deflection
s stability axes
----------------------------------------------------------------------------
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
--------------------------------------------------------------------------*/
#include "ls_generic.h"
#include "ls_cockpit.h"
#include "ls_constants.h"
#include "ls_types.h"
#include <math.h>
#include <stdio.h>
#define NCL 11
#define DYN_ON_SPEED 33 /*20 knots*/
#ifdef USENZ
#define NZ generic_.n_cg_body_v[2]
#else
#define NZ 1
#endif
extern COCKPIT cockpit_;
FILE *out;
SCALAR interp(SCALAR *y_table, SCALAR *x_table, int Ntable, SCALAR x)
{
SCALAR slope;
int i=1;
float y;
/* if x is outside the table, return value at x[0] or x[Ntable-1]*/
if(x <= x_table[0])
{
y=y_table[0];
/* printf("x smaller than x_table[0]: %g %g\n",x,x_table[0]); */
}
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); */
}
else /*x is within the table, interpolate linearly to find y value*/
{
while(x_table[i] <= x) {i++;}
slope=(y_table[i]-y_table[i-1])/(x_table[i]-x_table[i-1]);
/* printf("x: %g, i: %d, cl[i]: %g, cl[i-1]: %g, slope: %g\n",x,i,y_table[i],y_table[i-1],slope); */
y=slope*(x-x_table[i-1]) +y_table[i-1];
}
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};
/*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)
{
out=fopen("flight.csv","w");
/* Initialize aero coefficients */
}
record();
/*
LaRCsim uses:
Cm > 0 => ANU
Cl > 0 => Right wing down
Cn > 0 => ANL
so:
elevator > 0 => AND -- aircraft nose down
aileron > 0 => right wing up
rudder > 0 => ANL
*/
if(Aft_trim) long_trim = long_trim - trim_inc;
if(Fwd_trim) long_trim = long_trim + trim_inc;
/*scale pct control to degrees deflection*/
if ((Long_control+long_trim) <= 0)
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; */
/*
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*/
/*the dynamic terms might be negligible at low ground speeds anyway*/
if(V_rel_wind > DYN_ON_SPEED)
{
cbar_2V=cbar/(2*V_rel_wind);
b_2V=b/(2*V_rel_wind);
}
else
{
cbar_2V=0;
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;
/* 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;
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;
/*calculate wind axes forces*/
F_X_wind=-1*cd*qS;
F_Y_wind=cy*qS;
F_Z_wind=-1*CL*qS;
/*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;
/*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;
}

84
Simulator/FDM/LaRCsim/c172_engine.c Normal file
View file

@ -0,0 +1,84 @@
/***************************************************************************
TITLE: engine.c
----------------------------------------------------------------------------
FUNCTION: dummy engine routine
----------------------------------------------------------------------------
MODULE STATUS: incomplete
----------------------------------------------------------------------------
GENEALOGY: This is a renamed navion_engine.c originall written by E. Bruce
Jackson
----------------------------------------------------------------------------
DESIGNED BY: designer
CODED BY: programmer
MAINTAINED BY: maintainer
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
CURRENT RCS HEADER INFO:
$Header$
* Revision 1.1 92/12/30 13:21:46 bjax
* Initial revision
*
----------------------------------------------------------------------------
REFERENCES:
----------------------------------------------------------------------------
CALLED BY: ls_model();
----------------------------------------------------------------------------
CALLS TO: none
----------------------------------------------------------------------------
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
#include "ls_types.h"
#include "ls_constants.h"
#include "ls_generic.h"
#include "ls_sim_control.h"
#include "ls_cockpit.h"
extern SIM_CONTROL sim_control_;
void engine( SCALAR dt, int init ) {
/* if (init) { */
Throttle[3] = Throttle_pct;
/* } */
/* 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;
Throttle_pct = Throttle[3];
}

340
Simulator/FDM/LaRCsim/c172_gear.c Normal file
View file

@ -0,0 +1,340 @@
/***************************************************************************
TITLE: gear
----------------------------------------------------------------------------
FUNCTION: Landing gear model for example simulation
----------------------------------------------------------------------------
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Renamed navion_gear.c originally created 931012 by E. B. Jackson
----------------------------------------------------------------------------
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
931218 Added navion.h header to allow connection with
aileron displacement for nosewheel steering. EBJ
940511 Connected nosewheel to rudder pedal; adjusted gain.
CURRENT RCS HEADER:
$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.
----------------------------------------------------------------------------
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
#include "ls_types.h"
#include "ls_constants.h"
#include "ls_generic.h"
#include "ls_cockpit.h"
void 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[] )
{
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[] )
{
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[] )
{
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[] )
{
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[] )
{
v[0] = 0.; v[1] = 0.; v[2] = 0.;
}
void gear( SCALAR dt, int Initialize ) {
char rcsid[] = "$Id$";
/*
* Aircraft specific initializations and data goes here
*/
#define NUM_WHEELS 3
static int num_wheels = NUM_WHEELS; /* number of wheels */
static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations */
{
{ 10., 0., 4. }, /* in feet */
{ -1., 3., 4. },
{ -1., -3., 4. }
};
static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
{ 1500., 5000., 5000. };
static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
{ 100., 150., 150. };
static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
{ 0., 0., 0. }; /* 0 = none, 1 = full */
static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
{ 0., 0., 0.}; /* radians, +CW */
/*
* End of aircraft specific code
*/
/*
* Constants & coefficients for tyres on tarmac - ref [1]
*/
/* skid function looks like:
*
* mu ^
* |
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* +--+------------------------>
* | | | sideward V
* 0 bkout skid
* V V
*/
static DATA sliding_mu = 0.5;
static DATA rolling_mu = 0.01;
static DATA max_brake_mu = 0.6;
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static DATA skid_v = 1.0;
/*
* Local data variables
*/
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_wheel_force, sideward_wheel_force;
int i; /* per wheel loop counter */
/*
* Execution starts here
*/
beta_mu = max_mu/(skid_v-bkout_v);
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
/*
* Put aircraft specific executable code here
*/
/* replace with cockpit brake handle connection code */
percent_brake[1] = Brake_pct;
percent_brake[2] = percent_brake[1];
caster_angle_rad[0] = 0.03*Rudder_pedal;
for (i=0;i<num_wheels;i++) /* Loop for each wheel */
{
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
/* then converting to local (North-East-Down) axes... */
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
/* Runway axes correction - third element is Altitude, not (-)Z... */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
/* Add wheel offset to cg location in local axes */
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
/* remove Runway axes correction so right hand rule applies */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/* contribution due to angular rates */
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
/* transform into local axes */
multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
/* plus contribution due to cg velocities */
add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
/* Add any anticipation, or frame lead/prediction, here... */
/* no lead used at present */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
/* Calculate normal load force (simple spring constant) */
reaction_normal_force = 0.;
if( d_wheel_rwy_local_v[2] < 0. )
{
reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*spring_damping[i];
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
}
/* Calculate friction coefficients */
forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu;
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
/* Calculate foreward and sideward reaction forces */
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* Rotate into local (N-E-D) axes */
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Calculate moments from force and offsets in body axes */
cross3( d_wheel_cg_body_v, tempF, tempM );
/* Sum forces and moments across all wheels */
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
}
}

76
Simulator/FDM/LaRCsim/c172_init.c Normal file
View file

@ -0,0 +1,76 @@
/***************************************************************************
TITLE: navion_init.c
----------------------------------------------------------------------------
FUNCTION: Initializes navion math model
----------------------------------------------------------------------------
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Renamed navion_init.c originally created on 930111 by Bruce Jackson
----------------------------------------------------------------------------
DESIGNED BY: EBJ
CODED BY: EBJ
MAINTAINED BY: EBJ
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
950314 Removed initialization of state variables, since this is
now done (version 1.4b1) in ls_init. EBJ
950406 Removed #include of "shmdefs.h"; shmdefs.h is a duplicate
of "navion.h". EBJ
CURRENT RCS HEADER:
----------------------------------------------------------------------------
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
--------------------------------------------------------------------------*/
#include "ls_types.h"
#include "ls_generic.h"
#include "ls_cockpit.h"
#include "ls_constants.h"
void model_init( void ) {
Throttle[3] = 0.2; Rudder_pedal = 0; Lat_control = 0; Long_control = 0;
Dx_pilot = 0; Dy_pilot = 0; Dz_pilot = 0;
Mass=2300*INVG;
I_xx=948;
I_yy=1346;
I_zz=1967;
I_xz=0;
}

413
Simulator/FDM/LaRCsim/c172_main.c Normal file
View file

@ -0,0 +1,413 @@
// LaRCsim.cxx -- interface to the LaRCsim flight model
//
// Written by Curtis Olson, started October 1998.
//
// Copyright (C) 1998 Curtis L. Olson - curt@me.umn.edu
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
#include <FDM/LaRCsim/ls_cockpit.h>
#include <FDM/LaRCsim/ls_generic.h>
#include <FDM/LaRCsim/ls_interface.h>
#include <FDM/LaRCsim/ls_constants.h>
// 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() {
double save_alt = 0.0;
int multiloop=1;
double time=0;
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;
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;
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); */
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 );
}
// 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;
} */