From: curt Date: Thu, 29 May 1997 00:09:51 +0000 (+0000) Subject: Initial Flight Gear revision. X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=724e5d3d5ea7732a82c156e96f2f554198777a74;p=flightgear.git Initial Flight Gear revision. --- diff --git a/LaRCsim/LaRCsim.c b/LaRCsim/LaRCsim.c new file mode 100644 index 000000000..b3d094fda --- /dev/null +++ b/LaRCsim/LaRCsim.c @@ -0,0 +1,604 @@ +/*************************************************************************** + + TITLE: LaRCsim.c + +---------------------------------------------------------------------------- + + FUNCTION: Top level routine for LaRCSIM. Includes + global variable declarations. + +---------------------------------------------------------------------------- + + MODULE STATUS: Developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Written 921230 by Bruce Jackson + +---------------------------------------------------------------------------- + + DESIGNED BY: EBJ + + CODED BY: EBJ + + MAINTAINED BY: EBJ + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + 930111 Added "progname" variable to keep name of invoking command. + EBJ + 931012 Removed altitude < 0. test to support gear development. EBJ + 931214 Added various pressures (Impact, Dynamic, Static, etc.) EBJ + 931215 Adopted new generic variable structure. EBJ + 931218 Added command line options decoding. EBJ + 940110 Changed file type of matrix file to ".m" EBJ + 940513 Renamed this routine "LaRCsim.c" from "ls_main.c" EBJ + 940513 Added time_stamp routine, t_stamp. EBJ + 950225 Added options flag, 'i', to set I/O output rate. EBJ + 950306 Added calls to ls_get_settings() and ls_put_settings() EBJ + 950314 Options flag 'i' now reads IC file; 'o' is output rate EBJ + 950406 Many changes: added definition of default value macros; + removed local variables term_update_hz, model_dt, endtime, + substituted sim_control_ globals for these; removed + initialization of sim_control_.tape_channels; moved optarg + to generic extern; added mod_end_time & mod_buf_size flags + and temporary buffer_time and data_rate locals to + ls_checkopts(); added additional command line switches '-s' + and '-b'; made psuedo-mandatory file names for data output + switches; considerable rewrite of logic for setting data + buffer length and interleave parameters; updated '-h' help + output message; added protection logic to calculations of + these parameters; added check of return value on first call + to ls_cockpit() so abort works from initial pause + state; added call to ls_unsync() immediately following + first ls_sync() call, if paused (to avoid alarm clock + timeout); moved call to ls_record() into non-paused + multiloop path (was filling buffer with identical data + during pause); put check of paused flag before calling sync + routine ls_pause(); and added call to exit() on termination. + + +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:09:51 curt +Initial Flight Gear revision. + + * Revision 1.4.1.7 1995/04/07 01:04:37 bjax + * Many changes made to support storage of sim options from run to run, + * as well as restructuring storage buffer sizing and some loop logic + * changes. See the modification log for details. + * + * Revision 1.4.1.6 1995/03/29 16:12:09 bjax + * Added argument to -o switch; changed run loop to pass dt=0 + * if in paused mode. EBj + * + * Revision 1.4.1.5 1995/03/15 12:30:20 bjax + * Set paused flag to non-zero by default; moved 'i' I/O rate flag + * switch to 'o'; made 'i' an initial conditions file switch; added + * null string to ls_get_settings() call so that default settings + * file will be read. EBJ + * + * Revision 1.4.1.4 1995/03/08 12:31:34 bjax + * Added userid retrieval and proper termination of time & date strings. + * + * Revision 1.4.1.3 1995/03/08 12:00:21 bjax + * Moved setting of default options to ls_setdefopts from + * ls_checkopts; rearranged order of ls_get_settings() call + * to between ls_setdefopts and ls_checkopts, so command + * line options will override settings file options. + * EBJ + * + * Revision 1.4.1.2 1995/03/06 18:48:49 bjax + * Added calles to ls_get_settings() and ls_put_settings(); added + * passing of dt and init flags in ls_model(). EBJ + * + * Revision 1.4.1.1 1995/03/03 02:23:08 bjax + * Beta version for LaRCsim, version 1.4 + * + * Revision 1.3.2.7 1995/02/27 20:00:21 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.2.6 1995/02/25 16:52:31 bjax + * Added 'i' option to set I/O iteration rate. EBJ + * + * Revision 1.3.2.5 1995/02/06 19:33:15 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.2.4 1995/02/06 19:30:30 bjax + * Oops, should really compile these before checking in. Fixed capitailzation of + * Initialize in ls_loop parameter. + * + * Revision 1.3.2.3 1995/02/06 19:25:44 bjax + * Moved main simulation loop into subroutine ls_loop. EBJ + * + * Revision 1.3.2.2 1994/05/20 21:46:45 bjax + * A little better logic on checking for option arguments. + * + * Revision 1.3.2.1 1994/05/20 19:29:51 bjax + * Added options arguments to command line. + * + * Revision 1.3.1.16 1994/05/17 15:08:45 bjax + * Corrected so that full name to directyr and file is saved + * in new global variable "fullname"; this allows symbol table + * to be extracted when in another default directory. + * + * Revision 1.3.1.15 1994/05/17 14:50:24 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.14 1994/05/17 14:50:23 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.13 1994/05/17 14:50:21 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.12 1994/05/17 14:50:20 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.11 1994/05/17 13:56:24 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.10 1994/05/17 13:23:03 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.9 1994/05/17 13:20:03 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.8 1994/05/17 13:19:23 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.7 1994/05/17 13:18:29 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.6 1994/05/17 13:16:30 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.5 1994/05/17 13:03:44 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.4 1994/05/17 13:03:38 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.3 1994/05/17 12:49:08 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.2 1994/05/17 12:48:45 bjax + * *** empty log message *** + * + * Revision 1.3.1.1 1994/05/13 20:39:17 bjax + * Top of 1.3 branch. + * + * Revision 1.2 1994/05/13 19:51:50 bjax + * Skip rev + * + +---------------------------------------------------------------------------- + + REFERENCES: + +---------------------------------------------------------------------------- + + CALLED BY: + +---------------------------------------------------------------------------- + + CALLS TO: + +---------------------------------------------------------------------------- + + INPUTS: + +---------------------------------------------------------------------------- + + OUTPUTS: + +--------------------------------------------------------------------------*/ + +#include "ls_types.h" +#include "ls_constants.h" +#include "ls_generic.h" +#include "ls_sim_control.h" +#include "ls_cockpit.h" +/* #include "ls_tape.h" */ +#ifndef linux +# include +#endif +#include +#include +#include +#include +#include +#include + +/* global variable declarations */ + +/* TAPE *Tape; */ +GENERIC generic_; +SIM_CONTROL sim_control_; +COCKPIT cockpit_; + +SCALAR Simtime; + +/* #define DEFAULT_TERM_UPDATE_HZ 20 */ /* original value */ +#define DEFAULT_TERM_UPDATE_HZ 20 +#define DEFAULT_MODEL_HZ 120 +#define DEFAULT_END_TIME 3600. +#define DEFAULT_SAVE_SPACING 8 +#define DEFAULT_WRITE_SPACING 1 +#define MAX_FILE_NAME_LENGTH 80 + +/* global variables */ + +char *progname; +char *fullname; + +/* file variables - default simulation settings */ + +static float io_dt; +static float speedup; +static char asc1name[MAX_FILE_NAME_LENGTH] = "run.asc1"; +static char tabname[MAX_FILE_NAME_LENGTH] = "run.dat"; +static char fltname[MAX_FILE_NAME_LENGTH] = "run.flt"; +static char matname[MAX_FILE_NAME_LENGTH] = "run.m"; + + + +void ls_stamp() +{ + char rcsid[] = "$Id$"; + char revid[] = "$Revision$"; + char dateid[] = "$Date$"; + struct tm *nowtime; + time_t nowtime_t; + long date; + + printf("\nLaRCsim %s, %s\n\n", revid, dateid); /* report version of LaRCsim */ + + nowtime_t = time( 0 ); + nowtime = localtime( &nowtime_t ); /* set fields to correct time values */ + date = (nowtime->tm_year)*10000 + + (nowtime->tm_mon + 1)*100 + + (nowtime->tm_mday); + sprintf(sim_control_.date_string, "%06d\0", date); + sprintf(sim_control_.time_stamp, "%02d:%02d:%02d\0", + nowtime->tm_hour, nowtime->tm_min, nowtime->tm_sec); + cuserid( sim_control_.userid ); /* set up user id */ + + return; +} + +void ls_setdefopts() +{ + /* set default values for most options */ + + sim_control_.debug = 0; /* change to non-zero if in dbx! */ + sim_control_.vision = 0; + sim_control_.write_av = 0; /* write Agile-Vu '.flt' file */ + sim_control_.write_mat = 0; /* write matrix-x/matlab script */ + sim_control_.write_tab = 0; /* write tab delim. history file */ + sim_control_.write_asc1 = 0; /* write GetData file */ + sim_control_.sim_type = GLmouse; /* hook up to mouse */ + sim_control_.save_spacing = DEFAULT_SAVE_SPACING; + /* interpolation on recording */ + sim_control_.write_spacing = DEFAULT_WRITE_SPACING; + /* interpolation on output */ + sim_control_.end_time = DEFAULT_END_TIME; + sim_control_.model_hz = DEFAULT_MODEL_HZ; + sim_control_.term_update_hz = DEFAULT_TERM_UPDATE_HZ; + sim_control_.time_slices = DEFAULT_END_TIME*DEFAULT_MODEL_HZ/DEFAULT_SAVE_SPACING; + sim_control_.paused = 1; + + speedup = 1.0; +} + + +/* return result codes from ls_checkopts */ + +#define OPT_OK 0 +#define OPT_ERR 1 + +extern char *optarg; +extern int optind; + +int ls_checkopts(argc, argv) /* check and set options flags */ + int argc; + char *argv[]; + { + int c; + int opt_err = 0; + int mod_end_time = 0; + int mod_buf_size = 0; + float buffer_time, data_rate; + + /* set default values */ + + buffer_time = sim_control_.time_slices * sim_control_.save_spacing / sim_control_.model_hz; + data_rate = sim_control_.model_hz / sim_control_.save_spacing; + + while ((c = getopt(argc, argv, "Aa:b:de:f:hi:kmo:r:s:t:x:")) != EOF) + switch (c) { + case 'A': + if (sim_control_.sim_type == GLmouse) + { + fprintf(stderr, "Cannot specify both keyboard (k) and ACES (A) cockpits option\n"); + fprintf(stderr, "Keyboard operation assumed.\n"); + break; + } + sim_control_.sim_type = cockpit; + break; + case 'a': + sim_control_.write_av = 1; + if (optarg != NULL) + if (*optarg != '-') + strncpy(fltname, optarg, MAX_FILE_NAME_LENGTH); + else + optind--; + break; + case 'b': + buffer_time = atof(optarg); + if (buffer_time <= 0.) opt_err = -1; + mod_buf_size++; + break; + case 'd': + sim_control_.debug = 1; + break; + case 'e': + sim_control_.end_time = atof(optarg); + mod_end_time++; + break; + case 'f': + sim_control_.model_hz = atof(optarg); + break; + case 'h': + opt_err = 1; + break; + case 'i': + /* ls_get_settings( optarg ); */ + break; + case 'k': + sim_control_.sim_type = GLmouse; + break; + case 'm': + sim_control_.vision = 1; + break; + case 'o': + sim_control_.term_update_hz = atof(optarg); + if (sim_control_.term_update_hz <= 0.) opt_err = 1; + break; + case 'r': + sim_control_.write_mat = 1; + if (optarg != NULL) + if (*optarg != '-') + strncpy(matname, optarg, MAX_FILE_NAME_LENGTH); + else + optind--; + break; + case 's': + data_rate = atof(optarg); + if (data_rate <= 0.) opt_err = -1; + break; + case 't': + sim_control_.write_tab = 1; + if (optarg != NULL) + if (*optarg != '-') + strncpy(tabname, optarg, MAX_FILE_NAME_LENGTH); + else + optind--; + break; + case 'x': + sim_control_.write_asc1 = 1; + if (optarg != NULL) + if (*optarg != '-') + strncpy(asc1name, optarg, MAX_FILE_NAME_LENGTH); + else + optind--; + break; + default: + opt_err = 1; + + } + + if (opt_err) + { + fprintf(stderr, "Usage: %s [-options]\n", progname); + fprintf(stderr, "\n"); + fprintf(stderr, " where [-options] is zero or more of the following:\n"); + fprintf(stderr, "\n"); + fprintf(stderr, " [A|k] Run mode: [A]CES cockpit [default]\n"); + fprintf(stderr, " or [k]eyboard\n"); + fprintf(stderr, "\n"); + fprintf(stderr, " [i ] [i]nitial conditions filename\n"); + fprintf(stderr, "\n"); + fprintf(stderr, " [f ] Iteration rate [f]requency, Hz (default is %5.2f Hz)\n", + sim_control_.model_hz); + fprintf(stderr, "\n"); + fprintf(stderr, " [o ] Display [o]utput frequency, Hz (default is %5.2f Hz)\n", + sim_control_.term_update_hz); + fprintf(stderr, "\n"); + fprintf(stderr, " [s ] Data storage frequency, Hz (default is %5.2f Hz)\n", + data_rate); + fprintf(stderr, "\n"); + fprintf(stderr, " [e ] [e]nd time in seconds (default %5.1f seconds)\n", + sim_control_.end_time); + fprintf(stderr, "\n"); + fprintf(stderr, " [b ] circular time history storage [b]uffer size, in seconds \n"); + fprintf(stderr, " (default %5.1f seconds) (normally same as end time)\n", + sim_control_.time_slices*sim_control_.save_spacing/ + sim_control_.model_hz); + fprintf(stderr, "\n"); + fprintf(stderr, " [atxr []] Output: [a]gile-vu (default name: %s )\n", fltname); + fprintf(stderr, " and/or [t]ab delimited ( '' name: %s )\n", tabname); + fprintf(stderr, " and/or [x]plot (default name: %s)\n", asc1name); + fprintf(stderr, " and/or mat[r]ix script ( '' name: %s )\n", matname); + fprintf(stderr, "\n"); + return OPT_ERR; + } + +/* calculate additional controls */ + + io_dt = 1.0/sim_control_.term_update_hz; + + sim_control_.save_spacing = (int) (0.5 + sim_control_.model_hz / data_rate); + if (sim_control_.save_spacing < 1) sim_control_.save_spacing = 1; + + sim_control_.time_slices = buffer_time * sim_control_.model_hz / sim_control_.save_spacing; + if (sim_control_.time_slices < 2) sim_control_.time_slices = 2; + + return OPT_OK; + } + + +void ls_loop( dt, initialize ) + +SCALAR dt; +int initialize; + +{ + /* printf (" In ls_loop()\n"); */ + ls_step( dt, initialize ); + /* if (sim_control_.sim_type == cockpit ) ls_ACES(); */ + ls_aux(); + ls_model( dt, initialize ); + ls_accel(); +} + + + +/* Stub ls_cockpit routines for testing */ +#define FALSE 0 +#define TRUE 1 + +int ls_cockpit() { + int abort = FALSE; + + sim_control_.paused = 0; + + Throttle_pct = 0.85; + + /* printf("Mach = %.2f ", Mach_number); + printf("%.4f,%.4f,%.2f ", Latitude, Longitude, Altitude); + printf("%.2f,%.2f,%.2f\n", Phi, Theta, Psi); */ + + return(abort); +} + +int ls_cockpit_exit() { + exit(0); +} + + + + +main(argc, argv) +int argc; +char *argv[]; +{ + int i, multiloop, abrt; + SCALAR model_dt; + + fullname = argv[0]; /* point to full directory & path name of our program */ + progname = basename(argv[0]); /* point to name of program that invoked us */ + strcpy(sim_control_.simname, progname); /* really should check for overflow*/ + + ls_setdefopts(); /* set default options */ + + /* Number_of_Continuous_States = 22; */ + + generic_.geodetic_position_v[0] = 2.793445E-05; + generic_.geodetic_position_v[1] = 3.262070E-07; + generic_.geodetic_position_v[2] = 3.758099E+00; + generic_.v_local_v[0] = 7.287719E+00; + generic_.v_local_v[1] = 1.521770E+03; + generic_.v_local_v[2] = -1.265722E-05; + generic_.euler_angles_v[0] = -2.658474E-06; + generic_.euler_angles_v[1] = 7.401790E-03; + generic_.euler_angles_v[2] = 1.391358E-03; + generic_.omega_body_v[0] = 7.206685E-05; + generic_.omega_body_v[1] = 0.000000E+00; + generic_.omega_body_v[2] = 9.492658E-05; + generic_.earth_position_angle = 0.000000E+00; + generic_.mass = 8.547270E+01; + generic_.i_xx = 1.048000E+03; + generic_.i_yy = 3.000000E+03; + generic_.i_zz = 3.530000E+03; + generic_.i_xz = 0.000000E+00; + generic_.d_cg_rp_body_v[0] = 0.000000E+00; + generic_.d_cg_rp_body_v[1] = 0.000000E+00; + generic_.d_cg_rp_body_v[2] = 0.000000E+00; + + /* ls_get_settings( "" ); */ /* let settings file override them */ + + if (ls_checkopts(argc, argv) == OPT_ERR) return 1; /* and then command line opts */ + + ls_stamp(); /* ID stamp; record time and date of run */ + + if (speedup == 0.0) { + fprintf(stderr, "%s: Cannot run with speedup of 0.\n", progname); + return 1; + } + + model_dt = 1./sim_control_.model_hz; + + if (io_dt < model_dt) { + fprintf(stderr, "%s: Can't run I/O faster than model.\n", progname); + return 1; + } + + multiloop = (int) (io_dt/model_dt/fabs(speedup)); + model_dt = io_dt/multiloop; + + ls_init(); + + /* ls_record(); */ + if (speedup > 0) { + abrt = ls_cockpit(); + if (abrt) { + ls_cockpit_exit(); + exit( EXIT_SUCCESS ); + } + ls_sync(io_dt); + if (sim_control_.paused) ls_unsync(); + } + + do { + for(i=0;i 0) { + abrt = ls_cockpit(); + if (!sim_control_.paused) ls_pause(); + if (abrt) Simtime = sim_control_.end_time+model_dt; + } + } while (Simtime < sim_control_.end_time); + + if (speedup > 0) { + ls_unsync(); + ls_cockpit_exit(); + } + + /* if (sim_control_.write_mat ) ls_writemat( matname ); */ + /* if (sim_control_.write_av ) ls_writeav( fltname ); */ + /* if (sim_control_.write_tab ) ls_writetab( tabname ); */ + /* if (sim_control_.write_asc1) ls_writeasc1( asc1name ); */ + + /* ls_put_settings(); */ + + exit( EXIT_SUCCESS ); +} + +/* +Mon Feb 6 14:33:15 EST 1995 +bjax +*/ +/* +Mon Feb 27 15:00:20 EST 1995 +bjax +*/ diff --git a/LaRCsim/Makefile b/LaRCsim/Makefile new file mode 100644 index 000000000..5a7e137e7 --- /dev/null +++ b/LaRCsim/Makefile @@ -0,0 +1,64 @@ +#--------------------------------------------------------------------------- +# Makefile +# +# Written by Curtis Olson, started May 1997. +# +# $Id$ +# (Log is kept at end of this file) +#--------------------------------------------------------------------------- + + +TARGET=libLaRCsim.a + +LaRCsimFILES = atmos_62.c ls_accel.c ls_aux.c ls_geodesy.c ls_gravity.c \ + ls_step.c ls_model.c default_model_routines.c ls_init.c ls_sync.c \ + +NavionFILES = navion_aero.c navion_engine.c navion_gear.c navion_init.c + +InterfaceFILES = ls_interface.c + +CFILES = $(LaRCsimFILES) $(NavionFILES) $(InterfaceFILES) + +OFILES = $(CFILES:.c=.o) + +CC = gcc +CFLAGS = -g +# CFLAGS = -O2 + +AR = ar + +INCLUDES = + +LIBS = -lm + + +#--------------------------------------------------------------------------- +# Primary Targets +#--------------------------------------------------------------------------- + +$(TARGET): $(OFILES) + $(AR) rv $(TARGET) $(OFILES) + +simtest: $(TARGET) LaRCsim.o + $(CC) -o simtest LaRCsim.o libLaRCsim.a $(LIBS) + +all: $(TARGET) + +clean: + rm -f *.o $(TARGET) *~ core + + +#--------------------------------------------------------------------------- +# Secondary Targets +#--------------------------------------------------------------------------- + + + +#--------------------------------------------------------------------------- +# $Log$ +# Revision 1.1 1997/05/29 00:09:52 curt +# Initial Flight Gear revision. +# +# Revision 1.1 1997/05/16 16:04:44 curt +# Initial revision. +# diff --git a/LaRCsim/atmos_62.c b/LaRCsim/atmos_62.c new file mode 100644 index 000000000..f0ec1ab15 --- /dev/null +++ b/LaRCsim/atmos_62.c @@ -0,0 +1,252 @@ +/*************************************************************************** + + TITLE: atmos_62 + +---------------------------------------------------------------------------- + + FUNCTION: 1962 atmosphere table interpolation routine + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Created 920827 by Bruce Jackson as part of the C-castle + development effort. + +---------------------------------------------------------------------------- + + DESIGNED BY: B. Jackson + + CODED BY: B. Jackson + + MAINTAINED BY: B. Jackson + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + 931220 Added ambient pressure and temperature as outputs. EBJ + 940111 Changed includes from "ls_eom.h" to "ls_types.h" and + "ls_constants.h"; changed DATA to SCALAR types. EBJ + +---------------------------------------------------------------------------- + + REFERENCES: + + [ 1] Hornbeck, Robert W.: "Numerical Methods", Prentice-Hall, + 1975. ISBN 0-13-626614-2 + +---------------------------------------------------------------------------- + + CALLED BY: + +---------------------------------------------------------------------------- + + CALLS TO: + +---------------------------------------------------------------------------- + + INPUTS: + +---------------------------------------------------------------------------- + + OUTPUTS: + +--------------------------------------------------------------------------*/ + +#include "ls_types.h" +#include "ls_constants.h" +#include + +#define alt_0 d_a_table[index ][0] +#define alt_1 d_a_table[index+1][0] +#define den_0 d_a_table[index ][1] +#define den_1 d_a_table[index+1][1] +#define sps_0 d_a_table[index ][2] +#define sps_1 d_a_table[index+1][2] +#define gden_0 d_a_table[index ][3] +#define gden_1 d_a_table[index+1][3] +#define gsps_0 d_a_table[index ][4] +#define gsps_1 d_a_table[index+1][4] + +#define MAX_ALT_INDEX 121 +#define DELT_ALT 2000. +#define HLEV 36089. +#define TAMB0 518.7 +#define PAMB0 2113.8 +#define MAX_ALTITUDE 240000. + +void ls_atmos( SCALAR altitude, SCALAR * sigma, SCALAR * v_sound, + SCALAR * t_amb, SCALAR * p_amb ) +{ + + int index; + SCALAR daltp, daltn, daltp3, daltn3, density; + SCALAR t_amb_r, p_amb_r; + static SCALAR d_a_table[MAX_ALT_INDEX][5] = + { + { 0., 2.37701E-03, 1.11642E+03, 0.00000E+00, 0.00000E+00 }, + { 2000., 2.24098E-03, 1.10872E+03, 1.92857E-12, -1.75815E-08 }, + { 4000., 2.11099E-03, 1.10097E+03, 1.34570E-12, -1.21740E-08 }, + { 6000., 1.98684E-03, 1.09315E+03, 1.44862E-12, -1.47225E-08 }, + { 8000., 1.86836E-03, 1.08529E+03, 1.36481E-12, -1.44359E-08 }, + { 10000., 1.75537E-03, 1.07736E+03, 1.32716E-12, -1.45340E-08 }, + { 12000., 1.64768E-03, 1.06938E+03, 1.27657E-12, -1.44280E-08 }, + { 14000., 1.54511E-03, 1.06133E+03, 1.24656E-12, -1.62540E-08 }, + { 16000., 1.44751E-03, 1.05323E+03, 1.19220E-12, -1.50560E-08 }, + { 18000., 1.35469E-03, 1.04506E+03, 1.15463E-12, -1.65220E-08 }, + { 20000., 1.26649E-03, 1.03683E+03, 1.11926E-12, -1.63562E-08 }, + { 22000., 1.18276E-03, 1.02853E+03, 1.07333E-12, -1.70533E-08 }, + { 24000., 1.10333E-03, 1.02016E+03, 1.03743E-12, -1.59305E-08 }, + { 26000., 1.02805E-03, 1.01173E+03, 1.00195E-12, -2.27248E-08 }, + { 28000., 9.56760E-04, 1.00322E+03, 9.39764E-13, 3.29851E-10 }, + { 30000., 8.89320E-04, 9.94641E+02, 1.01399E-12, -8.80946E-08 }, + { 32000., 8.25570E-04, 9.85988E+02, 5.39268E-13, 2.41048E-07 }, + { 34000., 7.65380E-04, 9.77258E+02, 2.16894E-12, -9.91599E-07 }, + { 36000., 7.08600E-04, 9.68448E+02, -4.10001E-12, 3.60535E-06 }, + { 38000., 6.44190E-04, 9.68053E+02, 2.78612E-12, -8.07290E-07 }, + { 40000., 5.85146E-04, 9.68053E+02, 1.00455E-12, 2.16313E-07 }, + { 42000., 5.31517E-04, 9.68053E+02, 1.31819E-12, -5.79609E-08 }, + { 44000., 4.82801E-04, 9.68053E+02, 1.09217E-12, 1.55309E-08 }, + { 46000., 4.38554E-04, 9.68053E+02, 1.01661E-12, -4.16262E-09 }, + { 48000., 3.98359E-04, 9.68053E+02, 9.19375E-13, 1.11961E-09 }, + { 50000., 3.61850E-04, 9.68053E+02, 8.34886E-13, -3.15801E-10 }, + { 52000., 3.28686E-04, 9.68053E+02, 7.58579E-13, 1.43600E-10 }, + { 54000., 2.98561E-04, 9.68053E+02, 6.89297E-13, -2.58597E-10 }, + { 56000., 2.71197E-04, 9.68053E+02, 6.25735E-13, 8.90788E-10 }, + { 58000., 2.46341E-04, 9.68053E+02, 5.69765E-13, -3.30456E-09 }, + { 60000., 2.23765E-04, 9.68053E+02, 5.15206E-13, 1.23274E-08 }, + { 62000., 2.03256E-04, 9.68053E+02, 4.69912E-13, -4.60052E-08 }, + { 64000., 1.84627E-04, 9.68053E+02, 4.25146E-13, 1.71693E-07 }, + { 66000., 1.67616E-04, 9.68314E+02, 2.56502E-13, -2.49268E-07 }, + { 68000., 1.51855E-04, 9.68676E+02, 4.23844E-13, 9.76878E-07 }, + { 70000., 1.37615E-04, 9.71034E+02, 3.29621E-13, -6.64245E-07 }, + { 72000., 1.24744E-04, 9.72390E+02, 3.11170E-13, 1.77102E-07 }, + { 74000., 1.13107E-04, 9.73745E+02, 2.76697E-13, -4.56627E-08 }, + { 76000., 1.02584E-04, 9.75099E+02, 2.53043E-13, 4.04902E-09 }, + { 78000., 9.30660E-05, 9.76450E+02, 2.18633E-13, 2.49667E-08 }, + { 80000., 8.44530E-05, 9.77799E+02, 2.29927E-13, -1.06916E-07 }, + { 82000., 7.67140E-05, 9.78950E+02, 1.72660E-13, 1.05696E-07 }, + { 84000., 6.97010E-05, 9.80290E+02, 1.68432E-13, -3.23682E-08 }, + { 86000., 6.33490E-05, 9.81620E+02, 1.45113E-13, 8.77690E-09 }, + { 88000., 5.75880E-05, 9.82950E+02, 1.37617E-13, -2.73938E-09 }, + { 90000., 5.23700E-05, 9.84280E+02, 1.18918E-13, 2.18061E-09 }, + { 92000., 4.76350E-05, 9.85610E+02, 1.11210E-13, -5.98306E-09 }, + { 94000., 4.33410E-05, 9.86930E+02, 9.77408E-14, 6.75162E-09 }, + { 96000., 3.94430E-05, 9.88260E+02, 9.18264E-14, -6.02343E-09 }, + { 98000., 3.59080E-05, 9.89580E+02, 7.94534E-14, 2.34210E-09 }, + { 100000., 3.26960E-05, 9.90900E+02, 7.48600E-14, -3.34498E-09 }, + { 102000., 2.97810E-05, 9.92210E+02, 6.66067E-14, -3.96219E-09 }, + { 104000., 2.71320E-05, 9.93530E+02, 5.77131E-14, 3.41937E-08 }, + { 106000., 2.46980E-05, 9.95410E+02, 2.50410E-14, 7.07187E-07 }, + { 108000., 2.24140E-05, 9.99070E+02, 6.71229E-14, -1.92943E-07 }, + { 110000., 2.03570E-05, 1.00272E+03, 4.69675E-14, 4.95832E-08 }, + { 112000., 1.85010E-05, 1.00636E+03, 4.65069E-14, -2.03903E-08 }, + { 114000., 1.68270E-05, 1.00998E+03, 4.00047E-14, 1.97789E-09 }, + { 116000., 1.53150E-05, 1.01359E+03, 3.64744E-14, -2.52130E-09 }, + { 118000., 1.39480E-05, 1.01719E+03, 3.15976E-14, -6.89271E-09 }, + { 120000., 1.27100E-05, 1.02077E+03, 3.06351E-14, 9.21465E-11 }, + { 122000., 1.15920E-05, 1.02434E+03, 2.58618E-14, -8.47587E-09 }, + { 124000., 1.05790E-05, 1.02789E+03, 2.34176E-14, 3.81135E-09 }, + { 126000., 9.66010E-06, 1.03144E+03, 2.16178E-14, -6.76951E-09 }, + { 128000., 8.82710E-06, 1.03497E+03, 1.89611E-14, -6.73330E-09 }, + { 130000., 8.07070E-06, 1.03848E+03, 1.74377E-14, 3.70270E-09 }, + { 132000., 7.38380E-06, 1.04199E+03, 1.55382E-14, -8.07752E-09 }, + { 134000., 6.75940E-06, 1.04548E+03, 1.41595E-14, -1.39263E-09 }, + { 136000., 6.19160E-06, 1.04896E+03, 1.27239E-14, -1.35196E-09 }, + { 138000., 5.67490E-06, 1.05243E+03, 1.15951E-14, -8.19953E-09 }, + { 140000., 5.20450E-06, 1.05588E+03, 1.03459E-14, 4.15010E-09 }, + { 142000., 4.77570E-06, 1.05933E+03, 9.42149E-15, -8.40086E-09 }, + { 144000., 4.38470E-06, 1.06276E+03, 8.66820E-15, -5.46671E-10 }, + { 146000., 4.02820E-06, 1.06618E+03, 7.65573E-15, -4.41246E-09 }, + { 148000., 3.70260E-06, 1.06959E+03, 7.05890E-15, 3.19650E-09 }, + { 150000., 3.40520E-06, 1.07299E+03, 6.40867E-15, -2.33736E-08 }, + { 152000., 3.13330E-06, 1.07637E+03, 5.55641E-15, 6.02977E-08 }, + { 154000., 2.88480E-06, 1.07975E+03, 6.46568E-15, -2.17817E-07 }, + { 156000., 2.66270E-06, 1.08202E+03, 8.18087E-15, -8.54029E-07 }, + { 158000., 2.46830E-06, 1.08202E+03, 2.36086E-15, 2.28931E-07 }, + { 160000., 2.28810E-06, 1.08202E+03, 3.67571E-15, -6.16972E-08 }, + { 162000., 2.12120E-06, 1.08202E+03, 2.88632E-15, 1.78573E-08 }, + { 164000., 1.96640E-06, 1.08202E+03, 2.92903E-15, -9.73206E-09 }, + { 166000., 1.82300E-06, 1.08202E+03, 2.49757E-15, 2.10709E-08 }, + { 168000., 1.69000E-06, 1.08202E+03, 2.68069E-15, -7.45517E-08 }, + { 170000., 1.56680E-06, 1.08202E+03, 1.47966E-15, 2.77136E-07 }, + { 172000., 1.45250E-06, 1.08202E+03, 4.75068E-15, -1.03399E-06 }, + { 174000., 1.35240E-06, 1.07963E+03, 8.17622E-16, 2.73830E-07 }, + { 176000., 1.25880E-06, 1.07723E+03, 1.72883E-15, -7.63301E-08 }, + { 178000., 1.17130E-06, 1.07482E+03, 1.41704E-15, 1.64901E-08 }, + { 180000., 1.08960E-06, 1.07240E+03, 1.30299E-15, -4.63038E-09 }, + { 182000., 1.01320E-06, 1.06998E+03, 1.32100E-15, 2.03140E-09 }, + { 184000., 9.41950E-07, 1.06756E+03, 1.13799E-15, -3.49522E-09 }, + { 186000., 8.75370E-07, 1.06513E+03, 1.13202E-15, -3.05052E-09 }, + { 188000., 8.13260E-07, 1.06269E+03, 1.03892E-15, 6.97283E-10 }, + { 190000., 7.55320E-07, 1.06025E+03, 9.67290E-16, 2.61383E-10 }, + { 192000., 7.01260E-07, 1.05781E+03, 9.11920E-16, -1.74281E-09 }, + { 194000., 6.50850E-07, 1.05536E+03, 8.60032E-16, -8.29013E-09 }, + { 196000., 6.03870E-07, 1.05290E+03, 7.92951E-16, 1.99033E-08 }, + { 198000., 5.60110E-07, 1.05044E+03, 7.98164E-16, -7.13232E-08 }, + { 200000., 5.19320E-07, 1.04798E+03, 4.69394E-16, 2.65389E-07 }, + { 202000., 4.81340E-07, 1.04550E+03, 1.53926E-15, -1.02023E-06 }, + { 204000., 4.47960E-07, 1.04063E+03, 2.73571E-16, 2.30547E-07 }, + { 206000., 4.16690E-07, 1.03565E+03, 5.31456E-16, -6.69551E-08 }, + { 208000., 3.87320E-07, 1.03065E+03, 4.50605E-16, 7.27308E-09 }, + { 210000., 3.59790E-07, 1.02562E+03, 4.26126E-16, -7.13720E-09 }, + { 212000., 3.33970E-07, 1.02057E+03, 4.09893E-16, -8.72426E-09 }, + { 214000., 3.09780E-07, 1.01549E+03, 3.79301E-16, -2.96576E-09 }, + { 216000., 2.87120E-07, 1.01039E+03, 3.67902E-16, -9.41272E-09 }, + { 218000., 2.65920E-07, 1.00526E+03, 3.39092E-16, -4.38337E-09 }, + { 220000., 2.46090E-07, 1.00011E+03, 3.30732E-16, -3.05378E-09 }, + { 222000., 2.27570E-07, 9.94940E+02, 3.02981E-16, -1.34015E-08 }, + { 224000., 2.10270E-07, 9.89730E+02, 2.87343E-16, -3.34027E-09 }, + { 226000., 1.94120E-07, 9.84500E+02, 2.72646E-16, -3.23743E-09 }, + { 228000., 1.79060E-07, 9.79250E+02, 2.57074E-16, -1.37100E-08 }, + { 230000., 1.65030E-07, 9.73960E+02, 2.44060E-16, -1.92258E-09 }, + { 232000., 1.51970E-07, 9.68650E+02, 2.21687E-16, -8.59969E-09 }, + { 234000., 1.39810E-07, 9.63310E+02, 2.19191E-16, -8.67865E-09 }, + { 236000., 1.28510E-07, 9.57940E+02, 1.91549E-16, -1.68569E-09 }, + { 238000., 1.18020E-07, 9.52550E+02, 2.29613E-16, -1.45786E-08 }, + { 240000., 1.08270E-07, 9.47120E+02, 0.00000E+00, 0.00000E+00 } + }; + + index = altitude / 2000; + if (index > (MAX_ALT_INDEX-2)) + { + index = MAX_ALT_INDEX-2; /* limit maximum altitude */ + altitude = MAX_ALTITUDE; + } + if (index < 0) index = 0; + daltp = alt_1 - altitude; + daltp3 = daltp*daltp*daltp; + daltn = altitude - alt_0; + daltn3 = daltn*daltn*daltn; + + density = (gden_0/6)*((daltp3/2000) - 2000*daltp) + + (gden_1/6)*((daltn3/2000) - 2000*daltn) + + den_0*daltp/2000 + den_1*daltn/2000; + + *v_sound = (gsps_0/6)*((daltp3/2000) - 2000*daltp) + + (gsps_1/6)*((daltn3/2000) - 2000*daltn) + + sps_0*daltp/2000 + sps_1*daltn/2000; + + *sigma = density/SEA_LEVEL_DENSITY; + + if (altitude < HLEV) /* BUG - these curve fits are only good to about 75000 ft */ + { + t_amb_r = 1. - 6.875e-6*altitude; + p_amb_r = pow( t_amb_r, 5.256 ); + } + else + { + t_amb_r = 0.751895; + p_amb_r = 0.2234*exp( -4.806e-5 * (altitude - HLEV)); + } + + *p_amb = p_amb_r * PAMB0; + *t_amb = t_amb_r * TAMB0; + +/* end of atmos_62 */ +} +/**************************************************************************/ diff --git a/LaRCsim/default_model_routines.c b/LaRCsim/default_model_routines.c new file mode 100644 index 000000000..65e72af77 --- /dev/null +++ b/LaRCsim/default_model_routines.c @@ -0,0 +1,81 @@ +/*************************************************************************** + + TITLE: engine.c + +---------------------------------------------------------------------------- + + FUNCTION: dummy engine routine + +---------------------------------------------------------------------------- + + MODULE STATUS: incomplete + +---------------------------------------------------------------------------- + + GENEALOGY: Created 15 OCT 92 as dummy routine for checkout. EBJ + +---------------------------------------------------------------------------- + + DESIGNED BY: designer + + CODED BY: programmer + + MAINTAINED BY: maintainer + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + CURRENT RCS HEADER INFO: + +$Header$ + +$Log$ +Revision 1.1 1997/05/29 00:09:53 curt +Initial Flight Gear revision. + + * Revision 1.3 1994/01/11 19:10:45 bjax + * Removed include files. + * + * Revision 1.2 1993/03/14 12:16:10 bjax + * simple correction: added ';' to end of default routines. EBJ + * + * Revision 1.1 93/03/10 06:43:46 bjax + * Initial revision + * + * Revision 1.1 92/12/30 13:21:46 bjax + * Initial revision + * + +---------------------------------------------------------------------------- + + REFERENCES: + +---------------------------------------------------------------------------- + + CALLED BY: ls_model(); + +---------------------------------------------------------------------------- + + CALLS TO: none + +---------------------------------------------------------------------------- + + INPUTS: + +---------------------------------------------------------------------------- + + OUTPUTS: + +--------------------------------------------------------------------------*/ + +void inertias() {} +void subsystems() {} +/* void engine() {} */ +/* void gear() {} */ + + + + diff --git a/LaRCsim/ls_accel.c b/LaRCsim/ls_accel.c new file mode 100644 index 000000000..172d2dafb --- /dev/null +++ b/LaRCsim/ls_accel.c @@ -0,0 +1,202 @@ +/*************************************************************************** + + TITLE: ls_Accel + + ---------------------------------------------------------------------------- + + FUNCTION: Sums forces and moments and calculates accelerations + + ---------------------------------------------------------------------------- + + MODULE STATUS: developmental + + ---------------------------------------------------------------------------- + + GENEALOGY: Written 920731 by Bruce Jackson. Based upon equations + given in reference [1] and a Matrix-X/System Build block + diagram model of equations of motion coded by David Raney + at NASA-Langley in June of 1992. + + ---------------------------------------------------------------------------- + + DESIGNED BY: Bruce Jackson + + CODED BY: Bruce Jackson + + MAINTAINED BY: + + ---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE + + 931007 Moved calculations of auxiliary accelerations here from ls_aux.c BY + and corrected minus sign in front of A_Y_Pilot + contribution from Q_body*P_body*D_X_pilot term. + 940111 Changed DATA to SCALAR; updated header files + +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:09:53 curt +Initial Flight Gear revision. + + * Revision 1.5 1994/01/11 18:42:16 bjax + * Oops! Changed data types from DATA to SCALAR for precision control. + * + * Revision 1.4 1994/01/11 18:36:58 bjax + * Removed ls_eom.h include directive; replaced with ls_types, ls_constants, + * and ls_generic.h includes. + * + * Revision 1.3 1993/10/07 18:45:24 bjax + * Added local defn of d[xyz]_pilot_from_cg to support previous mod. EBJ + * + * Revision 1.2 1993/10/07 18:41:31 bjax + * Moved calculations of auxiliary accelerations here from ls_aux, and + * corrected sign on Q_body*P_body*d_x_pilot term of A_Y_pilot calc. EBJ + * + * Revision 1.1 1992/12/30 13:17:02 bjax + * Initial revision + * + + ---------------------------------------------------------------------------- + + REFERENCES: + + [ 1] McFarland, Richard E.: "A Standard Kinematic Model + for Flight Simulation at NASA-Ames", NASA CR-2497, + January 1975 + + ---------------------------------------------------------------------------- + + CALLED BY: + + ---------------------------------------------------------------------------- + + CALLS TO: + + ---------------------------------------------------------------------------- + + INPUTS: Aero, engine, gear forces & moments + + ---------------------------------------------------------------------------- + + OUTPUTS: State derivatives + + -------------------------------------------------------------------------*/ +#include "ls_types.h" +#include "ls_generic.h" +#include "ls_constants.h" +#include + +void ls_accel( ) +{ + + SCALAR inv_Mass, inv_Radius; + SCALAR ixz2, c0, c1, c2, c3, c4, c5, c6, c7, c8, c9, c10; + SCALAR dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg; + + + /* Sum forces and moments at reference point */ + + + F_X = F_X_aero + F_X_engine + F_X_gear; + F_Y = F_Y_aero + F_Y_engine + F_Y_gear; + F_Z = F_Z_aero + F_Z_engine + F_Z_gear; + + M_l_rp = M_l_aero + M_l_engine + M_l_gear; + M_m_rp = M_m_aero + M_m_engine + M_m_gear; + M_n_rp = M_n_aero + M_n_engine + M_n_gear; + + /* Transfer moments to center of gravity */ + + M_l_cg = M_l_rp + F_Y*Dz_cg - F_Z*Dy_cg; + M_m_cg = M_m_rp + F_Z*Dx_cg - F_X*Dz_cg; + M_n_cg = M_n_rp + F_X*Dy_cg - F_Y*Dx_cg; + + /* Transform from body to local frame */ + + F_north = T_local_to_body_11*F_X + T_local_to_body_21*F_Y + + T_local_to_body_31*F_Z; + F_east = T_local_to_body_12*F_X + T_local_to_body_22*F_Y + + T_local_to_body_32*F_Z; + F_down = T_local_to_body_13*F_X + T_local_to_body_23*F_Y + + T_local_to_body_33*F_Z; + + /* Calculate linear accelerations */ + + inv_Mass = 1/Mass; + inv_Radius = 1/Radius_to_vehicle; + V_dot_north = inv_Mass*F_north + + inv_Radius*(V_north*V_down - V_east*V_east*tan(Lat_geocentric)); + V_dot_east = inv_Mass*F_east + + inv_Radius*(V_east*V_down + V_north*V_east*tan(Lat_geocentric)); + V_dot_down = inv_Mass*(F_down) + Gravity - + inv_Radius*(V_north*V_north + V_east*V_east); + + /* Invert the symmetric inertia matrix */ + + ixz2 = I_xz*I_xz; + c0 = 1/(I_xx*I_zz - ixz2); + c1 = c0*((I_yy-I_zz)*I_zz - ixz2); + c2 = c0*I_xz*(I_xx - I_yy + I_zz); + c3 = c0*I_zz; + c4 = c0*I_xz; + c7 = 1/I_yy; + c5 = c7*(I_zz - I_xx); + c6 = c7*I_xz; + c8 = c0*((I_xx - I_yy)*I_xx + ixz2); + c9 = c0*I_xz*(I_yy - I_zz - I_xx); + c10 = c0*I_xx; + + /* Calculate the rotational body axis accelerations */ + + P_dot_body = (c1*R_body + c2*P_body)*Q_body + c3*M_l_cg + c4*M_n_cg; + Q_dot_body = c5*R_body*P_body + c6*(R_body*R_body - P_body*P_body) + c7*M_m_cg; + R_dot_body = (c8*P_body + c9*R_body)*Q_body + c4*M_l_cg + c10*M_n_cg; + + /* Calculate body axis accelerations (move to ls_accel?) */ + + inv_Mass = 1/Mass; + + A_X_cg = F_X * inv_Mass; + A_Y_cg = F_Y * inv_Mass; + A_Z_cg = F_Z * inv_Mass; + + dx_pilot_from_cg = Dx_pilot - Dx_cg; + dy_pilot_from_cg = Dy_pilot - Dy_cg; + dz_pilot_from_cg = Dz_pilot - Dz_cg; + + A_X_pilot = A_X_cg + (-R_body*R_body - Q_body*Q_body)*dx_pilot_from_cg + + ( P_body*Q_body - R_dot_body )*dy_pilot_from_cg + + ( P_body*R_body + Q_dot_body )*dz_pilot_from_cg; + A_Y_pilot = A_Y_cg + ( P_body*Q_body + R_dot_body )*dx_pilot_from_cg + + (-P_body*P_body - R_body*R_body)*dy_pilot_from_cg + + ( Q_body*R_body - P_dot_body )*dz_pilot_from_cg; + A_Z_pilot = A_Z_cg + ( P_body*R_body - Q_dot_body )*dx_pilot_from_cg + + ( Q_body*R_body + P_dot_body )*dy_pilot_from_cg + + (-Q_body*Q_body - P_body*P_body)*dz_pilot_from_cg; + + N_X_cg = INVG*A_X_cg; + N_Y_cg = INVG*A_Y_cg; + N_Z_cg = INVG*A_Z_cg; + + N_X_pilot = INVG*A_X_pilot; + N_Y_pilot = INVG*A_Y_pilot; + N_Z_pilot = INVG*A_Z_pilot; + + + U_dot_body = T_local_to_body_11*V_dot_north + T_local_to_body_12*V_dot_east + + T_local_to_body_13*V_dot_down - Q_total*W_body + R_total*V_body; + V_dot_body = T_local_to_body_21*V_dot_north + T_local_to_body_22*V_dot_east + + T_local_to_body_23*V_dot_down - R_total*U_body + P_total*W_body; + W_dot_body = T_local_to_body_31*V_dot_north + T_local_to_body_32*V_dot_east + + T_local_to_body_33*V_dot_down - P_total*V_body + Q_total*U_body; + /* End of ls_accel */ + +} +/**************************************************************************/ + + + + diff --git a/LaRCsim/ls_aux.c b/LaRCsim/ls_aux.c new file mode 100644 index 000000000..b77f8df88 --- /dev/null +++ b/LaRCsim/ls_aux.c @@ -0,0 +1,328 @@ +/*************************************************************************** + + TITLE: ls_aux + +---------------------------------------------------------------------------- + + FUNCTION: Atmospheric and auxilary relationships for LaRCSim EOM + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Created 9208026 as part of C-castle simulation project. + +---------------------------------------------------------------------------- + + DESIGNED BY: B. Jackson + + CODED BY: B. Jackson + + MAINTAINED BY: B. Jackson + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE + + 931006 Moved calculations of auxiliary accelerations from here + to ls_accel.c and corrected minus sign in front of A_Y_Pilot + contribution from Q_body*P_body*D_X_pilot term. EBJ + 931014 Changed calculation of Alpha from atan to atan2 so sign is correct. + EBJ + 931220 Added calculations for static and total temperatures & pressures, + as well as dynamic and impact pressures and calibrated airspeed. + EBJ + 940111 Changed #included header files from old "ls_eom.h" to newer + "ls_types.h", "ls_constants.h" and "ls_generic.h". EBJ + + 950207 Changed use of "abs" to "fabs" in calculation of signU. EBJ + + 950228 Fixed bug in calculation of beta_dot. EBJ + + CURRENT RCS HEADER INFO: + +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:09:54 curt +Initial Flight Gear revision. + + * Revision 1.12 1995/02/28 17:57:16 bjax + * Corrected calculation of beta_dot. EBJ + * + * Revision 1.11 1995/02/07 21:09:47 bjax + * Corrected calculation of "signU"; was using divide by + * abs(), which returns an integer; now using fabs(), which + * returns a double. EBJ + * + * Revision 1.10 1994/05/10 20:09:42 bjax + * Fixed a major problem with dx_pilot_from_cg, etc. not being calculated locally. + * + * Revision 1.9 1994/01/11 18:44:33 bjax + * Changed header files to use ls_types, ls_constants, and ls_generic. + * + * Revision 1.8 1993/12/21 14:36:33 bjax + * Added calcs of pressures, temps and calibrated airspeeds. + * + * Revision 1.7 1993/10/14 11:25:38 bjax + * Changed calculation of Alpha to use 'atan2' instead of 'atan' so alphas + * larger than +/- 90 degrees are calculated correctly. EBJ + * + * Revision 1.6 1993/10/07 18:45:56 bjax + * A little cleanup; no significant changes. EBJ + * + * Revision 1.5 1993/10/07 18:42:22 bjax + * Moved calculations of auxiliary accelerations here from ls_aux, and + * corrected sign on Q_body*P_body*d_x_pilot term of A_Y_pilot calc. EBJ + * + * Revision 1.4 1993/07/16 18:28:58 bjax + * Changed call from atmos_62 to ls_atmos. EBJ + * + * Revision 1.3 1993/06/02 15:02:42 bjax + * Changed call to geodesy calcs from ls_geodesy to ls_geoc_to_geod. + * + * Revision 1.1 92/12/30 13:17:39 bjax + * Initial revision + * + + +---------------------------------------------------------------------------- + + REFERENCES: [ 1] Shapiro, Ascher H.: "The Dynamics and Thermodynamics + of Compressible Fluid Flow", Volume I, The Ronald + Press, 1953. + +---------------------------------------------------------------------------- + + CALLED BY: + +---------------------------------------------------------------------------- + + CALLS TO: + +---------------------------------------------------------------------------- + + INPUTS: + +---------------------------------------------------------------------------- + + OUTPUTS: + +--------------------------------------------------------------------------*/ +#include "ls_types.h" +#include "ls_constants.h" +#include "ls_generic.h" +#include + +void ls_aux() +{ + + SCALAR dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg; + SCALAR inv_Mass; + SCALAR v_XZ_plane_2, signU, v_tangential; + SCALAR inv_radius_ratio; + SCALAR cos_rwy_hdg, sin_rwy_hdg; + SCALAR mach2, temp_ratio, pres_ratio; + + /* update geodetic position */ + + ls_geoc_to_geod( Lat_geocentric, Radius_to_vehicle, + &Latitude, &Altitude, &Sea_level_radius ); + Longitude = Lon_geocentric - Earth_position_angle; + + /* Calculate body axis velocities */ + + /* Form relative velocity vector */ + + V_north_rel_ground = V_north; + V_east_rel_ground = V_east + - OMEGA_EARTH*Sea_level_radius*cos( Lat_geocentric ); + V_down_rel_ground = V_down; + + V_north_rel_airmass = V_north_rel_ground - V_north_airmass; + V_east_rel_airmass = V_east_rel_ground - V_east_airmass; + V_down_rel_airmass = V_down_rel_ground - V_down_airmass; + + U_body = T_local_to_body_11*V_north_rel_airmass + + T_local_to_body_12*V_east_rel_airmass + + T_local_to_body_13*V_down_rel_airmass + U_gust; + V_body = T_local_to_body_21*V_north_rel_airmass + + T_local_to_body_22*V_east_rel_airmass + + T_local_to_body_23*V_down_rel_airmass + V_gust; + W_body = T_local_to_body_31*V_north_rel_airmass + + T_local_to_body_32*V_east_rel_airmass + + T_local_to_body_33*V_down_rel_airmass + W_gust; + + V_rel_wind = sqrt(U_body*U_body + V_body*V_body + W_body*W_body); + + + /* Calculate alpha and beta rates */ + + v_XZ_plane_2 = (U_body*U_body + W_body*W_body); + + if (U_body == 0) + signU = 1; + else + signU = U_body/fabs(U_body); + + if( (v_XZ_plane_2 == 0) || (V_rel_wind == 0) ) + { + Alpha_dot = 0; + Beta_dot = 0; + } + else + { + Alpha_dot = (U_body*W_dot_body - W_body*U_dot_body)/ + v_XZ_plane_2; + Beta_dot = (signU*v_XZ_plane_2*V_dot_body + - V_body*(U_body*U_dot_body + W_body*W_dot_body)) + /(V_rel_wind*V_rel_wind*sqrt(v_XZ_plane_2)); + } + + /* Calculate flight path and other flight condition values */ + + if (U_body == 0) + Alpha = 0; + else + Alpha = atan2( W_body, U_body ); + + Cos_alpha = cos(Alpha); + Sin_alpha = sin(Alpha); + + if (V_rel_wind == 0) + Beta = 0; + else + Beta = asin( V_body/ V_rel_wind ); + + Cos_beta = cos(Beta); + Sin_beta = sin(Beta); + + V_true_kts = V_rel_wind * V_TO_KNOTS; + + V_ground_speed = sqrt(V_north_rel_ground*V_north_rel_ground + + V_east_rel_ground*V_east_rel_ground ); + + V_rel_ground = sqrt(V_ground_speed*V_ground_speed + + V_down_rel_ground*V_down_rel_ground ); + + v_tangential = sqrt(V_north*V_north + V_east*V_east); + + V_inertial = sqrt(v_tangential*v_tangential + V_down*V_down); + + if( (V_ground_speed == 0) && (V_down == 0) ) + Gamma_vert_rad = 0; + else + Gamma_vert_rad = atan2( -V_down, V_ground_speed ); + + if( (V_north_rel_ground == 0) && (V_east_rel_ground == 0) ) + Gamma_horiz_rad = 0; + else + Gamma_horiz_rad = atan2( V_east_rel_ground, V_north_rel_ground ); + + if (Gamma_horiz_rad < 0) + Gamma_horiz_rad = Gamma_horiz_rad + 2*PI; + + /* Calculate local gravity */ + + ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity ); + + /* call function for (smoothed) density ratio, sonic velocity, and + ambient pressure */ + + ls_atmos(Altitude, &Sigma, &V_sound, + &Static_temperature, &Static_pressure); + + Density = Sigma*SEA_LEVEL_DENSITY; + + Mach_number = V_rel_wind / V_sound; + + V_equiv = V_rel_wind*sqrt(Sigma); + + V_equiv_kts = V_equiv*V_TO_KNOTS; + + /* calculate temperature and pressure ratios (from [1]) */ + + mach2 = Mach_number*Mach_number; + temp_ratio = 1.0 + 0.2*mach2; + pres_ratio = pow( temp_ratio, 3.5 ); + + Total_temperature = temp_ratio*Static_temperature; + Total_pressure = pres_ratio*Static_pressure; + + /* calculate impact and dynamic pressures */ + + Impact_pressure = Total_pressure - Static_pressure; + + Dynamic_pressure = 0.5*Density*V_rel_wind*V_rel_wind; + + /* calculate calibrated airspeed indication */ + + V_calibrated = sqrt( 2.0*Dynamic_pressure / SEA_LEVEL_DENSITY ); + V_calibrated_kts = V_calibrated*V_TO_KNOTS; + + Centrifugal_relief = 1 - v_tangential/(Radius_to_vehicle*Gravity); + +/* Determine location in runway coordinates */ + + Radius_to_rwy = Sea_level_radius + Runway_altitude; + cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD); + sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD); + + D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude); + D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude) + *(Longitude - Runway_longitude); + D_cg_above_rwy = Radius_to_vehicle - Radius_to_rwy; + + X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg + + D_cg_east_of_rwy*sin_rwy_hdg; + Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg + + D_cg_east_of_rwy*cos_rwy_hdg; + H_cg_rwy = D_cg_above_rwy; + + dx_pilot_from_cg = Dx_pilot - Dx_cg; + dy_pilot_from_cg = Dy_pilot - Dy_cg; + dz_pilot_from_cg = Dz_pilot - Dz_cg; + + D_pilot_north_of_rwy = D_cg_north_of_rwy + + T_local_to_body_11*dx_pilot_from_cg + + T_local_to_body_21*dy_pilot_from_cg + + T_local_to_body_31*dz_pilot_from_cg; + D_pilot_east_of_rwy = D_cg_east_of_rwy + + T_local_to_body_12*dx_pilot_from_cg + + T_local_to_body_22*dy_pilot_from_cg + + T_local_to_body_32*dz_pilot_from_cg; + D_pilot_above_rwy = D_cg_above_rwy + - T_local_to_body_13*dx_pilot_from_cg + - T_local_to_body_23*dy_pilot_from_cg + - T_local_to_body_33*dz_pilot_from_cg; + + X_pilot_rwy = D_pilot_north_of_rwy*cos_rwy_hdg + + D_pilot_east_of_rwy*sin_rwy_hdg; + Y_pilot_rwy = -D_pilot_north_of_rwy*sin_rwy_hdg + + D_pilot_east_of_rwy*cos_rwy_hdg; + H_pilot_rwy = D_pilot_above_rwy; + +/* Calculate Euler rates */ + + Sin_phi = sin(Phi); + Cos_phi = cos(Phi); + Sin_theta = sin(Theta); + Cos_theta = cos(Theta); + Sin_psi = sin(Psi); + Cos_psi = cos(Psi); + + if( Cos_theta == 0 ) + Psi_dot = 0; + else + Psi_dot = (Q_total*Sin_phi + R_total*Cos_phi)/Cos_theta; + + Theta_dot = Q_total*Cos_phi - R_total*Sin_phi; + Phi_dot = P_total + Psi_dot*Sin_theta; + +/* end of ls_aux */ + +} +/*************************************************************************/ diff --git a/LaRCsim/ls_cockpit.h b/LaRCsim/ls_cockpit.h new file mode 100644 index 000000000..7543ba941 --- /dev/null +++ b/LaRCsim/ls_cockpit.h @@ -0,0 +1,80 @@ +/*************************************************************************** + + TITLE: ls_cockpit.h + +---------------------------------------------------------------------------- + + FUNCTION: Header for cockpit IO + +---------------------------------------------------------------------------- + + MODULE STATUS: Developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Created 20 DEC 93 by E. B. Jackson + +---------------------------------------------------------------------------- + + DESIGNED BY: E. B. Jackson + + CODED BY: E. B. Jackson + + MAINTAINED BY: E. B. Jackson + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + 950314 Added "throttle_pct" field to cockpit header for both + display and trim purposes. EBJ + + CURRENT RCS HEADER: + +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:09:54 curt +Initial Flight Gear revision. + + * Revision 1.3 1995/03/15 12:32:10 bjax + * Added throttle_pct field. + * + * Revision 1.2 1995/02/28 20:37:02 bjax + * Changed name of gear_sel_down switch to gear_sel_up to reflect + * correct sense of switch. EBJ + * + * Revision 1.1 1993/12/21 14:39:04 bjax + * Initial revision + * + +--------------------------------------------------------------------------*/ + +typedef struct { + float long_stick, lat_stick, rudder_pedal; + float throttle[4]; + short forward_trim, aft_trim, left_trim, right_trim; + short left_pb_on_stick, right_pb_on_stick, trig_pos_1, trig_pos_2; + short sb_extend, sb_retract, gear_sel_up; + float throttle_pct; +} COCKPIT; + +extern COCKPIT cockpit_; + +#define Left_button cockpit_.left_pb_on_stick +#define Right_button cockpit_.right_pb_on_stick +#define Rudder_pedal cockpit_.rudder_pedal +#define Throttle cockpit_.throttle +#define Throttle_pct cockpit_.throttle_pct +#define First_trigger cockpit_.trig_pos_1 +#define Second_trigger cockpit_.trig_pos_2 +#define Long_control cockpit_.long_stick +#define Lat_control cockpit_.lat_stick +#define Fwd_trim cockpit_.forward_trim +#define Aft_trim cockpit_.aft_trim +#define Left_trim cockpit_.left_trim +#define Right_trim cockpit_.right_trim +#define SB_extend cockpit_.sb_extend +#define SB_retract cockpit_.sb_retract +#define Gear_sel_up cockpit_.gear_sel_up diff --git a/LaRCsim/ls_constants.h b/LaRCsim/ls_constants.h new file mode 100644 index 000000000..3e90aab58 --- /dev/null +++ b/LaRCsim/ls_constants.h @@ -0,0 +1,122 @@ +/*************************************************************************** + + TITLE: ls_constants.h + +---------------------------------------------------------------------------- + + FUNCTION: LaRCSim constants definition header file + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Created 15 DEC 1993 by Bruce Jackson; was part of + old ls_eom.h header file + +---------------------------------------------------------------------------- + + DESIGNED BY: B. Jackson + + CODED BY: B. Jackson + + MAINTAINED BY: guess who + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + +---------------------------------------------------------------------------- + + REFERENCES: + + [ 1] McFarland, Richard E.: "A Standard Kinematic Model + for Flight Simulation at NASA-Ames", NASA CR-2497, + January 1975 + + [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos- + pheric and Space Flight Vehicle Coordinate Systems", + February 1992 + + [ 3] Beyer, William H., editor: "CRC Standard Mathematical + Tables, 28th edition", CRC Press, Boca Raton, FL, 1987, + ISBN 0-8493-0628-0 + + [ 4] Dowdy, M. C.; Jackson, E. B.; and Nichols, J. H.: + "Controls Analysis and Simulation Test Loop Environ- + ment (CASTLE) Programmer's Guide, Version 1.3", + NATC TM 89-11, 30 March 1989. + + [ 5] Halliday, David; and Resnick, Robert: "Fundamentals + of Physics, Revised Printing", Wiley and Sons, 1974. + ISBN 0-471-34431-1 + + [ 6] Anon: "U. S. Standard Atmosphere, 1962" + + [ 7] Anon: "Aeronautical Vest Pocket Handbook, 17th edition", + Pratt & Whitney Aircraft Group, Dec. 1977 + + [ 8] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft + Control and Simulation", Wiley and Sons, 1992. + ISBN 0-471-61397-5 + +--------------------------------------------------------------------------*/ + +#ifndef CONSTANTS + +#define CONSTANTS -1 + +/* Define application-wide macros */ + +#define PATHNAME "LARCSIMPATH" +#ifndef NIL_POINTER +#define NIL_POINTER 0L +#endif + +/* Define constants (note: many factors will need to change for other + systems of measure) */ + +/* Value of Pi from ref [3] */ +#define PI 3.14159265358979323846264338327950288419716939967511 + +/* Value of earth radius from [8], ft */ +#define EQUATORIAL_RADIUS 20925650. +#define RESQ 437882827922500. + +/* Value of earth flattening parameter from ref [8] + + Note: FP = f + E = 1-f + EPS = sqrt(1-(1-f)^2) */ + +#define FP .003352813178 +#define E .996647186 +#define EPS .081819221 +#define INVG .031080997 + +/* linear velocity of earth at equator from w*R; w=2pi/24 hrs, in ft/sec */ +#define OMEGA_EARTH .00007272205217 + +/* miscellaneous units conversions (ref [7]) */ +#define V_TO_KNOTS 0.5921 +#define DEG_TO_RAD 0.017453292 +#define RAD_TO_DEG 57.29577951 +#define FT_TO_METERS 0.3048 +#define METERS_TO_FT 3.2808 +#define K_TO_R 1.8 +#define R_TO_K 0.55555556 +#define NSM_TO_PSF 0.02088547 +#define PSF_TO_NSM 47.8801826 +#define KCM_TO_SCF 0.00194106 +#define SCF_TO_KCM 515.183616 + + +/* ENGLISH Atmospheric reference properties [6] */ +#define SEA_LEVEL_DENSITY 0.002376888 + +#endif + +/*------------------------- end of ls_constants.h -------------------------*/ diff --git a/LaRCsim/ls_generic.h b/LaRCsim/ls_generic.h new file mode 100644 index 000000000..a996aef4a --- /dev/null +++ b/LaRCsim/ls_generic.h @@ -0,0 +1,404 @@ +/*************************************************************************** + + TITLE: ls_generic.h + +---------------------------------------------------------------------------- + + FUNCTION: LaRCSim generic parameters header file + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Created 15 DEC 1993 by Bruce Jackson; + was part of old ls_eom.h header + +---------------------------------------------------------------------------- + + DESIGNED BY: B. Jackson + + CODED BY: B. Jackson + + MAINTAINED BY: guess who + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + +---------------------------------------------------------------------------- + + REFERENCES: + + [ 1] McFarland, Richard E.: "A Standard Kinematic Model + for Flight Simulation at NASA-Ames", NASA CR-2497, + January 1975 + + [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos- + pheric and Space Flight Vehicle Coordinate Systems", + February 1992 + + [ 3] Beyer, William H., editor: "CRC Standard Mathematical + Tables, 28th edition", CRC Press, Boca Raton, FL, 1987, + ISBN 0-8493-0628-0 + + [ 4] Dowdy, M. C.; Jackson, E. B.; and Nichols, J. H.: + "Controls Analysis and Simulation Test Loop Environ- + ment (CASTLE) Programmer's Guide, Version 1.3", + NATC TM 89-11, 30 March 1989. + + [ 5] Halliday, David; and Resnick, Robert: "Fundamentals + of Physics, Revised Printing", Wiley and Sons, 1974. + ISBN 0-471-34431-1 + + [ 6] Anon: "U. S. Standard Atmosphere, 1962" + + [ 7] Anon: "Aeronautical Vest Pocket Handbook, 17th edition", + Pratt & Whitney Aircraft Group, Dec. 1977 + + [ 8] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft + Control and Simulation", Wiley and Sons, 1992. + ISBN 0-471-61397-5 + +--------------------------------------------------------------------------*/ + +typedef struct { + +/*================== Mass properties and geometry values ==================*/ + + DATA mass, i_xx, i_yy, i_zz, i_xz; /* Inertias */ +#define Mass generic_.mass +#define I_xx generic_.i_xx +#define I_yy generic_.i_yy +#define I_zz generic_.i_zz +#define I_xz generic_.i_xz + + VECTOR_3 d_pilot_rp_body_v; /* Pilot location rel to ref pt */ +#define D_pilot_rp_body_v generic_.d_pilot_rp_body_v +#define Dx_pilot generic_.d_pilot_rp_body_v[0] +#define Dy_pilot generic_.d_pilot_rp_body_v[1] +#define Dz_pilot generic_.d_pilot_rp_body_v[2] + + VECTOR_3 d_cg_rp_body_v; /* CG position w.r.t. ref. point */ +#define D_cg_rp_body_v generic_.d_cg_rp_body_v +#define Dx_cg generic_.d_cg_rp_body_v[0] +#define Dy_cg generic_.d_cg_rp_body_v[1] +#define Dz_cg generic_.d_cg_rp_body_v[2] + +/*================================ Forces =================================*/ + + VECTOR_3 f_body_total_v; +#define F_body_total_v generic_.f_body_total_v +#define F_X generic_.f_body_total_v[0] +#define F_Y generic_.f_body_total_v[1] +#define F_Z generic_.f_body_total_v[2] + + VECTOR_3 f_local_total_v; +#define F_local_total_v generic_.f_local_total_v +#define F_north generic_.f_local_total_v[0] +#define F_east generic_.f_local_total_v[1] +#define F_down generic_.f_local_total_v[2] + + VECTOR_3 f_aero_v; +#define F_aero_v generic_.f_aero_v +#define F_X_aero generic_.f_aero_v[0] +#define F_Y_aero generic_.f_aero_v[1] +#define F_Z_aero generic_.f_aero_v[2] + + VECTOR_3 f_engine_v; +#define F_engine_v generic_.f_engine_v +#define F_X_engine generic_.f_engine_v[0] +#define F_Y_engine generic_.f_engine_v[1] +#define F_Z_engine generic_.f_engine_v[2] + + VECTOR_3 f_gear_v; +#define F_gear_v generic_.f_gear_v +#define F_X_gear generic_.f_gear_v[0] +#define F_Y_gear generic_.f_gear_v[1] +#define F_Z_gear generic_.f_gear_v[2] + +/*================================ Moments ================================*/ + + VECTOR_3 m_total_rp_v; +#define M_total_rp_v generic_.m_total_rp_v +#define M_l_rp generic_.m_total_rp_v[0] +#define M_m_rp generic_.m_total_rp_v[1] +#define M_n_rp generic_.m_total_rp_v[2] + + VECTOR_3 m_total_cg_v; +#define M_total_cg_v generic_.m_total_cg_v +#define M_l_cg generic_.m_total_cg_v[0] +#define M_m_cg generic_.m_total_cg_v[1] +#define M_n_cg generic_.m_total_cg_v[2] + + VECTOR_3 m_aero_v; +#define M_aero_v generic_.m_aero_v +#define M_l_aero generic_.m_aero_v[0] +#define M_m_aero generic_.m_aero_v[1] +#define M_n_aero generic_.m_aero_v[2] + + VECTOR_3 m_engine_v; +#define M_engine_v generic_.m_engine_v +#define M_l_engine generic_.m_engine_v[0] +#define M_m_engine generic_.m_engine_v[1] +#define M_n_engine generic_.m_engine_v[2] + + VECTOR_3 m_gear_v; +#define M_gear_v generic_.m_gear_v +#define M_l_gear generic_.m_gear_v[0] +#define M_m_gear generic_.m_gear_v[1] +#define M_n_gear generic_.m_gear_v[2] + +/*============================== Accelerations ============================*/ + + VECTOR_3 v_dot_local_v; +#define V_dot_local_v generic_.v_dot_local_v +#define V_dot_north generic_.v_dot_local_v[0] +#define V_dot_east generic_.v_dot_local_v[1] +#define V_dot_down generic_.v_dot_local_v[2] + + VECTOR_3 v_dot_body_v; +#define V_dot_body_v generic_.v_dot_body_v +#define U_dot_body generic_.v_dot_body_v[0] +#define V_dot_body generic_.v_dot_body_v[1] +#define W_dot_body generic_.v_dot_body_v[2] + + VECTOR_3 a_cg_body_v; +#define A_cg_body_v generic_.a_cg_body_v +#define A_X_cg generic_.a_cg_body_v[0] +#define A_Y_cg generic_.a_cg_body_v[1] +#define A_Z_cg generic_.a_cg_body_v[2] + + VECTOR_3 a_pilot_body_v; +#define A_pilot_body_v generic_.a_pilot_body_v +#define A_X_pilot generic_.a_pilot_body_v[0] +#define A_Y_pilot generic_.a_pilot_body_v[1] +#define A_Z_pilot generic_.a_pilot_body_v[2] + + VECTOR_3 n_cg_body_v; +#define N_cg_body_v generic_.n_cg_body_v +#define N_X_cg generic_.n_cg_body_v[0] +#define N_Y_cg generic_.n_cg_body_v[1] +#define N_Z_cg generic_.n_cg_body_v[2] + + VECTOR_3 n_pilot_body_v; +#define N_pilot_body_v generic_.n_pilot_body_v +#define N_X_pilot generic_.n_pilot_body_v[0] +#define N_Y_pilot generic_.n_pilot_body_v[1] +#define N_Z_pilot generic_.n_pilot_body_v[2] + + VECTOR_3 omega_dot_body_v; +#define Omega_dot_body_v generic_.omega_dot_body_v +#define P_dot_body generic_.omega_dot_body_v[0] +#define Q_dot_body generic_.omega_dot_body_v[1] +#define R_dot_body generic_.omega_dot_body_v[2] + + +/*============================== Velocities ===============================*/ + + VECTOR_3 v_local_v; +#define V_local_v generic_.v_local_v +#define V_north generic_.v_local_v[0] +#define V_east generic_.v_local_v[1] +#define V_down generic_.v_local_v[2] + + VECTOR_3 v_local_rel_ground_v; /* V rel w.r.t. earth surface */ +#define V_local_rel_ground_v generic_.v_local_rel_ground_v +#define V_north_rel_ground generic_.v_local_rel_ground_v[0] +#define V_east_rel_ground generic_.v_local_rel_ground_v[1] +#define V_down_rel_ground generic_.v_local_rel_ground_v[2] + + VECTOR_3 v_local_airmass_v; /* velocity of airmass (steady winds) */ +#define V_local_airmass_v generic_.v_local_airmass_v +#define V_north_airmass generic_.v_local_airmass_v[0] +#define V_east_airmass generic_.v_local_airmass_v[1] +#define V_down_airmass generic_.v_local_airmass_v[2] + + VECTOR_3 v_local_rel_airmass_v; /* velocity of veh. relative to airmass */ +#define V_local_rel_airmass_v generic_.v_local_rel_airmass_v +#define V_north_rel_airmass generic_.v_local_rel_airmass_v[0] +#define V_east_rel_airmass generic_.v_local_rel_airmass_v[1] +#define V_down_rel_airmass generic_.v_local_rel_airmass_v[2] + + VECTOR_3 v_local_gust_v; /* linear turbulence components, L frame */ +#define V_local_gust_v generic_.v_local_gust_v +#define U_gust generic_.v_local_gust_v[0] +#define V_gust generic_.v_local_gust_v[1] +#define W_gust generic_.v_local_gust_v[2] + + VECTOR_3 v_wind_body_v; /* Wind-relative velocities in body axis */ +#define V_wind_body_v generic_.v_wind_body_v +#define U_body generic_.v_wind_body_v[0] +#define V_body generic_.v_wind_body_v[1] +#define W_body generic_.v_wind_body_v[2] + + DATA v_rel_wind, v_true_kts, v_rel_ground, v_inertial; + DATA v_ground_speed, v_equiv, v_equiv_kts; + DATA v_calibrated, v_calibrated_kts; +#define V_rel_wind generic_.v_rel_wind +#define V_true_kts generic_.v_true_kts +#define V_rel_ground generic_.v_rel_ground +#define V_inertial generic_.v_inertial +#define V_ground_speed generic_.v_ground_speed +#define V_equiv generic_.v_equiv +#define V_equiv_kts generic_.v_equiv_kts +#define V_calibrated generic_.v_calibrated +#define V_calibrated_kts generic_.v_calibrated_kts + + VECTOR_3 omega_body_v; /* Angular B rates */ +#define Omega_body_v generic_.omega_body_v +#define P_body generic_.omega_body_v[0] +#define Q_body generic_.omega_body_v[1] +#define R_body generic_.omega_body_v[2] + + VECTOR_3 omega_local_v; /* Angular L rates */ +#define Omega_local_v generic_.omega_local_v +#define P_local generic_.omega_local_v[0] +#define Q_local generic_.omega_local_v[1] +#define R_local generic_.omega_local_v[2] + + VECTOR_3 omega_total_v; /* Diff btw B & L */ +#define Omega_total_v generic_.omega_total_v +#define P_total generic_.omega_total_v[0] +#define Q_total generic_.omega_total_v[1] +#define R_total generic_.omega_total_v[2] + + VECTOR_3 euler_rates_v; +#define Euler_rates_v generic_.euler_rates_v +#define Phi_dot generic_.euler_rates_v[0] +#define Theta_dot generic_.euler_rates_v[1] +#define Psi_dot generic_.euler_rates_v[2] + + VECTOR_3 geocentric_rates_v; /* Geocentric linear velocities */ +#define Geocentric_rates_v generic_.geocentric_rates_v +#define Latitude_dot generic_.geocentric_rates_v[0] +#define Longitude_dot generic_.geocentric_rates_v[1] +#define Radius_dot generic_.geocentric_rates_v[2] + +/*=============================== Positions ===============================*/ + + VECTOR_3 geocentric_position_v; +#define Geocentric_position_v generic_.geocentric_position_v +#define Lat_geocentric generic_.geocentric_position_v[0] +#define Lon_geocentric generic_.geocentric_position_v[1] +#define Radius_to_vehicle generic_.geocentric_position_v[2] + + VECTOR_3 geodetic_position_v; +#define Geodetic_position_v generic_.geodetic_position_v +#define Latitude generic_.geodetic_position_v[0] +#define Longitude generic_.geodetic_position_v[1] +#define Altitude generic_.geodetic_position_v[2] + + VECTOR_3 euler_angles_v; +#define Euler_angles_v generic_.euler_angles_v +#define Phi generic_.euler_angles_v[0] +#define Theta generic_.euler_angles_v[1] +#define Psi generic_.euler_angles_v[2] + +/*======================= Miscellaneous quantities ========================*/ + + DATA t_local_to_body_m[3][3]; /* Transformation matrix L to B */ +#define T_local_to_body_m generic_.t_local_to_body_m +#define T_local_to_body_11 generic_.t_local_to_body_m[0][0] +#define T_local_to_body_12 generic_.t_local_to_body_m[0][1] +#define T_local_to_body_13 generic_.t_local_to_body_m[0][2] +#define T_local_to_body_21 generic_.t_local_to_body_m[1][0] +#define T_local_to_body_22 generic_.t_local_to_body_m[1][1] +#define T_local_to_body_23 generic_.t_local_to_body_m[1][2] +#define T_local_to_body_31 generic_.t_local_to_body_m[2][0] +#define T_local_to_body_32 generic_.t_local_to_body_m[2][1] +#define T_local_to_body_33 generic_.t_local_to_body_m[2][2] + + DATA gravity; /* Local acceleration due to G */ +#define Gravity generic_.gravity + + DATA centrifugal_relief; /* load factor reduction due to speed */ +#define Centrifugal_relief generic_.centrifugal_relief + + DATA alpha, beta, alpha_dot, beta_dot; /* in radians */ +#define Alpha generic_.alpha +#define Beta generic_.beta +#define Alpha_dot generic_.alpha_dot +#define Beta_dot generic_.beta_dot + + DATA cos_alpha, sin_alpha, cos_beta, sin_beta; +#define Cos_alpha generic_.cos_alpha +#define Sin_alpha generic_.sin_alpha +#define Cos_beta generic_.cos_beta +#define Sin_beta generic_.sin_beta + + DATA cos_phi, sin_phi, cos_theta, sin_theta, cos_psi, sin_psi; +#define Cos_phi generic_.cos_phi +#define Sin_phi generic_.sin_phi +#define Cos_theta generic_.cos_theta +#define Sin_theta generic_.sin_theta +#define Cos_psi generic_.cos_psi +#define Sin_psi generic_.sin_psi + + DATA gamma_vert_rad, gamma_horiz_rad; /* Flight path angles */ +#define Gamma_vert_rad generic_.gamma_vert_rad +#define Gamma_horiz_rad generic_.gamma_horiz_rad + + DATA sigma, density, v_sound, mach_number; +#define Sigma generic_.sigma +#define Density generic_.density +#define V_sound generic_.v_sound +#define Mach_number generic_.mach_number + + DATA static_pressure, total_pressure, impact_pressure, dynamic_pressure; +#define Static_pressure generic_.static_pressure +#define Total_pressure generic_.total_pressure +#define Impact_pressure generic_.impact_pressure +#define Dynamic_pressure generic_.dynamic_pressure + + DATA static_temperature, total_temperature; +#define Static_temperature generic_.static_temperature +#define Total_temperature generic_.total_temperature + + DATA sea_level_radius, earth_position_angle; +#define Sea_level_radius generic_.sea_level_radius +#define Earth_position_angle generic_.earth_position_angle + + DATA runway_altitude, runway_latitude, runway_longitude, runway_heading; +#define Runway_altitude generic_.runway_altitude +#define Runway_latitude generic_.runway_latitude +#define Runway_longitude generic_.runway_longitude +#define Runway_heading generic_.runway_heading + + DATA radius_to_rwy; +#define Radius_to_rwy generic_.radius_to_rwy + + VECTOR_3 d_cg_rwy_local_v; /* CG rel. to rwy in local coords */ +#define D_cg_rwy_local_v generic_.d_cg_rwy_local_v +#define D_cg_north_of_rwy generic_.d_cg_rwy_local_v[0] +#define D_cg_east_of_rwy generic_.d_cg_rwy_local_v[1] +#define D_cg_above_rwy generic_.d_cg_rwy_local_v[2] + + VECTOR_3 d_cg_rwy_rwy_v; /* CG relative to runway, in rwy coordinates */ +#define D_cg_rwy_rwy_v generic_.d_cg_rwy_rwy_v +#define X_cg_rwy generic_.d_cg_rwy_rwy_v[0] +#define Y_cg_rwy generic_.d_cg_rwy_rwy_v[1] +#define H_cg_rwy generic_.d_cg_rwy_rwy_v[2] + + VECTOR_3 d_pilot_rwy_local_v; /* pilot rel. to rwy in local coords */ +#define D_pilot_rwy_local_v generic_.d_pilot_rwy_local_v +#define D_pilot_north_of_rwy generic_.d_pilot_rwy_local_v[0] +#define D_pilot_east_of_rwy generic_.d_pilot_rwy_local_v[1] +#define D_pilot_above_rwy generic_.d_pilot_rwy_local_v[2] + + VECTOR_3 d_pilot_rwy_rwy_v; /* pilot rel. to rwy, in rwy coords. */ +#define D_pilot_rwy_rwy_v generic_.d_pilot_rwy_rwy_v +#define X_pilot_rwy generic_.d_pilot_rwy_rwy_v[0] +#define Y_pilot_rwy generic_.d_pilot_rwy_rwy_v[1] +#define H_pilot_rwy generic_.d_pilot_rwy_rwy_v[2] + + +} GENERIC; + +extern GENERIC generic_; /* usually defined in ls_main.c */ + +/*--------------------------- end of ls_generic.h ------------------------*/ diff --git a/LaRCsim/ls_geodesy.c b/LaRCsim/ls_geodesy.c new file mode 100644 index 000000000..b083ee572 --- /dev/null +++ b/LaRCsim/ls_geodesy.c @@ -0,0 +1,158 @@ +/*************************************************************************** + + TITLE: ls_geodesy + +---------------------------------------------------------------------------- + + FUNCTION: Converts geocentric coordinates to geodetic positions + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Written as part of LaRCSim project by E. B. Jackson + +---------------------------------------------------------------------------- + + DESIGNED BY: E. B. Jackson + + CODED BY: E. B. Jackson + + MAINTAINED BY: E. B. Jackson + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + 930208 Modified to avoid singularity near polar region. EBJ + 930602 Moved backwards calcs here from ls_step. EBJ + 931214 Changed erroneous Latitude and Altitude variables to + *lat_geod and *alt in routine ls_geoc_to_geod. EBJ + 940111 Changed header files from old ls_eom.h style to ls_types, + and ls_constants. Also replaced old DATA type with new + SCALAR type. EBJ + + CURRENT RCS HEADER: + +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:09:56 curt +Initial Flight Gear revision. + + * Revision 1.5 1994/01/11 18:47:05 bjax + * Changed include files to use types and constants, not ls_eom.h + * Also changed DATA type to SCALAR type. + * + * Revision 1.4 1993/12/14 21:06:47 bjax + * Removed global variable references Altitude and Latitude. EBJ + * + * Revision 1.3 1993/06/02 15:03:40 bjax + * Made new subroutine for calculating geodetic to geocentric; changed name + * of forward conversion routine from ls_geodesy to ls_geoc_to_geod. + * + +---------------------------------------------------------------------------- + + REFERENCES: + + [ 1] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft + Control and Simulation", Wiley and Sons, 1992. + ISBN 0-471-61397-5 + + +---------------------------------------------------------------------------- + + CALLED BY: ls_aux + +---------------------------------------------------------------------------- + + CALLS TO: + +---------------------------------------------------------------------------- + + INPUTS: + lat_geoc Geocentric latitude, radians, + = North + radius C.G. radius to earth center, ft + +---------------------------------------------------------------------------- + + OUTPUTS: + lat_geod Geodetic latitude, radians, + = North + alt C.G. altitude above mean sea level, ft + sea_level_r radius from earth center to sea level at + local vertical (surface normal) of C.G. + +--------------------------------------------------------------------------*/ + +#include "ls_types.h" +#include "ls_constants.h" +#include + +/* ONE_SECOND is pi/180/60/60, or about 100 feet at earths' equator */ +#define ONE_SECOND 4.848136811E-6 +#define HALF_PI 0.5*PI + + +void ls_geoc_to_geod( lat_geoc, radius, lat_geod, alt, sea_level_r ) + SCALAR lat_geoc; + SCALAR radius; + SCALAR *lat_geod; + SCALAR *alt; + SCALAR *sea_level_r; +{ + SCALAR t_lat, x_alpha, mu_alpha, delt_mu, r_alpha, l_point, rho_alpha; + SCALAR sin_mu_a, denom,delt_lambda, lambda_sl, sin_lambda_sl; + + if( ( (HALF_PI - lat_geoc) < ONE_SECOND ) /* near North pole */ + || ( (HALF_PI + lat_geoc) < ONE_SECOND ) ) /* near South pole */ + { + *lat_geod = lat_geoc; + *sea_level_r = EQUATORIAL_RADIUS*E; + *alt = radius - *sea_level_r; + } + else + { + t_lat = tan(lat_geoc); + x_alpha = E*EQUATORIAL_RADIUS/sqrt(t_lat*t_lat + E*E); + mu_alpha = atan2(sqrt(RESQ - x_alpha*x_alpha),E*x_alpha); + if (lat_geoc < 0) mu_alpha = - mu_alpha; + sin_mu_a = sin(mu_alpha); + delt_lambda = mu_alpha - lat_geoc; + r_alpha = x_alpha/cos(lat_geoc); + l_point = radius - r_alpha; + *alt = l_point*cos(delt_lambda); + denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a); + rho_alpha = EQUATORIAL_RADIUS*(1-EPS)/ + (denom*denom*denom); + delt_mu = atan2(l_point*sin(delt_lambda),rho_alpha + *alt); + *lat_geod = mu_alpha - delt_mu; + lambda_sl = atan( E*E * tan(*lat_geod) ); /* SL geoc. latitude */ + sin_lambda_sl = sin( lambda_sl ); + *sea_level_r = sqrt(RESQ + /(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl)); + } +} + +void ls_geod_to_geoc( lat_geod, alt, sl_radius, lat_geoc ) + SCALAR lat_geod; + SCALAR alt; + SCALAR *sl_radius; + SCALAR *lat_geoc; +{ + SCALAR lambda_sl, sin_lambda_sl, cos_lambda_sl, sin_mu, cos_mu, px, py; + + lambda_sl = atan( E*E * tan(lat_geod) ); /* sea level geocentric latitude */ + sin_lambda_sl = sin( lambda_sl ); + cos_lambda_sl = cos( lambda_sl ); + sin_mu = sin(lat_geod); /* Geodetic (map makers') latitude */ + cos_mu = cos(lat_geod); + *sl_radius = sqrt(RESQ + /(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl)); + py = *sl_radius*sin_lambda_sl + alt*sin_mu; + px = *sl_radius*cos_lambda_sl + alt*cos_mu; + *lat_geoc = atan2( py, px ); +} diff --git a/LaRCsim/ls_gravity.c b/LaRCsim/ls_gravity.c new file mode 100644 index 000000000..e146b5cb2 --- /dev/null +++ b/LaRCsim/ls_gravity.c @@ -0,0 +1,91 @@ +/*************************************************************************** + + TITLE: ls_gravity + +---------------------------------------------------------------------------- + + FUNCTION: Gravity model for LaRCsim + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Created by Bruce Jackson on September 25, 1992. + +---------------------------------------------------------------------------- + + DESIGNED BY: Bruce Jackson + + CODED BY: Bruce Jackson + + MAINTAINED BY: Bruce Jackson + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + 940111 Changed include files to "ls_types.h" and + "ls_constants.h" from "ls_eom.h"; also changed DATA types + to SCALAR types. EBJ + + +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:09:56 curt +Initial Flight Gear revision. + + * Revision 1.2 1994/01/11 18:50:35 bjax + * Corrected include files (was ls_eom.h) and DATA types changed + * to SCALARs. EBJ + * + * Revision 1.1 1992/12/30 13:18:46 bjax + * Initial revision + * + +---------------------------------------------------------------------------- + + REFERENCES: Stevens, Brian L.; and Lewis, Frank L.: "Aircraft + Control and Simulation", Wiley and Sons, 1992. + ISBN 0-471- + +---------------------------------------------------------------------------- + + CALLED BY: + +---------------------------------------------------------------------------- + + CALLS TO: + +---------------------------------------------------------------------------- + + INPUTS: + +---------------------------------------------------------------------------- + + OUTPUTS: + +--------------------------------------------------------------------------*/ +#include "ls_types.h" +#include "ls_constants.h" +#include + +#define GM 1.4076431E16 +#define J2 1.08263E-3 + +void ls_gravity( SCALAR radius, SCALAR lat, SCALAR *gravity ) +{ + + SCALAR radius_ratio, rrsq, sinsqlat; + + radius_ratio = radius/EQUATORIAL_RADIUS; + rrsq = radius_ratio*radius_ratio; + sinsqlat = sin(lat)*sin(lat); + *gravity = (GM/(radius*radius)) + *sqrt(2.25*rrsq*rrsq*J2*J2*(5*sinsqlat*sinsqlat -2*sinsqlat + 1) + + 3*rrsq*J2*(1 - 3*sinsqlat) + 1); + +} diff --git a/LaRCsim/ls_init.c b/LaRCsim/ls_init.c new file mode 100644 index 000000000..731f91162 --- /dev/null +++ b/LaRCsim/ls_init.c @@ -0,0 +1,347 @@ +/*************************************************************************** + + TITLE: ls_init.c + +---------------------------------------------------------------------------- + + FUNCTION: Initializes simulation + +---------------------------------------------------------------------------- + + MODULE STATUS: incomplete + +---------------------------------------------------------------------------- + + GENEALOGY: Written 921230 by Bruce Jackson + +---------------------------------------------------------------------------- + + DESIGNED BY: EBJ + + CODED BY: EBJ + + MAINTAINED BY: EBJ + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + 950314 Added get_set, put_set, and init routines. EBJ + + CURRENT RCS HEADER: + +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:09:57 curt +Initial Flight Gear revision. + + * Revision 1.4 1995/03/15 12:15:23 bjax + * Added ls_init_get_set() and ls_init_put_set() and ls_init_init() + * routines. EBJ + * + * Revision 1.3 1994/01/11 19:09:44 bjax + * Fixed header includes. + * + * Revision 1.2 1992/12/30 14:04:53 bjax + * Added call to ls_step(0, 1). + * + * Revision 1.1 92/12/30 14:02:19 bjax + * Initial revision + * + * Revision 1.1 92/12/30 13:21:21 bjax + * Initial revision + * + * Revision 1.3 93/12/31 10:34:11 bjax + * Added $Log marker as well. + * + +---------------------------------------------------------------------------- + + REFERENCES: + +---------------------------------------------------------------------------- + + CALLED BY: + +---------------------------------------------------------------------------- + + CALLS TO: + +---------------------------------------------------------------------------- + + INPUTS: + +---------------------------------------------------------------------------- + + OUTPUTS: + +--------------------------------------------------------------------------*/ +static char rcsid[] = "$Id$"; + +#include +#include +#include "ls_types.h" +#include "ls_sym.h" + +#define MAX_NUMBER_OF_CONTINUOUS_STATES 100 +#define MAX_NUMBER_OF_DISCRETE_STATES 20 +#define HARDWIRED 13 +#define NIL_POINTER 0L + +#define FACILITY_NAME_STRING "init" +#define CURRENT_VERSION 10 + +typedef struct +{ + symbol_rec Symbol; + double value; +} cont_state_rec; + +typedef struct +{ + symbol_rec Symbol; + long value; +} disc_state_rec; + + +extern SCALAR Simtime; + +static int Symbols_loaded = 0; +static int Number_of_Continuous_States = 0; +static int Number_of_Discrete_States = 0; +static cont_state_rec Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ]; +static disc_state_rec Discrete_States[ MAX_NUMBER_OF_DISCRETE_STATES ]; + + +void ls_init_init() +{ + int i, error; + + if (Number_of_Continuous_States == 0) + { + Number_of_Continuous_States = HARDWIRED; + + for (i=0;i bufptr)) + { + bufptr = strtok( 0L, "\n"); + if (bufptr == 0) return 0L; + if (strncasecmp( bufptr, "end", 3) == 0) break; + + sscanf( bufptr, "%s", line ); + if (line[0] != '#') /* ignore comments */ + { + switch (looking_for) + { + case cont_states_header: + { + if (strncasecmp( line, "continuous_states", 17) == 0) + { + n = sscanf( bufptr, "%s%d", line, + &Number_of_Continuous_States ); + if (n != 2) abrt = 1; + looking_for = cont_states; + i = 0; + } + break; + } + case cont_states: + { + n = sscanf( bufptr, "%s%s%le", + Continuous_States[i].Symbol.Mod_Name, + Continuous_States[i].Symbol.Par_Name, + &Continuous_States[i].value ); + if (n != 3) abrt = 1; + Continuous_States[i].Symbol.Addr = NIL_POINTER; + i++; + if (i >= Number_of_Continuous_States) + looking_for = disc_states_header; + break; + } + case disc_states_header: + { + if (strncasecmp( line, "discrete_states", 15) == 0) + { + n = sscanf( bufptr, "%s%d", line, + &Number_of_Discrete_States ); + if (n != 2) abrt = 1; + looking_for = disc_states; + i = 0; + } + break; + } + case disc_states: + { + n = sscanf( bufptr, "%s%s%ld", + Discrete_States[i].Symbol.Mod_Name, + Discrete_States[i].Symbol.Par_Name, + &Discrete_States[i].value ); + if (n != 3) abrt = 1; + Discrete_States[i].Symbol.Addr = NIL_POINTER; + i++; + if (i >= Number_of_Discrete_States) looking_for = done; + } + case done: + { + break; + } + } + + } + } + + Symbols_loaded = !abrt; + + return bufptr; +} + + + +void ls_init_put_set( FILE *fp ) +{ + int i; + + if (fp==0) return; + fprintf(fp, "\n"); + fprintf(fp, "#============================== %s\n", FACILITY_NAME_STRING); + fprintf(fp, "\n"); + fprintf(fp, FACILITY_NAME_STRING); + fprintf(fp, "\n"); + fprintf(fp, "%04d\n", CURRENT_VERSION); + fprintf(fp, " continuous_states: %d\n", Number_of_Continuous_States); + fprintf(fp, "# module parameter value\n"); + for (i=0; i abort works from initial pause + state; added call to ls_unsync() immediately following + first ls_sync() call, if paused (to avoid alarm clock + timeout); moved call to ls_record() into non-paused + multiloop path (was filling buffer with identical data + during pause); put check of paused flag before calling sync + routine ls_pause(); and added call to exit() on termination. + + +$Header$ +$Original log: LaRCsim.c,v $ + * Revision 1.4.1.7 1995/04/07 01:04:37 bjax + * Many changes made to support storage of sim options from run to run, + * as well as restructuring storage buffer sizing and some loop logic + * changes. See the modification log for details. + * + * Revision 1.4.1.6 1995/03/29 16:12:09 bjax + * Added argument to -o switch; changed run loop to pass dt=0 + * if in paused mode. EBj + * + * Revision 1.4.1.5 1995/03/15 12:30:20 bjax + * Set paused flag to non-zero by default; moved 'i' I/O rate flag + * switch to 'o'; made 'i' an initial conditions file switch; added + * null string to ls_get_settings() call so that default settings + * file will be read. EBJ + * + * Revision 1.4.1.4 1995/03/08 12:31:34 bjax + * Added userid retrieval and proper termination of time & date strings. + * + * Revision 1.4.1.3 1995/03/08 12:00:21 bjax + * Moved setting of default options to ls_setdefopts from + * ls_checkopts; rearranged order of ls_get_settings() call + * to between ls_setdefopts and ls_checkopts, so command + * line options will override settings file options. + * EBJ + * + * Revision 1.4.1.2 1995/03/06 18:48:49 bjax + * Added calles to ls_get_settings() and ls_put_settings(); added + * passing of dt and init flags in ls_model(). EBJ + * + * Revision 1.4.1.1 1995/03/03 02:23:08 bjax + * Beta version for LaRCsim, version 1.4 + * + * Revision 1.3.2.7 1995/02/27 20:00:21 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.2.6 1995/02/25 16:52:31 bjax + * Added 'i' option to set I/O iteration rate. EBJ + * + * Revision 1.3.2.5 1995/02/06 19:33:15 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.2.4 1995/02/06 19:30:30 bjax + * Oops, should really compile these before checking in. Fixed capitailzation of + * Initialize in ls_loop parameter. + * + * Revision 1.3.2.3 1995/02/06 19:25:44 bjax + * Moved main simulation loop into subroutine ls_loop. EBJ + * + * Revision 1.3.2.2 1994/05/20 21:46:45 bjax + * A little better logic on checking for option arguments. + * + * Revision 1.3.2.1 1994/05/20 19:29:51 bjax + * Added options arguments to command line. + * + * Revision 1.3.1.16 1994/05/17 15:08:45 bjax + * Corrected so that full name to directyr and file is saved + * in new global variable "fullname"; this allows symbol table + * to be extracted when in another default directory. + * + * Revision 1.3.1.15 1994/05/17 14:50:24 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.14 1994/05/17 14:50:23 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.13 1994/05/17 14:50:21 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.12 1994/05/17 14:50:20 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.11 1994/05/17 13:56:24 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.10 1994/05/17 13:23:03 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.9 1994/05/17 13:20:03 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.8 1994/05/17 13:19:23 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.7 1994/05/17 13:18:29 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.6 1994/05/17 13:16:30 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.5 1994/05/17 13:03:44 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.4 1994/05/17 13:03:38 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.3 1994/05/17 12:49:08 bjax + * Rebuilt LaRCsim + * + * Revision 1.3.1.2 1994/05/17 12:48:45 bjax + * *** empty log message *** + * + * Revision 1.3.1.1 1994/05/13 20:39:17 bjax + * Top of 1.3 branch. + * + * Revision 1.2 1994/05/13 19:51:50 bjax + * Skip rev + * + +---------------------------------------------------------------------------- + + REFERENCES: + +---------------------------------------------------------------------------- + + CALLED BY: + +---------------------------------------------------------------------------- + + CALLS TO: + +---------------------------------------------------------------------------- + + INPUTS: + +---------------------------------------------------------------------------- + + OUTPUTS: + +--------------------------------------------------------------------------*/ + +#include "ls_interface.h" + +#include "ls_types.h" +#include "ls_constants.h" +#include "ls_generic.h" +#include "ls_sim_control.h" +#include "ls_cockpit.h" +/* #include "ls_tape.h" */ +#ifndef linux +# include +#endif +#include +#include +#include +#include +#include +#include + +/* global variable declarations */ + +/* TAPE *Tape; */ +GENERIC generic_; +SIM_CONTROL sim_control_; +COCKPIT cockpit_; + +SCALAR Simtime; + +/* #define DEFAULT_TERM_UPDATE_HZ 20 */ /* original value */ +#define DEFAULT_TERM_UPDATE_HZ 20 +#define DEFAULT_MODEL_HZ 120 +#define DEFAULT_END_TIME 3600. +#define DEFAULT_SAVE_SPACING 8 +#define DEFAULT_WRITE_SPACING 1 +#define MAX_FILE_NAME_LENGTH 80 + +/* global variables */ + +char *progname; +char *fullname; + +/* file variables - default simulation settings */ + +static double model_dt; +static double speedup; +static char asc1name[MAX_FILE_NAME_LENGTH] = "run.asc1"; +static char tabname[MAX_FILE_NAME_LENGTH] = "run.dat"; +static char fltname[MAX_FILE_NAME_LENGTH] = "run.flt"; +static char matname[MAX_FILE_NAME_LENGTH] = "run.m"; + + + +void ls_stamp() +{ + char rcsid[] = "$Id$"; + char revid[] = "$Revision$"; + char dateid[] = "$Date$"; + struct tm *nowtime; + time_t nowtime_t; + long date; + + /* report version of LaRCsim*/ + printf("\nLaRCsim %s, %s\n\n", revid, dateid); + + nowtime_t = time( 0 ); + nowtime = localtime( &nowtime_t ); /* set fields to correct time values */ + date = (nowtime->tm_year)*10000 + + (nowtime->tm_mon + 1)*100 + + (nowtime->tm_mday); + sprintf(sim_control_.date_string, "%06d\0", date); + sprintf(sim_control_.time_stamp, "%02d:%02d:%02d\0", + nowtime->tm_hour, nowtime->tm_min, nowtime->tm_sec); + cuserid( sim_control_.userid ); /* set up user id */ + + return; +} + +void ls_setdefopts() +{ + /* set default values for most options */ + + sim_control_.debug = 0; /* change to non-zero if in dbx! */ + sim_control_.vision = 0; + sim_control_.write_av = 0; /* write Agile-Vu '.flt' file */ + sim_control_.write_mat = 0; /* write matrix-x/matlab script */ + sim_control_.write_tab = 0; /* write tab delim. history file */ + sim_control_.write_asc1 = 0; /* write GetData file */ + sim_control_.sim_type = GLmouse; /* hook up to mouse */ + sim_control_.save_spacing = DEFAULT_SAVE_SPACING; + /* interpolation on recording */ + sim_control_.write_spacing = DEFAULT_WRITE_SPACING; + /* interpolation on output */ + sim_control_.end_time = DEFAULT_END_TIME; + sim_control_.model_hz = DEFAULT_MODEL_HZ; + sim_control_.term_update_hz = DEFAULT_TERM_UPDATE_HZ; + sim_control_.time_slices = DEFAULT_END_TIME * DEFAULT_MODEL_HZ / + DEFAULT_SAVE_SPACING; + sim_control_.paused = 0; + + speedup = 1.0; +} + + +/* return result codes from ls_checkopts */ + +#define OPT_OK 0 +#define OPT_ERR 1 + +extern char *optarg; +extern int optind; + +int ls_checkopts(argc, argv) /* check and set options flags */ + int argc; + char *argv[]; + { + int c; + int opt_err = 0; + int mod_end_time = 0; + int mod_buf_size = 0; + float buffer_time, data_rate; + + /* set default values */ + + buffer_time = sim_control_.time_slices * sim_control_.save_spacing / + sim_control_.model_hz; + data_rate = sim_control_.model_hz / sim_control_.save_spacing; + + while ((c = getopt(argc, argv, "Aa:b:de:f:hi:kmo:r:s:t:x:")) != EOF) + switch (c) { + case 'A': + if (sim_control_.sim_type == GLmouse) + { + fprintf(stderr, "Cannot specify both keyboard (k) and ACES (A) cockpits option\n"); + fprintf(stderr, "Keyboard operation assumed.\n"); + break; + } + sim_control_.sim_type = cockpit; + break; + case 'a': + sim_control_.write_av = 1; + if (optarg != NULL) + if (*optarg != '-') + strncpy(fltname, optarg, MAX_FILE_NAME_LENGTH); + else + optind--; + break; + case 'b': + buffer_time = atof(optarg); + if (buffer_time <= 0.) opt_err = -1; + mod_buf_size++; + break; + case 'd': + sim_control_.debug = 1; + break; + case 'e': + sim_control_.end_time = atof(optarg); + mod_end_time++; + break; + case 'f': + sim_control_.model_hz = atof(optarg); + break; + case 'h': + opt_err = 1; + break; + case 'i': + /* ls_get_settings( optarg ); */ + break; + case 'k': + sim_control_.sim_type = GLmouse; + break; + case 'm': + sim_control_.vision = 1; + break; + case 'o': + sim_control_.term_update_hz = atof(optarg); + if (sim_control_.term_update_hz <= 0.) opt_err = 1; + break; + case 'r': + sim_control_.write_mat = 1; + if (optarg != NULL) + if (*optarg != '-') + strncpy(matname, optarg, MAX_FILE_NAME_LENGTH); + else + optind--; + break; + case 's': + data_rate = atof(optarg); + if (data_rate <= 0.) opt_err = -1; + break; + case 't': + sim_control_.write_tab = 1; + if (optarg != NULL) + if (*optarg != '-') + strncpy(tabname, optarg, MAX_FILE_NAME_LENGTH); + else + optind--; + break; + case 'x': + sim_control_.write_asc1 = 1; + if (optarg != NULL) + if (*optarg != '-') + strncpy(asc1name, optarg, MAX_FILE_NAME_LENGTH); + else + optind--; + break; + default: + opt_err = 1; + + } + + if (opt_err) + { + fprintf(stderr, "Usage: %s [-options]\n", progname); + fprintf(stderr, "\n"); + fprintf(stderr, " where [-options] is zero or more of the following:\n"); + fprintf(stderr, "\n"); + fprintf(stderr, " [A|k] Run mode: [A]CES cockpit [default]\n"); + fprintf(stderr, " or [k]eyboard\n"); + fprintf(stderr, "\n"); + fprintf(stderr, " [i ] [i]nitial conditions filename\n"); + fprintf(stderr, "\n"); + fprintf(stderr, " [f ] Iteration rate [f]requency, Hz (default is %5.2f Hz)\n", + sim_control_.model_hz); + fprintf(stderr, "\n"); + fprintf(stderr, " [o ] Display [o]utput frequency, Hz (default is %5.2f Hz)\n", + sim_control_.term_update_hz); + fprintf(stderr, "\n"); + fprintf(stderr, " [s ] Data storage frequency, Hz (default is %5.2f Hz)\n", + data_rate); + fprintf(stderr, "\n"); + fprintf(stderr, " [e ] [e]nd time in seconds (default %5.1f seconds)\n", + sim_control_.end_time); + fprintf(stderr, "\n"); + fprintf(stderr, " [b ] circular time history storage [b]uffer size, in seconds \n"); + fprintf(stderr, " (default %5.1f seconds) (normally same as end time)\n", + sim_control_.time_slices*sim_control_.save_spacing/ + sim_control_.model_hz); + fprintf(stderr, "\n"); + fprintf(stderr, " [atxr []] Output: [a]gile-vu (default name: %s )\n", fltname); + fprintf(stderr, " and/or [t]ab delimited ( '' name: %s )\n", tabname); + fprintf(stderr, " and/or [x]plot (default name: %s)\n", asc1name); + fprintf(stderr, " and/or mat[r]ix script ( '' name: %s )\n", matname); + fprintf(stderr, "\n"); + return OPT_ERR; + } + +/* calculate additional controls */ + + sim_control_.save_spacing = (int) (0.5 + sim_control_.model_hz / data_rate); + if (sim_control_.save_spacing < 1) sim_control_.save_spacing = 1; + + sim_control_.time_slices = buffer_time * sim_control_.model_hz / + sim_control_.save_spacing; + if (sim_control_.time_slices < 2) sim_control_.time_slices = 2; + + return OPT_OK; + } + + +void ls_loop( dt, initialize ) + +SCALAR dt; +int initialize; + +{ + /* printf (" In ls_loop()\n"); */ + ls_step( dt, initialize ); + /* if (sim_control_.sim_type == cockpit ) ls_ACES(); */ + ls_aux(); + ls_model( dt, initialize ); + ls_accel(); +} + + + +int ls_cockpit() { + + sim_control_.paused = 0; + + Throttle_pct = 0.85; + + /* printf("Mach = %.2f ", Mach_number); + printf("%.4f,%.4f,%.2f ", Latitude, Longitude, Altitude); + printf("%.2f,%.2f,%.2f\n", Phi, Theta, Psi); */ + +} + + +/* Initialize the LaRCsim flight model, dt is the time increment for + each subsequent iteration through the EOM */ +int fgLaRCsimInit(double dt) { + + model_dt = dt; + + ls_setdefopts(); /* set default options */ + + /* Number_of_Continuous_States = 22; */ + + generic_.geodetic_position_v[0] = 2.793445E-05; + generic_.geodetic_position_v[1] = 3.262070E-07; + generic_.geodetic_position_v[2] = 3.758099E+00; + generic_.v_local_v[0] = 7.287719E+00; + generic_.v_local_v[1] = 1.521770E+03; + generic_.v_local_v[2] = -1.265722E-05; + generic_.euler_angles_v[0] = -2.658474E-06; + generic_.euler_angles_v[1] = 7.401790E-03; + generic_.euler_angles_v[2] = 1.391358E-03; + generic_.omega_body_v[0] = 7.206685E-05; + generic_.omega_body_v[1] = 0.000000E+00; + generic_.omega_body_v[2] = 9.492658E-05; + generic_.earth_position_angle = 0.000000E+00; + generic_.mass = 8.547270E+01; + generic_.i_xx = 1.048000E+03; + generic_.i_yy = 3.000000E+03; + generic_.i_zz = 3.530000E+03; + generic_.i_xz = 0.000000E+00; + generic_.d_cg_rp_body_v[0] = 0.000000E+00; + generic_.d_cg_rp_body_v[1] = 0.000000E+00; + generic_.d_cg_rp_body_v[2] = 0.000000E+00; + + ls_stamp(); /* ID stamp; record time and date of run */ + + if (speedup == 0.0) { + fprintf(stderr, "%s: Cannot run with speedup of 0.\n", progname); + return 1; + } + + ls_init(); + + if (speedup > 0) { + /* Initialize (get) cockpit (controls) settings */ + ls_cockpit(); + } + + return(1); +} + + +/* Run an iteration of the EOM (equations of motion) */ +int fgLaRCsimUpdate(int multiloop) { + int i; + + if (speedup > 0) { + ls_cockpit(); + } + + for ( i = 0; i < multiloop; i++ ) { + ls_loop( model_dt, 0); + } + + return(1); +} + + +/* Flight Gear Modification Log + * + * $Log$ + * Revision 1.1 1997/05/29 00:09:57 curt + * Initial Flight Gear revision. + * + */ diff --git a/LaRCsim/ls_interface.h b/LaRCsim/ls_interface.h new file mode 100644 index 000000000..60d955015 --- /dev/null +++ b/LaRCsim/ls_interface.h @@ -0,0 +1,45 @@ +/************************************************************************** + * ls_interface.h -- interface to the "LaRCsim" flight model + * + * Written by Curtis Olson, started May 1997. + * + * Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com + * + * 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$ + * (Log is kept at end of this file) + **************************************************************************/ + + +#ifndef LS_INTERFACE_H +#define LS_INTERFACE_H + + +/* reset flight params to a specific position */ +int fgLaRCsimInit(double dt); + +/* update position based on inputs, positions, velocities, etc. */ +int fgLaRCsimUpdate(); + + +#endif LS_INTERFACE_H + + +/* $Log$ +/* Revision 1.1 1997/05/29 00:09:58 curt +/* Initial Flight Gear revision. +/* + */ diff --git a/LaRCsim/ls_model.c b/LaRCsim/ls_model.c new file mode 100644 index 000000000..ff8fd20f1 --- /dev/null +++ b/LaRCsim/ls_model.c @@ -0,0 +1,86 @@ +/*************************************************************************** + + TITLE: ls_model() + +---------------------------------------------------------------------------- + + FUNCTION: Model loop executive + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Created 15 October 1992 as part of LaRCSIM project + by Bruce Jackson. + +---------------------------------------------------------------------------- + + DESIGNED BY: Bruce Jackson + + CODED BY: Bruce Jackson + + MAINTAINED BY: maintainer + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + 950306 Added parameters to call: dt, which is the step size + to be taken this loop (caution: may vary from call to call) + and Initialize, which if non-zero, implies an initialization + is requested. EBJ + + CURRENT RCS HEADER INFO: +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:09:58 curt +Initial Flight Gear revision. + + * Revision 1.3 1995/03/06 18:49:46 bjax + * Added dt and initialize flag parameters to subroutine calls. This will + * support trim routine (to allow single throttle setting to drive + * all four throttle positions, for example, if initialize is TRUE). + * + * Revision 1.2 1993/03/10 06:38:09 bjax + * Added additional calls: inertias() and subsystems()... EBJ + * + * Revision 1.1 92/12/30 13:19:08 bjax + * Initial revision + * + +---------------------------------------------------------------------------- + + REFERENCES: + +---------------------------------------------------------------------------- + + CALLED BY: ls_step (in initialization), ls_loop (planned) + +---------------------------------------------------------------------------- + + CALLS TO: aero(), engine(), gear() + +---------------------------------------------------------------------------- + + INPUTS: + +---------------------------------------------------------------------------- + + OUTPUTS: + +--------------------------------------------------------------------------*/ +#include "ls_types.h" + +void ls_model( SCALAR dt, int Initialize ) + +{ + inertias( dt, Initialize ); + subsystems( dt, Initialize ); + aero( dt, Initialize ); + engine( dt, Initialize ); + gear( dt, Initialize ); +} diff --git a/LaRCsim/ls_sim_control.h b/LaRCsim/ls_sim_control.h new file mode 100644 index 000000000..fbbe2a03d --- /dev/null +++ b/LaRCsim/ls_sim_control.h @@ -0,0 +1,111 @@ +/*************************************************************************** + + TITLE: ls_sim_control.h + +---------------------------------------------------------------------------- + + FUNCTION: LaRCSim simulation control parameters header file + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Created 18 DEC 1993 by Bruce Jackson + +---------------------------------------------------------------------------- + + DESIGNED BY: B. Jackson + + CODED BY: B. Jackson + + MAINTAINED BY: guess who + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + 940204 Added "overrun" flag to indicate non-real-time frame. + 940210 Added "vision" flag to indicate use of shared memory. + 940513 Added "max_tape_channels" and "max_time_slices" EBJ + 950308 Increased size of time_stamp and date_string to include + terminating null char. EBJ + 950314 Addedf "paused" flag to make this global (was local to + ls_cockpit routine). EBJ + 950406 Removed tape_channels parameter, and added end_time, model_hz, + and term_update_hz parameters. EBJ + +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:09:59 curt +Initial Flight Gear revision. + + * Revision 1.11 1995/04/07 01:39:09 bjax + * Removed tape_channels and added end_time, model_hz, and term_update_hz. + * + * Revision 1.10 1995/03/15 12:33:29 bjax + * Added 'paused' flag. + * + * Revision 1.9 1995/03/08 12:34:21 bjax + * Increased size of date_string and time_stamp by 1 to include terminating null; + * added userid field and include of stdio.h. EBJ + * + * Revision 1.8 1994/05/13 20:41:43 bjax + * Increased size of time_stamp to 8 chars to allow for colons. + * Added fields "tape_channels" and "time_slices" to allow user to change. + * + * Revision 1.7 1994/05/10 15:18:49 bjax + * Modified write_cmp2 flag to write_asc1 flag, since XPLOT 4.00 doesn't + * support cmp2. Also added RCS header and log entries in header. + * + + +--------------------------------------------------------------------------*/ +#include + +#ifndef SIM_CONTROL + +typedef struct { + + enum { batch, terminal, GLmouse, cockpit } sim_type; + char simname[64]; /* name of simulation */ + int run_number; /* run number of this session */ + char date_string[7]; /* like "931220" */ + char time_stamp[9]; /* like "13:00:00" */ + char userid[L_cuserid]; /* who is running this sim */ + long time_slices; /* number of points that can be recorded (circ buff) */ + int write_av; /* will be writing out an Agile_VU file after run */ + int write_mat; /* will be writing out a matrix script of session */ + int write_tab; /* will be writing out a tab-delimited time history */ + int write_asc1; /* will be writing out a GetData ASCII 1 file */ + int save_spacing; /* spacing between data points when recording + data to memory; 0 = every point, 1 = every + other point; 2 = every fourth point, etc. */ + int write_spacing; /* spacing between data points when writing + output files; 0 = every point, 1 = every + other point; 2 = every fourth point, etc. */ + int overrun; /* indicates, if non-zero, a frame overrun + occurred in the previous frame. Suitable for + setting a display flag or writing an error + message. */ + int vision; /* indicates, if non-zero, marriage to LaRC VISION + software (developed A. Dare and J. Burley of the + former Cockpit Technologies Branch) */ + int debug; /* indicates, if non-zero, to operate in debug mode + which implies disable double-buffering and synch. + attempts to avoid errors */ + int paused; /* indicates simulation is paused */ + float end_time; /* end of simulation run value */ + float model_hz; /* current inner loop frame rate */ + float term_update_hz; /* current terminal refresh frequency */ + +} SIM_CONTROL; + +extern SIM_CONTROL sim_control_; + +#endif + +/*------------------------ end of ls_sim_control.h ----------------------*/ diff --git a/LaRCsim/ls_step.c b/LaRCsim/ls_step.c new file mode 100644 index 000000000..5f618f202 --- /dev/null +++ b/LaRCsim/ls_step.c @@ -0,0 +1,339 @@ +/*************************************************************************** + + TITLE: ls_step + +---------------------------------------------------------------------------- + + FUNCTION: Integration routine for equations of motion + (vehicle states) + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Written 920802 by Bruce Jackson. Based upon equations + given in reference [1] and a Matrix-X/System Build block + diagram model of equations of motion coded by David Raney + at NASA-Langley in June of 1992. + +---------------------------------------------------------------------------- + + DESIGNED BY: Bruce Jackson + + CODED BY: Bruce Jackson + + MAINTAINED BY: + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + 921223 Modified calculation of Phi and Psi to use the "atan2" routine + rather than the "atan" to allow full circular angles. + "atan" limits to +/- pi/2. EBJ + + 940111 Changed from oldstyle include file ls_eom.h; also changed + from DATA to SCALAR type. EBJ + + 950207 Initialized Alpha_dot and Beta_dot to zero on first pass; calculated + thereafter. EBJ + + 950224 Added logic to avoid adding additional increment to V_east + in case V_east already accounts for rotating earth. + EBJ + + CURRENT RCS HEADER: + +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:09:59 curt +Initial Flight Gear revision. + + * Revision 1.5 1995/03/02 20:24:13 bjax + * Added logic to avoid adding additional increment to V_east + * in case V_east already accounts for rotating earth. EBJ + * + * Revision 1.4 1995/02/07 20:52:21 bjax + * Added initialization of Alpha_dot and Beta_dot to zero on first + * pass; they get calculated by ls_aux on next pass... EBJ + * + * Revision 1.3 1994/01/11 19:01:12 bjax + * Changed from DATA to SCALAR type; also fixed header files (was ls_eom.h) + * + * Revision 1.2 1993/06/02 15:03:09 bjax + * Moved initialization of geocentric position to subroutine ls_geod_to_geoc. + * + * Revision 1.1 92/12/30 13:16:11 bjax + * Initial revision + * + +---------------------------------------------------------------------------- + + REFERENCES: + + [ 1] McFarland, Richard E.: "A Standard Kinematic Model + for Flight Simulation at NASA-Ames", NASA CR-2497, + January 1975 + + [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos- + pheric and Space Flight Vehicle Coordinate Systems", + February 1992 + + +---------------------------------------------------------------------------- + + CALLED BY: + +---------------------------------------------------------------------------- + + CALLS TO: None. + +---------------------------------------------------------------------------- + + INPUTS: State derivatives + +---------------------------------------------------------------------------- + + OUTPUTS: States + +--------------------------------------------------------------------------*/ + +#include "ls_types.h" +#include "ls_constants.h" +#include "ls_generic.h" +/* #include "ls_sim_control.h" */ +#include + +extern SCALAR Simtime; /* defined in ls_main.c */ + +void ls_step( dt, Initialize ) + +SCALAR dt; +int Initialize; + +{ + static int inited = 0; + SCALAR dth; + static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past; + static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past; + static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past; + SCALAR p_local_in_body, q_local_in_body, r_local_in_body; + SCALAR epsilon, inv_eps, local_gnd_veast; + SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3; + static SCALAR e_0, e_1, e_2, e_3; + static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past; + +/* I N I T I A L I Z A T I O N */ + + + if ( (inited == 0) || (Initialize != 0) ) + { +/* Set past values to zero */ + v_dot_north_past = v_dot_east_past = v_dot_down_past = 0; + latitude_dot_past = longitude_dot_past = radius_dot_past = 0; + p_dot_body_past = q_dot_body_past = r_dot_body_past = 0; + e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0; + +/* Initialize geocentric position from geodetic latitude and altitude */ + + ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric); + Earth_position_angle = 0; + Lon_geocentric = Longitude; + Radius_to_vehicle = Altitude + Sea_level_radius; + +/* Correct eastward velocity to account for earths' rotation, if necessary */ + + local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric); + if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast ) + V_east = V_east + local_gnd_veast; + +/* Initialize quaternions and transformation matrix from Euler angles */ + + e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5) + + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5); + e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5) + - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5); + e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5) + + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5); + e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5) + + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5); + T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3; + T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3); + T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2); + T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3); + T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3; + T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1); + T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2); + T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1); + T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3; + +/* Calculate local gravitation acceleration */ + + ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity ); + +/* Initialize vehicle model */ + + ls_aux(); + ls_model(); + +/* Calculate initial accelerations */ + + ls_accel(); + +/* Initialize auxiliary variables */ + + ls_aux(); + Alpha_dot = 0.; + Beta_dot = 0.; + +/* set flag; disable integrators */ + + inited = -1; + dt = 0; + + } + +/* Update time */ + + dth = 0.5*dt; + Simtime = Simtime + dt; + +/* L I N E A R V E L O C I T I E S */ + +/* Integrate linear accelerations to get velocities */ +/* Using predictive Adams-Bashford algorithm */ + + V_north = V_north + dth*(3*V_dot_north - v_dot_north_past); + V_east = V_east + dth*(3*V_dot_east - v_dot_east_past ); + V_down = V_down + dth*(3*V_dot_down - v_dot_down_past ); + +/* record past states */ + + v_dot_north_past = V_dot_north; + v_dot_east_past = V_dot_east; + v_dot_down_past = V_dot_down; + +/* Calculate trajectory rate (geocentric coordinates) */ + + if (cos(Lat_geocentric) != 0) + Longitude_dot = V_east/(Radius_to_vehicle*cos(Lat_geocentric)); + + Latitude_dot = V_north/Radius_to_vehicle; + Radius_dot = -V_down; + +/* A N G U L A R V E L O C I T I E S A N D P O S I T I O N S */ + +/* Integrate rotational accelerations to get velocities */ + + P_body = P_body + dth*(3*P_dot_body - p_dot_body_past); + Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past); + R_body = R_body + dth*(3*R_dot_body - r_dot_body_past); + +/* Save past states */ + + p_dot_body_past = P_dot_body; + q_dot_body_past = Q_dot_body; + r_dot_body_past = R_dot_body; + +/* Calculate local axis frame rates due to travel over curved earth */ + + P_local = V_east/Radius_to_vehicle; + Q_local = -V_north/Radius_to_vehicle; + R_local = -V_east*tan(Lat_geocentric)/Radius_to_vehicle; + +/* Transform local axis frame rates to body axis rates */ + + p_local_in_body = T_local_to_body_11*P_local + T_local_to_body_12*Q_local + T_local_to_body_13*R_local; + q_local_in_body = T_local_to_body_21*P_local + T_local_to_body_22*Q_local + T_local_to_body_23*R_local; + r_local_in_body = T_local_to_body_31*P_local + T_local_to_body_32*Q_local + T_local_to_body_33*R_local; + +/* Calculate total angular rates in body axis */ + + P_total = P_body - p_local_in_body; + Q_total = Q_body - q_local_in_body; + R_total = R_body - r_local_in_body; + +/* Transform to quaternion rates (see Appendix E in [2]) */ + + e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 ); + e_dot_1 = 0.5*( P_total*e_0 - Q_total*e_3 + R_total*e_2 ); + e_dot_2 = 0.5*( P_total*e_3 + Q_total*e_0 - R_total*e_1 ); + e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 ); + +/* Integrate using trapezoidal as before */ + + e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past); + e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past); + e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past); + e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past); + +/* calculate orthagonality correction - scale quaternion to unity length */ + + epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3); + inv_eps = 1/epsilon; + + e_0 = inv_eps*e_0; + e_1 = inv_eps*e_1; + e_2 = inv_eps*e_2; + e_3 = inv_eps*e_3; + +/* Save past values */ + + e_dot_0_past = e_dot_0; + e_dot_1_past = e_dot_1; + e_dot_2_past = e_dot_2; + e_dot_3_past = e_dot_3; + +/* Update local to body transformation matrix */ + + T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3; + T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3); + T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2); + T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3); + T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3; + T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1); + T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2); + T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1); + T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3; + +/* Calculate Euler angles */ + + Theta = asin( -T_local_to_body_13 ); + + if( T_local_to_body_11 == 0 ) + Psi = 0; + else + Psi = atan2( T_local_to_body_12, T_local_to_body_11 ); + + if( T_local_to_body_33 == 0 ) + Phi = 0; + else + Phi = atan2( T_local_to_body_23, T_local_to_body_33 ); + +/* Resolve Psi to 0 - 359.9999 */ + + if (Psi < 0 ) Psi = Psi + 2*PI; + +/* L I N E A R P O S I T I O N S */ + +/* Trapezoidal acceleration for position */ + + Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past ); + Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past); + Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past ); + Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH; + +/* Save past values */ + + latitude_dot_past = Latitude_dot; + longitude_dot_past = Longitude_dot; + radius_dot_past = Radius_dot; + +/* end of ls_step */ +} +/*************************************************************************/ + diff --git a/LaRCsim/ls_sym.h b/LaRCsim/ls_sym.h new file mode 100644 index 000000000..a5c53b12e --- /dev/null +++ b/LaRCsim/ls_sym.h @@ -0,0 +1,155 @@ +/*************************************************************************** + + TITLE: ls_sym.h + +---------------------------------------------------------------------------- + + FUNCTION: Header file for symbol table routines + +---------------------------------------------------------------------------- + + MODULE STATUS: production + +---------------------------------------------------------------------------- + + GENEALOGY: Created 930629 by E. B. Jackson + +---------------------------------------------------------------------------- + + DESIGNED BY: Bruce Jackson + + CODED BY: same + + MAINTAINED BY: + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + 950227 Added header and declarations for ls_print_findsym_error(), + ls_get_double(), and ls_get_double() routines. EBJ + + 950302 Added structure for symbol description. EBJ + + 950306 Added ls_get_sym_val() and ls_set_sym_val() routines. + This is now the production version. EBJ + + CURRENT RCS HEADER: + +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:10:00 curt +Initial Flight Gear revision. + + * Revision 1.9 1995/03/07 12:52:33 bjax + * This production version now specified ls_get_sym_val() and ls_put_sym_val(). + * + * Revision 1.6.1.2 1995/03/06 18:45:41 bjax + * Added def'n of ls_get_sym_val() and ls_set_sym_val(); changed symbol_rec + * Addr field from void * to char *. + * EBJ + * + * Revision 1.6.1.1 1995/03/03 01:17:44 bjax + * Experimental version with just ls_get_double and ls_set_double() routines. + * + * Revision 1.6 1995/02/27 19:50:49 bjax + * Added header and declarations for ls_print_findsym_error(), + * ls_get_double(), and ls_set_double(). EBJ + * + +---------------------------------------------------------------------------- + + REFERENCES: + +---------------------------------------------------------------------------- + + CALLED BY: + +---------------------------------------------------------------------------- + + CALLS TO: + +---------------------------------------------------------------------------- + + INPUTS: + +---------------------------------------------------------------------------- + + OUTPUTS: + +--------------------------------------------------------------------------*/ + +/* Return codes */ + +#define SYM_NOT_LOADED -2 +#define SYM_UNEXPECTED_ERR -1 +#define SYM_OK 0 +#define SYM_OPEN_ERR 1 +#define SYM_NO_SYMS 2 +#define SYM_MOD_NOT_FOUND 3 +#define SYM_VAR_NOT_FOUND 4 +#define SYM_NOT_SCALAR 5 +#define SYM_NOT_STATIC 6 +#define SYM_MEMORY_ERR 7 +#define SYM_UNMATCHED_PAREN 8 +#define SYM_BAD_SYNTAX 9 +#define SYM_INDEX_BOUNDS_ERR 10 + +typedef enum {Unknown, Char, UChar, SHint, USHint, Sint, Uint, Slng, Ulng, flt, dbl} vartype; + +typedef char SYMBOL_NAME[64]; +typedef vartype SYMBOL_TYPE; + + + +typedef struct +{ + SYMBOL_NAME Mod_Name; + SYMBOL_NAME Par_Name; + SYMBOL_TYPE Par_Type; + SYMBOL_NAME Alias; + char *Addr; +} symbol_rec; + + +extern int ls_findsym( const char *modname, const char *varname, + char **addr, vartype *vtype ); + +extern void ls_print_findsym_error(int result, + char *mod_name, + char *var_name); + +extern double ls_get_double(vartype sym_type, void *addr ); + +extern void ls_set_double(vartype sym_type, void *addr, double value ); + +extern double ls_get_sym_val( symbol_rec *symrec, int *error ); + + /* This routine attempts to return the present value of the symbol + described in symbol_rec. If Addr is non-zero, the value of that + location, interpreted as type double, will be returned. If Addr + is zero, and Mod_Name and Par_Name are both not null strings, + the ls_findsym() routine is used to try to obtain the address + by looking at debugger symbol tables in the executable image, and + the value of the double contained at that address is returned, + and the symbol record is updated to contain the address of that + symbol. If an error is discovered, 'error' will be non-zero and + and error message is printed on stderr. */ + +extern void ls_set_sym_val( symbol_rec *symrec, double value ); + + /* This routine sets the value of a double at the location pointed + to by the symbol_rec's Addr field, if Addr is non-zero. If Addr + is zero, and Mod_Name and Par_Name are both not null strings, + the ls_findsym() routine is used to try to obtain the address + by looking at debugger symbol tables in the executable image, and + the value of the double contained at that address is returned, + and the symbol record is updated to contain the address of that + symbol. If an error is discovered, 'error' will be non-zero and + and error message is printed on stderr. */ + + + + diff --git a/LaRCsim/ls_sync.c b/LaRCsim/ls_sync.c new file mode 100644 index 000000000..837df3646 --- /dev/null +++ b/LaRCsim/ls_sync.c @@ -0,0 +1,187 @@ +/*************************************************************************** + + TITLE: ls_sync.c + +---------------------------------------------------------------------------- + + FUNCTION: Real-time synchronization routines for LaRCSIM + +---------------------------------------------------------------------------- + + MODULE STATUS: Developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Written 921229 by Bruce Jackson + +---------------------------------------------------------------------------- + + DESIGNED BY: EBJ + + CODED BY: EBJ + + MAINTAINED BY: EBJ + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + + 930104 Added ls_resync() call to avoid having to pass DT as a + global variable. EBJ + 940204 Added calculation of sim_control variable overrun to + indicate a frame overrun has occurred. EBJ + 940506 Added support for sim_control_.debug flag, which disables + synchronization (there is still a local dbg flag that + enables synch error logging) EBJ + + CURRENT RCS HEADER: + +$Header$ +$Log$ +Revision 1.1 1997/05/29 00:10:00 curt +Initial Flight Gear revision. + + * Revision 1.7 1994/05/06 15:34:54 bjax + * Removed "freerun" variable, and substituted sim_control_.debug flag. + * + * Revision 1.6 1994/02/16 13:01:22 bjax + * Added logic to signal frame overrun; corrected syntax on ls_catch call + * (old style may be BSD format). EBJ + * + * Revision 1.5 1993/07/30 18:33:14 bjax + * Added 'dt' parameter to call to ls_sync from ls_resync routine. + * + * Revision 1.4 1993/03/15 14:56:13 bjax + * Removed call to ls_pause; this should be done in cockpit routine. + * + * Revision 1.3 93/03/13 20:34:09 bjax + * Modified to allow for sync times longer than a second; added ls_pause() EBJ + * + * Revision 1.2 93/01/06 09:50:47 bjax + * Added ls_resync() function. + * + * Revision 1.1 92/12/30 13:19:51 bjax + * Initial revision + * + * Revision 1.3 93/12/31 10:34:11 bjax + * Added $Log marker as well. + * + +---------------------------------------------------------------------------- + + REFERENCES: + +---------------------------------------------------------------------------- + + CALLED BY: + +---------------------------------------------------------------------------- + + CALLS TO: + +---------------------------------------------------------------------------- + + INPUTS: + +---------------------------------------------------------------------------- + + OUTPUTS: + +--------------------------------------------------------------------------*/ +#include +#include +#include +#include "ls_types.h" +#include "ls_sim_control.h" + + +extern SCALAR Simtime; + +/* give the time interval data structure FILE visibility */ + +static struct itimerval t, ot; + +static int dbug = 0; + +/*void ls_catch( sig, code, sc) /* signal handler */ +/*int sig; +int code; +struct sigcontext *sc;*/ +void ls_catch() +{ + static DATA lastSimtime = -99.9; + + /* printf("In ls_catch()\n"); */ + + /*if (lastSimtime == Simtime) fprintf(stderr, "Overrun.\n"); */ + if (dbug) printf("ls_catch called\n"); + sim_control_.overrun = (lastSimtime == Simtime); + lastSimtime = Simtime; + signal(SIGALRM, ls_catch); +} + +void ls_sync(dt) +float dt; + +/* this routine syncs up the interval timer for a new dt value */ +{ + int terr; + int isec; + float usec; + + if (sim_control_.debug!=0) return; + + isec = (int) dt; + usec = 1000000* (dt - (float) isec); + + t.it_interval.tv_sec = isec; + t.it_interval.tv_usec = usec; + t.it_value.tv_sec = isec; + t.it_value.tv_usec = usec; + if (dbug) printf("ls_sync called\n"); + ls_catch(); /* set up for SIGALRM signal catch */ + terr = setitimer( ITIMER_REAL, &t, &ot ); + if (terr) perror("Error returned from setitimer"); +} + +void ls_unsync() +/* this routine unsyncs the interval timer */ +{ + int terr; + + if (sim_control_.debug!=0) return; + t.it_interval.tv_sec = 0; + t.it_interval.tv_usec = 0; + t.it_value.tv_sec = 0; + t.it_value.tv_usec = 0; +if (dbug) printf("ls_unsync called\n"); + + terr = setitimer( ITIMER_REAL, &t, &ot ); + if (terr) perror("Error returned from setitimer"); + +} + +void ls_resync() +/* this routine resynchronizes the interval timer to the old + interrupt period, stored in struct ot by a previous call + to ls_unsync(). */ +{ + float dt; + + if (sim_control_.debug!=0) return; + if (dbug) printf("ls_resync called\n"); + dt = ((float) ot.it_interval.tv_usec)/1000000. + + ((float) ot.it_interval.tv_sec); + ls_sync(dt); +} + +void ls_pause() +/* this routine waits for the next interrupt */ +{ + if (sim_control_.debug!=0) return; + if (dbug) printf("ls_pause called\n"); + pause(); +} + diff --git a/LaRCsim/ls_types.h b/LaRCsim/ls_types.h new file mode 100644 index 000000000..34eec9ce6 --- /dev/null +++ b/LaRCsim/ls_types.h @@ -0,0 +1,43 @@ +/*************************************************************************** + + TITLE: ls_types.h + +---------------------------------------------------------------------------- + + FUNCTION: LaRCSim type definitions header file + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Created 15 DEC 1993 by Bruce Jackson from old + ls_eom.h header + +---------------------------------------------------------------------------- + + DESIGNED BY: B. Jackson + + CODED BY: B. Jackson + + MAINTAINED BY: guess who + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + +--------------------------------------------------------------------------*/ +/* SCALAR type is used throughout equations of motion code - sets precision */ + +typedef double SCALAR; + +typedef SCALAR VECTOR_3[3]; + +/* DATA type is old style; this statement for continuity */ + +#define DATA SCALAR + +/* --------------------------- end of ls_types.h -------------------------*/ diff --git a/LaRCsim/mymain.c b/LaRCsim/mymain.c new file mode 100644 index 000000000..2d9acd954 --- /dev/null +++ b/LaRCsim/mymain.c @@ -0,0 +1,13 @@ +/* test main for playing with the LaRCsim code */ + +#include "ls_types.h" +#include "ls_cockpit.h" +#include "ls_generic.h" + + +COCKPIT cockpit_; +GENERIC generic_; + +int main() { + model_init(); +} diff --git a/LaRCsim/navion_aero.c b/LaRCsim/navion_aero.c new file mode 100644 index 000000000..527af07a5 --- /dev/null +++ b/LaRCsim/navion_aero.c @@ -0,0 +1,218 @@ +/*************************************************************************** + + TITLE: Navion_aero + +---------------------------------------------------------------------------- + + FUNCTION: Linear aerodynamics model + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Based upon class notes from AA271, Stanford University, + Spring 1988. Dr. Robert Cannon, instructor. + +---------------------------------------------------------------------------- + + DESIGNED BY: Bruce Jackson + + CODED BY: Bruce Jackson + + MAINTAINED BY: Bruce Jackson + +---------------------------------------------------------------------------- + + MODIFICATION HISTORY: + + DATE PURPOSE BY + 921229 Changed Alpha, Beta into radians; added Alpha bias. + EBJ + 930105 Modified to support linear airframe simulation by + adding shared memory initialization routine. EBJ + 931013 Added scaling by airspeed, to allow for low-airspeed + ground operations. EBJ + 940216 Scaled long, lat stick and rudder to more appropriate values + of elevator and aileron. EBJ + +---------------------------------------------------------------------------- + + REFERENCES: + +The Navion "aero" routine is a simple representation of the North +American Navion airplane, a 1950-s vintage single-engine, low-wing +mono-lane built by NAA (who built the famous P-51 Mustang) supposedly +as a plane for returning WW-II fighter jocks to carry the family +around the country in. Unfortunately underpowered, it can still be +found in small airports across the United States. From behind, it sort +of looks like a Volkswagen driving a Piper by virtue of its nicely +rounded cabin roof and small rear window. + +The aero routine is only valid around 100 knots; it is referred to as +a "linear model" of the navion; the data having been extracted by +someone unknown from a more complete model, or more likely, from +in-flight measurements and manuever time histories. It probably came +from someone at Princeton U; they owned a couple modified Navions that +had a variable-stability system installed, and were highly +instrumented (and well calibrated, I assume). + +In any event, a linearized model, such as this one, contains various +"stability derivatives", or estimates of how aerodynamic forces and +moments vary with changes in angle of attack, angular body rates, and +control surface deflections. For example, L_beta is an estimate of how +much roll moment varies per degree of sideslip increase. A decoding +ring is given below: + + X Aerodynamic force, lbs, in X-axis (+ forward) + Y Aerodynamic force, lbs, in Y-axis (+ right) + Z Aerodynamic force, lbs, in Z-axis (+ down) + L Aero. moment about X-axis (+ roll right), ft-lbs + M Aero. moment about Y-axis (+ pitch up), ft-lbs + N Aero. moment about Z-axis (+ nose right), ft-lbs + + 0 Subscript implying initial, or nominal, value + u X-axis component of airspeed (ft/sec) (+ forward) + v Y-axis component of airspeed (ft/sec) (+ right) + w Z-axis component of airspeed (ft/sec) (+ down) + p X-axis ang. rate (rad/sec) (+ roll right), rad/sec + q Y-axis ang. rate (rad/sec) (+ pitch up), rad/sec + r Z-axis ang. rate (rad/sec) (+ yaw right), rad/sec + beta Angle of sideslip, degrees (+ wind in RIGHT ear) + da Aileron deflection, degrees (+ left ail. TE down) + de Elevator deflection, degrees (+ trailing edge down) + dr Rudder deflection, degrees (+ trailing edge LEFT) + +---------------------------------------------------------------------------- + + CALLED BY: + +---------------------------------------------------------------------------- + + CALLS TO: + +---------------------------------------------------------------------------- + + INPUTS: + +---------------------------------------------------------------------------- + + OUTPUTS: + +--------------------------------------------------------------------------*/ + +#include "ls_types.h" +#include "ls_generic.h" +#include "ls_cockpit.h" + +/* define trimmed w_body to correspond with alpha_trim = 5 */ +#define TRIMMED_W 15.34 + +extern COCKPIT cockpit_; + + +void aero() +{ + static int init = 0; + + SCALAR u, w; + static SCALAR elevator, aileron, rudder; + static SCALAR long_scale = 0.3; + static SCALAR lat_scale = 0.1; + static SCALAR yaw_scale = -0.1; + static SCALAR scale = 1.0; + + static SCALAR trim_inc = 0.0002; + static SCALAR long_trim; + + static DATA U_0; + static DATA X_0; + static DATA M_0; + static DATA Z_0; + static DATA X_u; + static DATA X_w; + static DATA X_de; + static DATA Y_v; + static DATA Z_u; + static DATA Z_w; + static DATA Z_de; + static DATA L_beta; + static DATA L_p; + static DATA L_r; + static DATA L_da; + static DATA L_dr; + static DATA M_w; + static DATA M_q; + static DATA M_de; + static DATA N_beta; + static DATA N_p; + static DATA N_r; + static DATA N_da; + static DATA N_dr; + + if (!init) + { + init = -1; + + /* Initialize aero coefficients */ + + U_0 = 176; + X_0 = -573.75; + M_0 = 0; + Z_0 = -2750; + X_u = -0.0451; /* original value */ + /* X_u = 0.0000; */ /* for MUCH better performance - EBJ */ + X_w = 0.03607; + X_de = 0; + Y_v = -0.2543; + Z_u = -0.3697; /* original value */ + /* Z_u = -0.03697; */ /* for better performance - EBJ */ + Z_w = -2.0244; + Z_de = -28.17; + L_beta = -15.982; + L_p = -8.402; + L_r = 2.193; + L_da = 28.984; + L_dr = 2.548; + M_w = -0.05; + M_q = -2.0767; + M_de = -11.1892; + N_beta = 4.495; + N_p = -0.3498; + N_r = -0.7605; + N_da = -0.2218; + N_dr = -4.597; + + + /* initialize trim 'actuator' */ + long_trim = 1.969572E-03; + } + + u = V_rel_wind - U_0; + w = W_body - TRIMMED_W; + + elevator = long_scale * Long_control; + aileron = lat_scale * Lat_control; + rudder = yaw_scale * Rudder_pedal; + + if(Aft_trim) long_trim = long_trim - trim_inc; + if(Fwd_trim) long_trim = long_trim + trim_inc; + + scale = V_rel_wind*V_rel_wind/(U_0*U_0); + if (scale > 1.0) scale = 1.0; /* ebj */ + + + F_X_aero = scale*(X_0 + Mass*(X_u*u + X_w*w + X_de*elevator)); + F_Y_aero = scale*(Mass*Y_v*V_body); + F_Z_aero = scale*(Z_0 + Mass*(Z_u*u + Z_w*w + Z_de*elevator)); + + M_l_aero = scale*(I_xx*(L_beta*Beta + L_p*P_body + L_r*R_body + + L_da*aileron + L_dr*rudder)); + M_m_aero = scale*(M_0 + I_yy*(M_w*w + M_q*Q_body + M_de*(elevator + long_trim))); + M_n_aero = scale*(I_zz*(N_beta*Beta + N_p*P_body + N_r*R_body + + N_da*aileron + N_dr*rudder)); + +} + + diff --git a/LaRCsim/navion_engine.c b/LaRCsim/navion_engine.c new file mode 100644 index 000000000..eebc849eb --- /dev/null +++ b/LaRCsim/navion_engine.c @@ -0,0 +1,80 @@ +/*************************************************************************** + + TITLE: engine.c + +---------------------------------------------------------------------------- + + FUNCTION: dummy engine routine + +---------------------------------------------------------------------------- + + MODULE STATUS: incomplete + +---------------------------------------------------------------------------- + + GENEALOGY: Created 15 OCT 92 as dummy routine for checkout. EBJ + +---------------------------------------------------------------------------- + + 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_cockpit.h" +#include "ls_sim_control.h" + + +void engine( SCALAR dt, int init ) + +{ + if (init || sim_control_.sim_type != cockpit) + Throttle[3] = Throttle_pct; + + F_X_engine = Throttle[3]*813.4/0.2; + F_Z_engine = Throttle[3]*11.36/0.2; + + Throttle_pct = Throttle[3]; +} + + diff --git a/LaRCsim/navion_gear.c b/LaRCsim/navion_gear.c new file mode 100644 index 000000000..e9c5b768a --- /dev/null +++ b/LaRCsim/navion_gear.c @@ -0,0 +1,314 @@ +/*************************************************************************** + + TITLE: gear + +---------------------------------------------------------------------------- + + FUNCTION: Landing gear model for example simulation + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: 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 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" + + +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]; +} + +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]; +} + +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]; +} + +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]; +} + +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]; +} + +clear3( DATA v[] ) +{ + v[0] = 0.; v[1] = 0.; v[2] = 0.; +} + +gear() +{ +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 + */ + + percent_brake[1] = 0.; /* replace with cockpit brake handle connection code */ + percent_brake[2] = percent_brake[1]; + + caster_angle_rad[0] = 0.03*Rudder_pedal; + + 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/LaRCsim/navion_init.c b/LaRCsim/navion_init.c new file mode 100644 index 000000000..6e2aa7fb8 --- /dev/null +++ b/LaRCsim/navion_init.c @@ -0,0 +1,75 @@ +/*************************************************************************** + + TITLE: navion_init.c + +---------------------------------------------------------------------------- + + FUNCTION: Initializes navion math model + +---------------------------------------------------------------------------- + + MODULE STATUS: developmental + +---------------------------------------------------------------------------- + + GENEALOGY: Added to navion package 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" + +void model_init() +{ + + Throttle[3] = 0.2; Rudder_pedal = 0; Lat_control = 0; Long_control = 0; + + Dx_pilot = 0; Dy_pilot = 0; Dz_pilot = 0; + + Runway_altitude = 0; + Runway_latitude = 0; + Runway_longitude = 0; + Runway_heading = 0; + +}