--- /dev/null
+/***************************************************************************
+
+ 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;
+
+}
+
+
--- /dev/null
+/***************************************************************************
+
+ 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];
+}
+
+
--- /dev/null
+/***************************************************************************
+
+ 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 );
+
+ }
+}
--- /dev/null
+/***************************************************************************
+
+ 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;
+
+
+}
--- /dev/null
+// 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;
+} */
+
projects / flightgear.git / commitdiff