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