1 /**************************************************************************
2 * ls_interface.c -- the FG interface to the LaRCsim routines
3 * This is a heavily modified version of LaRCsim.c
4 * As a result there is much old baggage left in this file.
6 * Originally Written 921230 by Bruce Jackson
7 * Modified by Curtis Olson, started May 1997.
9 * This program is free software; you can redistribute it and/or
10 * modify it under the terms of the GNU General Public License as
11 * published by the Free Software Foundation; either version 2 of the
12 * License, or (at your option) any later version.
14 * This program is distributed in the hope that it will be useful, but
15 * WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 * General Public License for more details.
19 * You should have received a copy of the GNU General Public License
20 * along with this program; if not, write to the Free Software
21 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24 * (Log is kept at end of this file)
25 **************************************************************************/
27 /* Original headers follow: */
29 /***************************************************************************
33 ----------------------------------------------------------------------------
35 FUNCTION: Top level routine for LaRCSIM. Includes
36 global variable declarations.
38 ----------------------------------------------------------------------------
40 MODULE STATUS: Developmental
42 ----------------------------------------------------------------------------
44 GENEALOGY: Written 921230 by Bruce Jackson
46 ----------------------------------------------------------------------------
54 ----------------------------------------------------------------------------
60 930111 Added "progname" variable to keep name of invoking command.
62 931012 Removed altitude < 0. test to support gear development. EBJ
63 931214 Added various pressures (Impact, Dynamic, Static, etc.) EBJ
64 931215 Adopted new generic variable structure. EBJ
65 931218 Added command line options decoding. EBJ
66 940110 Changed file type of matrix file to ".m" EBJ
67 940513 Renamed this routine "LaRCsim.c" from "ls_main.c" EBJ
68 940513 Added time_stamp routine, t_stamp. EBJ
69 950225 Added options flag, 'i', to set I/O output rate. EBJ
70 950306 Added calls to ls_get_settings() and ls_put_settings() EBJ
71 950314 Options flag 'i' now reads IC file; 'o' is output rate EBJ
72 950406 Many changes: added definition of default value macros;
73 removed local variables term_update_hz, model_dt, endtime,
74 substituted sim_control_ globals for these; removed
75 initialization of sim_control_.tape_channels; moved optarg
76 to generic extern; added mod_end_time & mod_buf_size flags
77 and temporary buffer_time and data_rate locals to
78 ls_checkopts(); added additional command line switches '-s'
79 and '-b'; made psuedo-mandatory file names for data output
80 switches; considerable rewrite of logic for setting data
81 buffer length and interleave parameters; updated '-h' help
82 output message; added protection logic to calculations of
83 these parameters; added check of return value on first call
84 to ls_cockpit() so <esc> abort works from initial pause
85 state; added call to ls_unsync() immediately following
86 first ls_sync() call, if paused (to avoid alarm clock
87 timeout); moved call to ls_record() into non-paused
88 multiloop path (was filling buffer with identical data
89 during pause); put check of paused flag before calling sync
90 routine ls_pause(); and added call to exit() on termination.
94 $Original log: LaRCsim.c,v $
95 * Revision 1.4.1.7 1995/04/07 01:04:37 bjax
96 * Many changes made to support storage of sim options from run to run,
97 * as well as restructuring storage buffer sizing and some loop logic
98 * changes. See the modification log for details.
100 * Revision 1.4.1.6 1995/03/29 16:12:09 bjax
101 * Added argument to -o switch; changed run loop to pass dt=0
102 * if in paused mode. EBj
104 * Revision 1.4.1.5 1995/03/15 12:30:20 bjax
105 * Set paused flag to non-zero by default; moved 'i' I/O rate flag
106 * switch to 'o'; made 'i' an initial conditions file switch; added
107 * null string to ls_get_settings() call so that default settings
108 * file will be read. EBJ
110 * Revision 1.4.1.4 1995/03/08 12:31:34 bjax
111 * Added userid retrieval and proper termination of time & date strings.
113 * Revision 1.4.1.3 1995/03/08 12:00:21 bjax
114 * Moved setting of default options to ls_setdefopts from
115 * ls_checkopts; rearranged order of ls_get_settings() call
116 * to between ls_setdefopts and ls_checkopts, so command
117 * line options will override settings file options.
120 * Revision 1.4.1.2 1995/03/06 18:48:49 bjax
121 * Added calles to ls_get_settings() and ls_put_settings(); added
122 * passing of dt and init flags in ls_model(). EBJ
124 * Revision 1.4.1.1 1995/03/03 02:23:08 bjax
125 * Beta version for LaRCsim, version 1.4
127 * Revision 1.3.2.7 1995/02/27 20:00:21 bjax
130 * Revision 1.3.2.6 1995/02/25 16:52:31 bjax
131 * Added 'i' option to set I/O iteration rate. EBJ
133 * Revision 1.3.2.5 1995/02/06 19:33:15 bjax
136 * Revision 1.3.2.4 1995/02/06 19:30:30 bjax
137 * Oops, should really compile these before checking in. Fixed capitailzation of
138 * Initialize in ls_loop parameter.
140 * Revision 1.3.2.3 1995/02/06 19:25:44 bjax
141 * Moved main simulation loop into subroutine ls_loop. EBJ
143 * Revision 1.3.2.2 1994/05/20 21:46:45 bjax
144 * A little better logic on checking for option arguments.
146 * Revision 1.3.2.1 1994/05/20 19:29:51 bjax
147 * Added options arguments to command line.
149 * Revision 1.3.1.16 1994/05/17 15:08:45 bjax
150 * Corrected so that full name to directyr and file is saved
151 * in new global variable "fullname"; this allows symbol table
152 * to be extracted when in another default directory.
154 * Revision 1.3.1.15 1994/05/17 14:50:24 bjax
157 * Revision 1.3.1.14 1994/05/17 14:50:23 bjax
160 * Revision 1.3.1.13 1994/05/17 14:50:21 bjax
163 * Revision 1.3.1.12 1994/05/17 14:50:20 bjax
166 * Revision 1.3.1.11 1994/05/17 13:56:24 bjax
169 * Revision 1.3.1.10 1994/05/17 13:23:03 bjax
172 * Revision 1.3.1.9 1994/05/17 13:20:03 bjax
175 * Revision 1.3.1.8 1994/05/17 13:19:23 bjax
178 * Revision 1.3.1.7 1994/05/17 13:18:29 bjax
181 * Revision 1.3.1.6 1994/05/17 13:16:30 bjax
184 * Revision 1.3.1.5 1994/05/17 13:03:44 bjax
187 * Revision 1.3.1.4 1994/05/17 13:03:38 bjax
190 * Revision 1.3.1.3 1994/05/17 12:49:08 bjax
193 * Revision 1.3.1.2 1994/05/17 12:48:45 bjax
194 * *** empty log message ***
196 * Revision 1.3.1.1 1994/05/13 20:39:17 bjax
199 * Revision 1.2 1994/05/13 19:51:50 bjax
203 ----------------------------------------------------------------------------
207 ----------------------------------------------------------------------------
211 ----------------------------------------------------------------------------
215 ----------------------------------------------------------------------------
219 ----------------------------------------------------------------------------
223 --------------------------------------------------------------------------*/
225 #include "ls_interface.h"
227 #include "ls_types.h"
228 #include "ls_constants.h"
229 #include "ls_generic.h"
230 #include "ls_sim_control.h"
231 #include "ls_cockpit.h"
232 /* #include "ls_tape.h" */
236 #include <sys/types.h>
237 #include <sys/stat.h>
243 /* global variable declarations */
247 SIM_CONTROL sim_control_;
252 /* #define DEFAULT_TERM_UPDATE_HZ 20 */ /* original value */
253 #define DEFAULT_TERM_UPDATE_HZ 20
254 #define DEFAULT_MODEL_HZ 120
255 #define DEFAULT_END_TIME 3600.
256 #define DEFAULT_SAVE_SPACING 8
257 #define DEFAULT_WRITE_SPACING 1
258 #define MAX_FILE_NAME_LENGTH 80
260 /* global variables */
265 /* file variables - default simulation settings */
267 static double model_dt;
268 static double speedup;
269 static char asc1name[MAX_FILE_NAME_LENGTH] = "run.asc1";
270 static char tabname[MAX_FILE_NAME_LENGTH] = "run.dat";
271 static char fltname[MAX_FILE_NAME_LENGTH] = "run.flt";
272 static char matname[MAX_FILE_NAME_LENGTH] = "run.m";
278 char rcsid[] = "$Id$";
279 char revid[] = "$Revision$";
280 char dateid[] = "$Date$";
285 /* report version of LaRCsim*/
286 printf("\nLaRCsim %s, %s\n\n", revid, dateid);
288 nowtime_t = time( 0 );
289 nowtime = localtime( &nowtime_t ); /* set fields to correct time values */
290 date = (nowtime->tm_year)*10000
291 + (nowtime->tm_mon + 1)*100
292 + (nowtime->tm_mday);
293 sprintf(sim_control_.date_string, "%06d\0", date);
294 sprintf(sim_control_.time_stamp, "%02d:%02d:%02d\0",
295 nowtime->tm_hour, nowtime->tm_min, nowtime->tm_sec);
296 cuserid( sim_control_.userid ); /* set up user id */
303 /* set default values for most options */
305 sim_control_.debug = 0; /* change to non-zero if in dbx! */
306 sim_control_.vision = 0;
307 sim_control_.write_av = 0; /* write Agile-Vu '.flt' file */
308 sim_control_.write_mat = 0; /* write matrix-x/matlab script */
309 sim_control_.write_tab = 0; /* write tab delim. history file */
310 sim_control_.write_asc1 = 0; /* write GetData file */
311 sim_control_.sim_type = GLmouse; /* hook up to mouse */
312 sim_control_.save_spacing = DEFAULT_SAVE_SPACING;
313 /* interpolation on recording */
314 sim_control_.write_spacing = DEFAULT_WRITE_SPACING;
315 /* interpolation on output */
316 sim_control_.end_time = DEFAULT_END_TIME;
317 sim_control_.model_hz = DEFAULT_MODEL_HZ;
318 sim_control_.term_update_hz = DEFAULT_TERM_UPDATE_HZ;
319 sim_control_.time_slices = DEFAULT_END_TIME * DEFAULT_MODEL_HZ /
320 DEFAULT_SAVE_SPACING;
321 sim_control_.paused = 0;
327 /* return result codes from ls_checkopts */
335 int ls_checkopts(argc, argv) /* check and set options flags */
341 int mod_end_time = 0;
342 int mod_buf_size = 0;
343 float buffer_time, data_rate;
345 /* set default values */
347 buffer_time = sim_control_.time_slices * sim_control_.save_spacing /
348 sim_control_.model_hz;
349 data_rate = sim_control_.model_hz / sim_control_.save_spacing;
351 while ((c = getopt(argc, argv, "Aa:b:de:f:hi:kmo:r:s:t:x:")) != EOF)
354 if (sim_control_.sim_type == GLmouse)
356 fprintf(stderr, "Cannot specify both keyboard (k) and ACES (A) cockpits option\n");
357 fprintf(stderr, "Keyboard operation assumed.\n");
360 sim_control_.sim_type = cockpit;
363 sim_control_.write_av = 1;
366 strncpy(fltname, optarg, MAX_FILE_NAME_LENGTH);
371 buffer_time = atof(optarg);
372 if (buffer_time <= 0.) opt_err = -1;
376 sim_control_.debug = 1;
379 sim_control_.end_time = atof(optarg);
383 sim_control_.model_hz = atof(optarg);
389 /* ls_get_settings( optarg ); */
392 sim_control_.sim_type = GLmouse;
395 sim_control_.vision = 1;
398 sim_control_.term_update_hz = atof(optarg);
399 if (sim_control_.term_update_hz <= 0.) opt_err = 1;
402 sim_control_.write_mat = 1;
405 strncpy(matname, optarg, MAX_FILE_NAME_LENGTH);
410 data_rate = atof(optarg);
411 if (data_rate <= 0.) opt_err = -1;
414 sim_control_.write_tab = 1;
417 strncpy(tabname, optarg, MAX_FILE_NAME_LENGTH);
422 sim_control_.write_asc1 = 1;
425 strncpy(asc1name, optarg, MAX_FILE_NAME_LENGTH);
436 fprintf(stderr, "Usage: %s [-options]\n", progname);
437 fprintf(stderr, "\n");
438 fprintf(stderr, " where [-options] is zero or more of the following:\n");
439 fprintf(stderr, "\n");
440 fprintf(stderr, " [A|k] Run mode: [A]CES cockpit [default]\n");
441 fprintf(stderr, " or [k]eyboard\n");
442 fprintf(stderr, "\n");
443 fprintf(stderr, " [i <filename>] [i]nitial conditions filename\n");
444 fprintf(stderr, "\n");
445 fprintf(stderr, " [f <value>] Iteration rate [f]requency, Hz (default is %5.2f Hz)\n",
446 sim_control_.model_hz);
447 fprintf(stderr, "\n");
448 fprintf(stderr, " [o <value>] Display [o]utput frequency, Hz (default is %5.2f Hz)\n",
449 sim_control_.term_update_hz);
450 fprintf(stderr, "\n");
451 fprintf(stderr, " [s <value>] Data storage frequency, Hz (default is %5.2f Hz)\n",
453 fprintf(stderr, "\n");
454 fprintf(stderr, " [e <value>] [e]nd time in seconds (default %5.1f seconds)\n",
455 sim_control_.end_time);
456 fprintf(stderr, "\n");
457 fprintf(stderr, " [b <value>] circular time history storage [b]uffer size, in seconds \n");
458 fprintf(stderr, " (default %5.1f seconds) (normally same as end time)\n",
459 sim_control_.time_slices*sim_control_.save_spacing/
460 sim_control_.model_hz);
461 fprintf(stderr, "\n");
462 fprintf(stderr, " [atxr [<filename>]] Output: [a]gile-vu (default name: %s )\n", fltname);
463 fprintf(stderr, " and/or [t]ab delimited ( '' name: %s )\n", tabname);
464 fprintf(stderr, " and/or [x]plot (default name: %s)\n", asc1name);
465 fprintf(stderr, " and/or mat[r]ix script ( '' name: %s )\n", matname);
466 fprintf(stderr, "\n");
470 /* calculate additional controls */
472 sim_control_.save_spacing = (int) (0.5 + sim_control_.model_hz / data_rate);
473 if (sim_control_.save_spacing < 1) sim_control_.save_spacing = 1;
475 sim_control_.time_slices = buffer_time * sim_control_.model_hz /
476 sim_control_.save_spacing;
477 if (sim_control_.time_slices < 2) sim_control_.time_slices = 2;
483 void ls_loop( dt, initialize )
489 /* printf (" In ls_loop()\n"); */
490 ls_step( dt, initialize );
491 /* if (sim_control_.sim_type == cockpit ) ls_ACES(); */
493 ls_model( dt, initialize );
501 sim_control_.paused = 0;
505 /* printf("Mach = %.2f ", Mach_number);
506 printf("%.4f,%.4f,%.2f ", Latitude, Longitude, Altitude);
507 printf("%.2f,%.2f,%.2f\n", Phi, Theta, Psi); */
512 /* Initialize the LaRCsim flight model, dt is the time increment for
513 each subsequent iteration through the EOM */
514 int fgLaRCsimInit(double dt) {
518 ls_setdefopts(); /* set default options */
520 /* Number_of_Continuous_States = 22; */
522 generic_.geodetic_position_v[0] = 2.793445E-05;
523 generic_.geodetic_position_v[1] = 3.262070E-07;
524 generic_.geodetic_position_v[2] = 3.758099E+00;
525 generic_.v_local_v[0] = 7.287719E+00;
526 generic_.v_local_v[1] = 1.521770E+03;
527 generic_.v_local_v[2] = -1.265722E-05;
528 generic_.euler_angles_v[0] = -2.658474E-06;
529 generic_.euler_angles_v[1] = 7.401790E-03;
530 generic_.euler_angles_v[2] = 1.391358E-03;
531 generic_.omega_body_v[0] = 7.206685E-05;
532 generic_.omega_body_v[1] = 0.000000E+00;
533 generic_.omega_body_v[2] = 9.492658E-05;
534 generic_.earth_position_angle = 0.000000E+00;
535 generic_.mass = 8.547270E+01;
536 generic_.i_xx = 1.048000E+03;
537 generic_.i_yy = 3.000000E+03;
538 generic_.i_zz = 3.530000E+03;
539 generic_.i_xz = 0.000000E+00;
540 generic_.d_cg_rp_body_v[0] = 0.000000E+00;
541 generic_.d_cg_rp_body_v[1] = 0.000000E+00;
542 generic_.d_cg_rp_body_v[2] = 0.000000E+00;
544 ls_stamp(); /* ID stamp; record time and date of run */
546 if (speedup == 0.0) {
547 fprintf(stderr, "%s: Cannot run with speedup of 0.\n", progname);
554 /* Initialize (get) cockpit (controls) settings */
562 /* Run an iteration of the EOM (equations of motion) */
563 int fgLaRCsimUpdate(int multiloop) {
570 for ( i = 0; i < multiloop; i++ ) {
571 ls_loop( model_dt, 0);
578 /* Flight Gear Modification Log
581 * Revision 1.1 1997/05/29 00:09:57 curt
582 * Initial Flight Gear revision.