src/FDM/LaRCsim/ls_init.c

   1 /***************************************************************************
   2
   3         TITLE:  ls_init.c
   4
   5 ----------------------------------------------------------------------------
   6
   7         FUNCTION:       Initializes simulation
   8
   9 ----------------------------------------------------------------------------
  10
  11         MODULE STATUS:  incomplete
  12
  13 ----------------------------------------------------------------------------
  14
  15         GENEALOGY:      Written 921230 by Bruce Jackson
  16
  17 ----------------------------------------------------------------------------
  18
  19         DESIGNED BY:    EBJ
  20
  21         CODED BY:       EBJ
  22
  23         MAINTAINED BY:  EBJ
  24
  25 ----------------------------------------------------------------------------
  26
  27         MODIFICATION HISTORY:
  28
  29         DATE    PURPOSE                                         BY
  30
  31         950314  Added get_set, put_set, and init routines.      EBJ
  32
  33         CURRENT RCS HEADER:
  34
  35 $Header$
  36 $Log$
  37 Revision 1.2  2003/07/25 17:53:35  mselig
  38 UIUC code initilization mods to tidy things up a bit.
  39
  40 Revision 1.1.1.1  2002/09/10 01:14:02  curt
  41 Initial revision of FlightGear-0.9.0
  42
  43 Revision 1.3  2000/05/24 04:10:01  curt
  44 MSVC5 portability changes contributed by Bruce Finney.
  45
  46 Revision 1.2  2000/04/10 18:09:41  curt
  47 David Megginson made a few (mostly minor) mods to the LaRCsim files, and
  48 it's now possible to choose the LaRCsim model at runtime, as in
  49
  50   fgfs --aircraft=c172
  51
  52 or
  53
  54   fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
  55
  56 I did this so that I could play with the UIUC stuff without losing
  57 Tony's C172 with its flaps, etc.  I did my best to respect the design
  58 of the LaRCsim code by staying in C, making only minimal changes, and
  59 not introducing any dependencies on the rest of FlightGear.  The
  60 modified files are attached.
  61
  62 Revision 1.1.1.1  1999/06/17 18:07:34  curt
  63 Start of 0.7.x branch
  64
  65 Revision 1.1.1.1  1999/04/05 21:32:45  curt
  66 Start of 0.6.x branch.
  67
  68 Revision 1.5  1998/07/12 03:11:03  curt
  69 Removed some printf()'s.
  70 Fixed the autopilot integration so it should be able to update it's control
  71   positions every time the internal flight model loop is run, and not just
  72   once per rendered frame.
  73 Added a routine to do the necessary stuff to force an arbitrary altitude
  74   change.
  75 Gave the Navion engine just a tad more power.
  76
  77 Revision 1.4  1998/01/19 18:40:26  curt
  78 Tons of little changes to clean up the code and to remove fatal errors
  79 when building with the c++ compiler.
  80
  81 Revision 1.3  1998/01/05 22:19:25  curt
  82 #ifdef'd out some unused code that was problematic for MSVC++ to compile.
  83
  84 Revision 1.2  1997/05/29 22:39:58  curt
  85 Working on incorporating the LaRCsim flight model.
  86
  87 Revision 1.1  1997/05/29 00:09:57  curt
  88 Initial Flight Gear revision.
  89
  90  * Revision 1.4  1995/03/15  12:15:23  bjax
  91  * Added ls_init_get_set() and ls_init_put_set() and ls_init_init()
  92  * routines. EBJ
  93  *
  94  * Revision 1.3  1994/01/11  19:09:44  bjax
  95  * Fixed header includes.
  96  *
  97  * Revision 1.2  1992/12/30  14:04:53  bjax
  98  * Added call to ls_step(0, 1).
  99  *
 100  * Revision 1.1  92/12/30  14:02:19  bjax
 101  * Initial revision
 102  *
 103  * Revision 1.1  92/12/30  13:21:21  bjax
 104  * Initial revision
 105  *
 106  * Revision 1.3  93/12/31  10:34:11  bjax
 107  * Added $Log marker as well.
 108  *
 109
 110 ----------------------------------------------------------------------------
 111
 112         REFERENCES:
 113
 114 ----------------------------------------------------------------------------
 115
 116         CALLED BY:
 117
 118 ----------------------------------------------------------------------------
 119
 120         CALLS TO:
 121
 122 ----------------------------------------------------------------------------
 123
 124         INPUTS:
 125
 126 ----------------------------------------------------------------------------
 127
 128         OUTPUTS:
 129
 130 --------------------------------------------------------------------------*/
 131 static char rcsid[] = "$Id$";
 132
 133 #include <string.h>
 134 #include <stdio.h>
 135 #include "ls_types.h"
 136 #include "ls_sym.h"
 137 #include "ls_step.h"
 138 #include "ls_init.h"
 139 #include "navion_init.h"
 140 #include "ls_model.h"
 141
 142 /* temp */
 143 #include "ls_generic.h"
 144
 145 #define MAX_NUMBER_OF_CONTINUOUS_STATES 100
 146 #define MAX_NUMBER_OF_DISCRETE_STATES  20
 147 #define HARDWIRED 13
 148 #define NIL_POINTER 0L
 149
 150 #define FACILITY_NAME_STRING "init"
 151 #define CURRENT_VERSION 10
 152
 153 void cherokee_init( void );
 154 void c172_init( void );
 155 void basic_init( void );
 156
 157 typedef struct
 158 {
 159     symbol_rec  Symbol;
 160     double      value;
 161 } cont_state_rec;
 162
 163 typedef struct
 164 {
 165     symbol_rec  Symbol;
 166     long        value;
 167 } disc_state_rec;
 168
 169
 170 extern SCALAR Simtime;
 171
 172 /* static int Symbols_loaded = 0; */
 173 static int Number_of_Continuous_States = 0;
 174 static int Number_of_Discrete_States = 0;
 175 static cont_state_rec   Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ];
 176 static disc_state_rec   Discrete_States[  MAX_NUMBER_OF_DISCRETE_STATES ];
 177
 178
 179 void ls_init_init( void ) {
 180     int i;
 181     /* int error; */
 182
 183     if (Number_of_Continuous_States == 0)
 184         {
 185             Number_of_Continuous_States = HARDWIRED;
 186
 187             for (i=0;i<HARDWIRED;i++)
 188                 strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
 189
 190             strcpy( Continuous_States[ 0].Symbol.Par_Name,
 191                     "generic_.geodetic_position_v[0]");
 192             strcpy( Continuous_States[ 1].Symbol.Par_Name,
 193                     "generic_.geodetic_position_v[1]");
 194             strcpy( Continuous_States[ 2].Symbol.Par_Name,
 195                     "generic_.geodetic_position_v[2]");
 196             strcpy( Continuous_States[ 3].Symbol.Par_Name,
 197                     "generic_.v_local_v[0]");
 198             strcpy( Continuous_States[ 4].Symbol.Par_Name,
 199                     "generic_.v_local_v[1]");
 200             strcpy( Continuous_States[ 5].Symbol.Par_Name,
 201                     "generic_.v_local_v[2]");
 202             strcpy( Continuous_States[ 6].Symbol.Par_Name,
 203                     "generic_.euler_angles_v[0]");
 204             strcpy( Continuous_States[ 7].Symbol.Par_Name,
 205                     "generic_.euler_angles_v[1]");
 206             strcpy( Continuous_States[ 8].Symbol.Par_Name,
 207                     "generic_.euler_angles_v[2]");
 208             strcpy( Continuous_States[ 9].Symbol.Par_Name,
 209                     "generic_.omega_body_v[0]");
 210             strcpy( Continuous_States[10].Symbol.Par_Name,
 211                     "generic_.omega_body_v[1]");
 212             strcpy( Continuous_States[11].Symbol.Par_Name,
 213                     "generic_.omega_body_v[2]");
 214             strcpy( Continuous_States[12].Symbol.Par_Name,
 215                     "generic_.earth_position_angle");
 216         }
 217
 218     /* commented out by CLO
 219     for (i=0;i<Number_of_Continuous_States;i++)
 220         {
 221             (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
 222             if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
 223         }
 224
 225     for (i=0;i<Number_of_Discrete_States;i++)
 226         {
 227             (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
 228             if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
 229         }
 230     */
 231
 232 }
 233 /* not all system have strcasecmp */
 234 #ifdef _MSC_VER
 235 #  define strcasecmp stricmp
 236 #endif
 237
 238 void ls_init( char * aircraft ) {
 239     /* int i; */
 240
 241     Simtime = 0;
 242
 243     if (!strcasecmp(aircraft, "c172")) {
 244       printf("Initializing LaRCsim for C172\n");
 245       current_model = C172;
 246     } else if (!strcasecmp(aircraft, "navion")) {
 247       printf("Initializing LaRCsim for Navion\n");
 248       current_model = NAVION;
 249     } else if (!strcasecmp(aircraft, "cherokee")) {
 250       printf("Initializing LaRCsim for Cherokee\n");
 251       current_model = CHEROKEE;
 252     } else if (!strcasecmp(aircraft, "basic")) {
 253       printf("Initializing LaRCsim for Basic\n");
 254       current_model = BASIC;
 255     } else if (!strcasecmp(aircraft, "uiuc")) {
 256       printf("Initializing LaRCsim for UIUC models\n");
 257       current_model = UIUC;
 258     } else {
 259       printf("Unknown LaRCsim aircraft: %s; defaulting to C172\n", aircraft);
 260       current_model = C172;
 261     }
 262
 263     /* printf("LS in init() pos = %.2f\n", Latitude); */
 264
 265     ls_init_init();
 266
 267     /* printf("LS after init_init() pos = %.2f\n", Latitude); */
 268
 269     /* move the states to proper values */
 270
 271     /* commented out by CLO
 272     for(i=0;i<Number_of_Continuous_States;i++)
 273         if (Continuous_States[i].Symbol.Addr)
 274             ls_set_sym_val( &Continuous_States[i].Symbol,
 275                             Continuous_States[i].value );
 276
 277     for(i=0;i<Number_of_Discrete_States;i++)
 278         if (Discrete_States[i].Symbol.Addr)
 279             ls_set_sym_val( &Discrete_States[i].Symbol,
 280                             (double) Discrete_States[i].value );
 281     */
 282
 283     switch (current_model) {
 284     case NAVION:
 285       navion_init();
 286       break;
 287     case C172:
 288       c172_init();
 289       break;
 290     case CHEROKEE:
 291       cherokee_init();
 292       break;
 293     case BASIC:
 294       basic_init();
 295       break;
 296     case UIUC:
 297       c172_init();
 298       break;
 299     }
 300
 301     /* printf("LS after model_init() pos = %.2f\n", Latitude); */
 302
 303     ls_step(0.0, -1);
 304
 305     /* printf("LS after ls_step() pos = %.2f\n", Latitude); */
 306 }
 307
 308
 309 #ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
 310
 311 char *ls_init_get_set(char *buffer, char *eob)
 312 /* This routine parses the settings file for "init" entries. */
 313 {
 314
 315     static char *fac_name = FACILITY_NAME_STRING;
 316     char *bufptr;
 317     char line[256];
 318     int n, ver, i, error, abrt;
 319
 320     enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
 321
 322     abrt = 0;
 323     looking_for = cont_states_header;
 324
 325     n = sscanf(buffer, "%s", line);
 326     if (n == 0) return 0L;
 327     if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
 328
 329     bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
 330     if (bufptr == 0) return 0L;
 331
 332     sscanf( bufptr, "%d", &ver );
 333     if (ver != CURRENT_VERSION) return 0L;
 334
 335     ls_init_init();
 336
 337     while( !abrt && (eob > bufptr))
 338       {
 339         bufptr = strtok( 0L, "\n");
 340         if (bufptr == 0) return 0L;
 341         if (strncasecmp( bufptr, "end", 3) == 0) break;
 342
 343         sscanf( bufptr, "%s", line );
 344         if (line[0] != '#') /* ignore comments */
 345             {
 346                 switch (looking_for)
 347                     {
 348                     case cont_states_header:
 349                         {
 350                             if (strncasecmp( line, "continuous_states", 17) == 0)
 351                                 {
 352                                     n = sscanf( bufptr, "%s%d", line,
 353                                                 &Number_of_Continuous_States );
 354                                     if (n != 2) abrt = 1;
 355                                     looking_for = cont_states;
 356                                     i = 0;
 357                                 }
 358                             break;
 359                         }
 360                     case cont_states:
 361                         {
 362                             n = sscanf( bufptr, "%s%s%le",
 363                                         Continuous_States[i].Symbol.Mod_Name,
 364                                         Continuous_States[i].Symbol.Par_Name,
 365                                         &Continuous_States[i].value );
 366                             if (n != 3) abrt = 1;
 367                             Continuous_States[i].Symbol.Addr = NIL_POINTER;
 368                             i++;
 369                             if (i >= Number_of_Continuous_States)
 370                                 looking_for = disc_states_header;
 371                             break;
 372                         }
 373                     case disc_states_header:
 374                         {
 375                             if (strncasecmp( line, "discrete_states", 15) == 0)
 376                                 {
 377                                     n = sscanf( bufptr, "%s%d", line,
 378                                                 &Number_of_Discrete_States );
 379                                     if (n != 2) abrt = 1;
 380                                     looking_for = disc_states;
 381                                     i = 0;
 382                                 }
 383                             break;
 384                         }
 385                     case disc_states:
 386                         {
 387                             n = sscanf( bufptr, "%s%s%ld",
 388                                         Discrete_States[i].Symbol.Mod_Name,
 389                                         Discrete_States[i].Symbol.Par_Name,
 390                                         &Discrete_States[i].value );
 391                             if (n != 3) abrt = 1;
 392                             Discrete_States[i].Symbol.Addr = NIL_POINTER;
 393                             i++;
 394                             if (i >= Number_of_Discrete_States) looking_for = done;
 395                         }
 396                     case done:
 397                         {
 398                             break;
 399                         }
 400                     }
 401
 402             }
 403       }
 404
 405     Symbols_loaded = !abrt;
 406
 407     return bufptr;
 408 }
 409 #endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
 410
 411
 412 void ls_init_put_set( FILE *fp )
 413 {
 414     int i;
 415
 416     if (fp==0) return;
 417     fprintf(fp, "\n");
 418     fprintf(fp, "#==============================  %s\n", FACILITY_NAME_STRING);
 419     fprintf(fp, "\n");
 420     fprintf(fp, FACILITY_NAME_STRING);
 421     fprintf(fp, "\n");
 422     fprintf(fp, "%04d\n", CURRENT_VERSION);
 423     fprintf(fp, "  continuous_states: %d\n", Number_of_Continuous_States);
 424     fprintf(fp, "#    module    parameter   value\n");
 425     for (i=0; i<Number_of_Continuous_States; i++)
 426         fprintf(fp, "    %s\t%s\t%E\n",
 427                 Continuous_States[i].Symbol.Mod_Name,
 428                 Continuous_States[i].Symbol.Par_Name,
 429                 Continuous_States[i].value );
 430
 431     fprintf(fp, "  discrete_states: %d\n", Number_of_Discrete_States);
 432     fprintf(fp, "#    module    parameter   value\n");
 433     for (i=0;i<Number_of_Discrete_States;i++)
 434         fprintf(fp, "    %s\t%s\t%ld\n",
 435                 Discrete_States[i].Symbol.Mod_Name,
 436                 Discrete_States[i].Symbol.Par_Name,
 437                 Discrete_States[i].value );
 438     fprintf(fp, "end\n");
 439     return;
 440 }
 441
 442 void ls_save_current_as_ic( void ) {
 443     /* Save current states as initial conditions */
 444
 445     /* int i, error; */
 446
 447     /* commented out by CLO
 448     for(i=0;i<Number_of_Continuous_States;i++)
 449         if (Continuous_States[i].Symbol.Addr)
 450             Continuous_States[i].value =
 451                 ls_get_sym_val( &Continuous_States[i].Symbol, &error );
 452
 453     for(i=0;i<Number_of_Discrete_States;i++)
 454         if (Discrete_States[i].Symbol.Addr)
 455             Discrete_States[i].value = (long)
 456                 ls_get_sym_val( &Discrete_States[i].Symbol, &error );
 457     */
 458 }