--- /dev/null
+/***************************************************************************
+
+ 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 <esc> 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 <libgen.h>
+#endif
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <time.h>
+
+/* 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 <filename>] [i]nitial conditions filename\n");
+ fprintf(stderr, "\n");
+ fprintf(stderr, " [f <value>] Iteration rate [f]requency, Hz (default is %5.2f Hz)\n",
+ sim_control_.model_hz);
+ fprintf(stderr, "\n");
+ fprintf(stderr, " [o <value>] Display [o]utput frequency, Hz (default is %5.2f Hz)\n",
+ sim_control_.term_update_hz);
+ fprintf(stderr, "\n");
+ fprintf(stderr, " [s <value>] Data storage frequency, Hz (default is %5.2f Hz)\n",
+ data_rate);
+ fprintf(stderr, "\n");
+ fprintf(stderr, " [e <value>] [e]nd time in seconds (default %5.1f seconds)\n",
+ sim_control_.end_time);
+ fprintf(stderr, "\n");
+ fprintf(stderr, " [b <value>] 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 [<filename>]] 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<multiloop;i++) {
+ if (sim_control_.paused) {
+ ls_loop( 0.0, 0);
+ } else {
+ ls_loop( model_dt, 0);
+ /* ls_record(); */
+ }
+ }
+ if (speedup > 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
+*/
--- /dev/null
+#---------------------------------------------------------------------------
+# 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.
+#
--- /dev/null
+/***************************************************************************
+
+ 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 <math.h>
+
+#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 */
+}
+/**************************************************************************/
--- /dev/null
+/***************************************************************************
+
+ 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() {} */
+
+
+
+
--- /dev/null
+/***************************************************************************
+
+ 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 <math.h>
+
+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 */
+
+}
+/**************************************************************************/
+
+
+
+
--- /dev/null
+/***************************************************************************
+
+ 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 <math.h>
+
+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 */
+
+}
+/*************************************************************************/
--- /dev/null
+/***************************************************************************
+
+ 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
--- /dev/null
+/***************************************************************************
+
+ 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 -------------------------*/
--- /dev/null
+/***************************************************************************
+
+ 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 ------------------------*/
--- /dev/null
+/***************************************************************************
+
+ 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 <math.h>
+
+/* 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 );
+}
--- /dev/null
+/***************************************************************************
+
+ 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 <math.h>
+
+#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);
+
+}
--- /dev/null
+/***************************************************************************
+
+ 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 <string.h>
+#include <stdio.h>
+#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<HARDWIRED;i++)
+ strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
+
+ strcpy( Continuous_States[ 0].Symbol.Par_Name,
+ "generic_.geodetic_position_v[0]");
+ strcpy( Continuous_States[ 1].Symbol.Par_Name,
+ "generic_.geodetic_position_v[1]");
+ strcpy( Continuous_States[ 2].Symbol.Par_Name,
+ "generic_.geodetic_position_v[2]");
+ strcpy( Continuous_States[ 3].Symbol.Par_Name,
+ "generic_.v_local_v[0]");
+ strcpy( Continuous_States[ 4].Symbol.Par_Name,
+ "generic_.v_local_v[1]");
+ strcpy( Continuous_States[ 5].Symbol.Par_Name,
+ "generic_.v_local_v[2]");
+ strcpy( Continuous_States[ 6].Symbol.Par_Name,
+ "generic_.euler_angles_v[0]");
+ strcpy( Continuous_States[ 7].Symbol.Par_Name,
+ "generic_.euler_angles_v[1]");
+ strcpy( Continuous_States[ 8].Symbol.Par_Name,
+ "generic_.euler_angles_v[2]");
+ strcpy( Continuous_States[ 9].Symbol.Par_Name,
+ "generic_.omega_body_v[0]");
+ strcpy( Continuous_States[10].Symbol.Par_Name,
+ "generic_.omega_body_v[1]");
+ strcpy( Continuous_States[11].Symbol.Par_Name,
+ "generic_.omega_body_v[2]");
+ strcpy( Continuous_States[12].Symbol.Par_Name,
+ "generic_.earth_position_angle");
+ }
+
+ /* commented out by CLO
+ for (i=0;i<Number_of_Continuous_States;i++)
+ {
+ (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
+ if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
+ }
+
+ for (i=0;i<Number_of_Discrete_States;i++)
+ {
+ (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
+ if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
+ }
+ */
+}
+
+void ls_init()
+{
+ int i;
+
+ Simtime = 0;
+
+ ls_init_init();
+ /* move the states to proper values */
+
+ /* commented out by CLO
+ for(i=0;i<Number_of_Continuous_States;i++)
+ if (Continuous_States[i].Symbol.Addr)
+ ls_set_sym_val( &Continuous_States[i].Symbol,
+ Continuous_States[i].value );
+
+ for(i=0;i<Number_of_Discrete_States;i++)
+ if (Discrete_States[i].Symbol.Addr)
+ ls_set_sym_val( &Discrete_States[i].Symbol,
+ (double) Discrete_States[i].value );
+ */
+
+ model_init();
+
+ ls_step(0.0, -1);
+}
+
+
+char *ls_init_get_set(char *buffer, char *eob)
+/* This routine parses the settings file for "init" entries. */
+{
+
+ static char *fac_name = FACILITY_NAME_STRING;
+ char *bufptr;
+ char line[256];
+ int n, ver, i, error, abrt;
+
+ enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
+
+ abrt = 0;
+ looking_for = cont_states_header;
+
+ n = sscanf(buffer, "%s", line);
+ if (n == 0) return 0L;
+ if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
+
+ bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
+ if (bufptr == 0) return 0L;
+
+ sscanf( bufptr, "%d", &ver );
+ if (ver != CURRENT_VERSION) return 0L;
+
+ ls_init_init();
+
+ while( !abrt && (eob > 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<Number_of_Continuous_States; i++)
+ fprintf(fp, " %s\t%s\t%E\n",
+ Continuous_States[i].Symbol.Mod_Name,
+ Continuous_States[i].Symbol.Par_Name,
+ Continuous_States[i].value );
+
+ fprintf(fp, " discrete_states: %d\n", Number_of_Discrete_States);
+ fprintf(fp, "# module parameter value\n");
+ for (i=0;i<Number_of_Discrete_States;i++)
+ fprintf(fp, " %s\t%s\t%ld\n",
+ Discrete_States[i].Symbol.Mod_Name,
+ Discrete_States[i].Symbol.Par_Name,
+ Discrete_States[i].value );
+ fprintf(fp, "end\n");
+ return;
+}
+
+void ls_save_current_as_ic()
+{
+ /* Save current states as initial conditions */
+
+ int i, error;
+
+ /* commented out by CLO
+ for(i=0;i<Number_of_Continuous_States;i++)
+ if (Continuous_States[i].Symbol.Addr)
+ Continuous_States[i].value =
+ ls_get_sym_val( &Continuous_States[i].Symbol, &error );
+
+ for(i=0;i<Number_of_Discrete_States;i++)
+ if (Discrete_States[i].Symbol.Addr)
+ Discrete_States[i].value = (long)
+ ls_get_sym_val( &Discrete_States[i].Symbol, &error );
+ */
+}
--- /dev/null
+/**************************************************************************
+ * ls_interface.c -- the FG interface to the LaRCsim routines
+ * This is a heavily modified version of LaRCsim.c
+ * As a result there is much old baggage left in this file.
+ *
+ * Originally Written 921230 by Bruce Jackson
+ * Modified by Curtis Olson, started May 1997.
+ *
+ * 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)
+ **************************************************************************/
+
+/* Original headers follow: */
+
+/***************************************************************************
+
+ 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 <esc> 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 <libgen.h>
+#endif
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <time.h>
+
+/* 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 <filename>] [i]nitial conditions filename\n");
+ fprintf(stderr, "\n");
+ fprintf(stderr, " [f <value>] Iteration rate [f]requency, Hz (default is %5.2f Hz)\n",
+ sim_control_.model_hz);
+ fprintf(stderr, "\n");
+ fprintf(stderr, " [o <value>] Display [o]utput frequency, Hz (default is %5.2f Hz)\n",
+ sim_control_.term_update_hz);
+ fprintf(stderr, "\n");
+ fprintf(stderr, " [s <value>] Data storage frequency, Hz (default is %5.2f Hz)\n",
+ data_rate);
+ fprintf(stderr, "\n");
+ fprintf(stderr, " [e <value>] [e]nd time in seconds (default %5.1f seconds)\n",
+ sim_control_.end_time);
+ fprintf(stderr, "\n");
+ fprintf(stderr, " [b <value>] 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 [<filename>]] 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.
+ *
+ */
--- /dev/null
+/**************************************************************************
+ * 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.
+/*
+ */
--- /dev/null
+/***************************************************************************
+
+ 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 );
+}
--- /dev/null
+/***************************************************************************
+
+ 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 <stdio.h>
+
+#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 ----------------------*/
--- /dev/null
+/***************************************************************************
+
+ 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 <math.h>
+
+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 */
+}
+/*************************************************************************/
+
--- /dev/null
+/***************************************************************************
+
+ 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. */
+
+
+
+
--- /dev/null
+/***************************************************************************
+
+ 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 <sys/time.h>
+#include <signal.h>
+#include <stdio.h>
+#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();
+}
+
--- /dev/null
+/***************************************************************************
+
+ 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 -------------------------*/
--- /dev/null
+/* 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();
+}
--- /dev/null
+/***************************************************************************
+
+ 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));
+
+}
+
+
--- /dev/null
+/***************************************************************************
+
+ 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 <math.h>
+#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];
+}
+
+
--- /dev/null
+/***************************************************************************
+
+ 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 <math.h>
+#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<num_wheels;i++) /* Loop for each wheel */
+ {
+ /*========================================*/
+ /* Calculate wheel position w.r.t. runway */
+ /*========================================*/
+
+ /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
+
+ sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
+
+ /* then converting to local (North-East-Down) axes... */
+
+ multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
+
+ /* Runway axes correction - third element is Altitude, not (-)Z... */
+
+ d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
+
+ /* Add wheel offset to cg location in local axes */
+
+ add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
+
+ /* remove Runway axes correction so right hand rule applies */
+
+ d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
+
+ /*============================*/
+ /* Calculate wheel velocities */
+ /*============================*/
+
+ /* contribution due to angular rates */
+
+ cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
+
+ /* transform into local axes */
+
+ multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
+
+ /* plus contribution due to cg velocities */
+
+ add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
+
+
+ /*===========================================*/
+ /* Calculate forces & moments for this wheel */
+ /*===========================================*/
+
+ /* Add any anticipation, or frame lead/prediction, here... */
+
+ /* no lead used at present */
+
+ /* Calculate sideward and forward velocities of the wheel
+ in the runway plane */
+
+ cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
+ sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
+
+ v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ + v_wheel_local_v[1]*sin_wheel_hdg_angle;
+ v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
+ - v_wheel_local_v[0]*sin_wheel_hdg_angle;
+
+ /* Calculate normal load force (simple spring constant) */
+
+ reaction_normal_force = 0.;
+ if( d_wheel_rwy_local_v[2] < 0. )
+ {
+ reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
+ - v_wheel_local_v[2]*spring_damping[i];
+ if (reaction_normal_force > 0.) reaction_normal_force = 0.;
+ /* to prevent damping component from swamping spring component */
+ }
+
+ /* Calculate friction coefficients */
+
+ forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
+ abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
+ sideward_mu = sliding_mu;
+ if (abs_v_wheel_sideward < skid_v)
+ sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
+ if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
+
+ /* Calculate foreward and sideward reaction forces */
+
+ forward_wheel_force = forward_mu*reaction_normal_force;
+ sideward_wheel_force = sideward_mu*reaction_normal_force;
+ if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
+ if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
+
+ /* Rotate into local (N-E-D) axes */
+
+ f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
+ - sideward_wheel_force*sin_wheel_hdg_angle;
+ f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ + sideward_wheel_force*cos_wheel_hdg_angle;
+ f_wheel_local_v[2] = reaction_normal_force;
+
+ /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
+
+ mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
+
+ /* Calculate moments from force and offsets in body axes */
+
+ cross3( d_wheel_cg_body_v, tempF, tempM );
+
+ /* Sum forces and moments across all wheels */
+
+ add3( tempF, F_gear_v, F_gear_v );
+ add3( tempM, M_gear_v, M_gear_v );
+
+ }
+}
--- /dev/null
+/***************************************************************************
+
+ TITLE: navion_init.c
+
+----------------------------------------------------------------------------
+
+ FUNCTION: Initializes navion math model
+
+----------------------------------------------------------------------------
+
+ MODULE STATUS: developmental
+
+----------------------------------------------------------------------------
+
+ GENEALOGY: 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;
+
+}
projects / flightgear.git / commitdiff