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