diff --git a/Simulator/FDM/LaRCsim/c172_aero.c b/Simulator/FDM/LaRCsim/c172_aero.c new file mode 100644 index 000000000..f2102f2b3 --- /dev/null +++ b/Simulator/FDM/LaRCsim/c172_aero.c @@ -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 +#include + + +#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; + +} + + diff --git a/Simulator/FDM/LaRCsim/c172_engine.c b/Simulator/FDM/LaRCsim/c172_engine.c new file mode 100644 index 000000000..14ff3054a --- /dev/null +++ b/Simulator/FDM/LaRCsim/c172_engine.c @@ -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 +#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]; +} + + diff --git a/Simulator/FDM/LaRCsim/c172_gear.c b/Simulator/FDM/LaRCsim/c172_gear.c new file mode 100644 index 000000000..667f6aba7 --- /dev/null +++ b/Simulator/FDM/LaRCsim/c172_gear.c @@ -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 +#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 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 ); + + } +} diff --git a/Simulator/FDM/LaRCsim/c172_init.c b/Simulator/FDM/LaRCsim/c172_init.c new file mode 100644 index 000000000..5bd23d23f --- /dev/null +++ b/Simulator/FDM/LaRCsim/c172_init.c @@ -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; + + +} diff --git a/Simulator/FDM/LaRCsim/c172_main.c b/Simulator/FDM/LaRCsim/c172_main.c new file mode 100644 index 000000000..d52865847 --- /dev/null +++ b/Simulator/FDM/LaRCsim/c172_main.c @@ -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 +#include +#include +#include + + +// 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; +} */ +