LaRCsim/ls_interface.c

   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.
   5  *
   6  * Originally Written 921230 by Bruce Jackson
   7  * Modified by Curtis Olson, started May 1997.
   8  *
   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.
  13  *
  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.
  18  *
  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.
  22  *
  23  * $Id$
  24  * (Log is kept at end of this file)
  25  **************************************************************************/
  26
  27 /* Original headers follow: */
  28
  29 /***************************************************************************
  30
  31         TITLE:          LaRCsim.c
  32
  33 ----------------------------------------------------------------------------
  34
  35         FUNCTION:       Top level routine for LaRCSIM.  Includes
  36                         global variable declarations.
  37
  38 ----------------------------------------------------------------------------
  39
  40         MODULE STATUS:  Developmental
  41
  42 ----------------------------------------------------------------------------
  43
  44         GENEALOGY:      Written 921230 by Bruce Jackson
  45
  46 ----------------------------------------------------------------------------
  47
  48         DESIGNED BY:    EBJ
  49
  50         CODED BY:       EBJ
  51
  52         MAINTAINED BY:  EBJ
  53
  54 ----------------------------------------------------------------------------
  55
  56         MODIFICATION HISTORY:
  57
  58         DATE    PURPOSE                                         BY
  59
  60         930111  Added "progname" variable to keep name of invoking command.
  61                                                                         EBJ
  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.
  91
  92
  93 $Header$
  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.
  99  *
 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
 103  *
 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
 109  *
 110  * Revision 1.4.1.4  1995/03/08  12:31:34  bjax
 111  * Added userid retrieval and proper termination of time & date strings.
 112  *
 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.
 118  * EBJ
 119  *
 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
 123  *
 124  * Revision 1.4.1.1  1995/03/03  02:23:08  bjax
 125  * Beta version for LaRCsim, version 1.4
 126  *
 127  * Revision 1.3.2.7  1995/02/27  20:00:21  bjax
 128  * Rebuilt LaRCsim
 129  *
 130  * Revision 1.3.2.6  1995/02/25  16:52:31  bjax
 131  * Added 'i' option to set I/O iteration rate. EBJ
 132  *
 133  * Revision 1.3.2.5  1995/02/06  19:33:15  bjax
 134  * Rebuilt LaRCsim
 135  *
 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.
 139  *
 140  * Revision 1.3.2.3  1995/02/06  19:25:44  bjax
 141  * Moved main simulation loop into subroutine ls_loop. EBJ
 142  *
 143  * Revision 1.3.2.2  1994/05/20  21:46:45  bjax
 144  * A little better logic on checking for option arguments.
 145  *
 146  * Revision 1.3.2.1  1994/05/20  19:29:51  bjax
 147  * Added options arguments to command line.
 148  *
 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.
 153  *
 154  * Revision 1.3.1.15  1994/05/17  14:50:24  bjax
 155  * Rebuilt LaRCsim
 156  *
 157  * Revision 1.3.1.14  1994/05/17  14:50:23  bjax
 158  * Rebuilt LaRCsim
 159  *
 160  * Revision 1.3.1.13  1994/05/17  14:50:21  bjax
 161  * Rebuilt LaRCsim
 162  *
 163  * Revision 1.3.1.12  1994/05/17  14:50:20  bjax
 164  * Rebuilt LaRCsim
 165  *
 166  * Revision 1.3.1.11  1994/05/17  13:56:24  bjax
 167  * Rebuilt LaRCsim
 168  *
 169  * Revision 1.3.1.10  1994/05/17  13:23:03  bjax
 170  * Rebuilt LaRCsim
 171  *
 172  * Revision 1.3.1.9  1994/05/17  13:20:03  bjax
 173  * Rebuilt LaRCsim
 174  *
 175  * Revision 1.3.1.8  1994/05/17  13:19:23  bjax
 176  * Rebuilt LaRCsim
 177  *
 178  * Revision 1.3.1.7  1994/05/17  13:18:29  bjax
 179  * Rebuilt LaRCsim
 180  *
 181  * Revision 1.3.1.6  1994/05/17  13:16:30  bjax
 182  * Rebuilt LaRCsim
 183  *
 184  * Revision 1.3.1.5  1994/05/17  13:03:44  bjax
 185  * Rebuilt LaRCsim
 186  *
 187  * Revision 1.3.1.4  1994/05/17  13:03:38  bjax
 188  * Rebuilt LaRCsim
 189  *
 190  * Revision 1.3.1.3  1994/05/17  12:49:08  bjax
 191  * Rebuilt LaRCsim
 192  *
 193  * Revision 1.3.1.2  1994/05/17  12:48:45  bjax
 194  * *** empty log message ***
 195  *
 196  * Revision 1.3.1.1  1994/05/13  20:39:17  bjax
 197  * Top of 1.3 branch.
 198  *
 199  * Revision 1.2  1994/05/13  19:51:50  bjax
 200  * Skip rev
 201  *
 202
 203 ----------------------------------------------------------------------------
 204
 205         REFERENCES:
 206
 207 ----------------------------------------------------------------------------
 208
 209         CALLED BY:
 210
 211 ----------------------------------------------------------------------------
 212
 213         CALLS TO:
 214
 215 ----------------------------------------------------------------------------
 216
 217         INPUTS:
 218
 219 ----------------------------------------------------------------------------
 220
 221         OUTPUTS:
 222
 223 --------------------------------------------------------------------------*/
 224
 225 #include "ls_interface.h"
 226
 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" */
 233 #ifndef linux
 234 # include <libgen.h>
 235 #endif
 236 #include <sys/types.h>
 237 #include <sys/stat.h>
 238 #include <stdlib.h>
 239 #include <stdio.h>
 240 #include <math.h>
 241 #include <time.h>
 242
 243 /* global variable declarations */
 244
 245 /* TAPE         *Tape; */
 246 GENERIC         generic_;
 247 SIM_CONTROL     sim_control_;
 248 COCKPIT         cockpit_;
 249
 250 SCALAR          Simtime;
 251
 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
 259
 260 /* global variables */
 261
 262 char    *progname;
 263 char    *fullname;
 264
 265 /* file variables - default simulation settings */
 266
 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";
 273
 274
 275
 276 void ls_stamp()
 277 {
 278     char rcsid[] = "$Id$";
 279     char revid[] = "$Revision$";
 280     char dateid[] = "$Date$";
 281     struct tm *nowtime;
 282     time_t nowtime_t;
 283     long date;
 284
 285     /* report version of LaRCsim*/
 286     printf("\nLaRCsim %s, %s\n\n", revid, dateid);
 287
 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 */
 297
 298     return;
 299 }
 300
 301 void ls_setdefopts()
 302 {
 303     /* set default values for most options */
 304
 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;
 322
 323     speedup = 1.0;
 324 }
 325
 326
 327 /* return result codes from ls_checkopts */
 328
 329 #define OPT_OK 0
 330 #define OPT_ERR 1
 331
 332 extern char *optarg;
 333 extern int optind;
 334
 335 int ls_checkopts(argc, argv)    /* check and set options flags */
 336   int argc;
 337   char *argv[];
 338   {
 339     int c;
 340     int opt_err = 0;
 341     int mod_end_time = 0;
 342     int mod_buf_size = 0;
 343     float buffer_time, data_rate;
 344
 345     /* set default values */
 346
 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;
 350
 351     while ((c = getopt(argc, argv, "Aa:b:de:f:hi:kmo:r:s:t:x:")) != EOF)
 352         switch (c) {
 353             case 'A':
 354                 if (sim_control_.sim_type == GLmouse)
 355                   {
 356                     fprintf(stderr, "Cannot specify both keyboard (k) and ACES (A) cockpits option\n");
 357                     fprintf(stderr, "Keyboard operation assumed.\n");
 358                     break;
 359                   }
 360                 sim_control_.sim_type = cockpit;
 361                 break;
 362             case 'a':
 363                 sim_control_.write_av = 1;
 364                 if (optarg != NULL)
 365                 if (*optarg != '-')
 366                     strncpy(fltname, optarg, MAX_FILE_NAME_LENGTH);
 367                 else
 368                     optind--;
 369                 break;
 370             case 'b':
 371                 buffer_time = atof(optarg);
 372                 if (buffer_time <= 0.) opt_err = -1;
 373                 mod_buf_size++;
 374                 break;
 375             case 'd':
 376                 sim_control_.debug = 1;
 377                 break;
 378             case 'e':
 379                 sim_control_.end_time = atof(optarg);
 380                 mod_end_time++;
 381                 break;
 382             case 'f':
 383                 sim_control_.model_hz = atof(optarg);
 384                 break;
 385             case 'h':
 386                 opt_err = 1;
 387                 break;
 388             case 'i':
 389                 /* ls_get_settings( optarg ); */
 390                 break;
 391             case 'k':
 392                 sim_control_.sim_type = GLmouse;
 393                 break;
 394             case 'm':
 395                 sim_control_.vision = 1;
 396                 break;
 397             case 'o':
 398                 sim_control_.term_update_hz = atof(optarg);
 399                 if (sim_control_.term_update_hz <= 0.) opt_err = 1;
 400                 break;
 401             case 'r':
 402                 sim_control_.write_mat = 1;
 403                 if (optarg != NULL)
 404                 if (*optarg != '-')
 405                     strncpy(matname, optarg, MAX_FILE_NAME_LENGTH);
 406                 else
 407                     optind--;
 408                 break;
 409             case 's':
 410                 data_rate = atof(optarg);
 411                 if (data_rate <= 0.) opt_err = -1;
 412                 break;
 413             case 't':
 414                 sim_control_.write_tab = 1;
 415                 if (optarg != NULL)
 416                 if (*optarg != '-')
 417                     strncpy(tabname, optarg, MAX_FILE_NAME_LENGTH);
 418                 else
 419                     optind--;
 420                 break;
 421             case 'x':
 422                 sim_control_.write_asc1 = 1;
 423                 if (optarg != NULL)
 424                 if (*optarg != '-')
 425                     strncpy(asc1name, optarg, MAX_FILE_NAME_LENGTH);
 426                 else
 427                     optind--;
 428                 break;
 429             default:
 430                 opt_err = 1;
 431
 432         }
 433
 434     if (opt_err)
 435       {
 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",
 452                                                 data_rate);
 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");
 467         return OPT_ERR;
 468       }
 469
 470 /* calculate additional controls */
 471
 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;
 474
 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;
 478
 479     return OPT_OK;
 480   }
 481
 482
 483 void ls_loop( dt, initialize )
 484
 485 SCALAR dt;
 486 int initialize;
 487
 488 {
 489     /* printf ("  In ls_loop()\n"); */
 490     ls_step( dt, initialize );
 491     /* if (sim_control_.sim_type == cockpit ) ls_ACES();  */
 492     ls_aux();
 493     ls_model( dt, initialize );
 494     ls_accel();
 495 }
 496
 497
 498
 499 int ls_cockpit() {
 500
 501     sim_control_.paused = 0;
 502
 503     Throttle_pct = 0.85;
 504
 505     /* printf("Mach = %.2f  ", Mach_number);
 506     printf("%.4f,%.4f,%.2f  ", Latitude, Longitude, Altitude);
 507     printf("%.2f,%.2f,%.2f\n", Phi, Theta, Psi); */
 508
 509 }
 510
 511
 512 /* Initialize the LaRCsim flight model, dt is the time increment for
 513    each subsequent iteration through the EOM */
 514 int fgLaRCsimInit(double dt) {
 515
 516     model_dt = dt;
 517
 518     ls_setdefopts();            /* set default options */
 519
 520     /* Number_of_Continuous_States = 22; */
 521
 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;
 543
 544     ls_stamp();   /* ID stamp; record time and date of run */
 545
 546     if (speedup == 0.0) {
 547         fprintf(stderr, "%s: Cannot run with speedup of 0.\n", progname);
 548         return 1;
 549     }
 550
 551     ls_init();
 552
 553     if (speedup > 0) {
 554         /* Initialize (get) cockpit (controls) settings */
 555         ls_cockpit();
 556     }
 557
 558     return(1);
 559 }
 560
 561
 562 /* Run an iteration of the EOM (equations of motion) */
 563 int fgLaRCsimUpdate(int multiloop) {
 564     int i;
 565
 566     if (speedup > 0) {
 567         ls_cockpit();
 568     }
 569
 570     for ( i = 0; i < multiloop; i++ ) {
 571         ls_loop( model_dt, 0);
 572     }
 573
 574     return(1);
 575 }
 576
 577
 578 /* Flight Gear Modification Log
 579  *
 580  * $Log$
 581  * Revision 1.1  1997/05/29 00:09:57  curt
 582  * Initial Flight Gear revision.
 583  *
 584  */