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