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.4  1998/01/19 18:40:26  curt
  38 Tons of little changes to clean up the code and to remove fatal errors
  39 when building with the c++ compiler.
  40
  41 Revision 1.3  1998/01/05 22:19:25  curt
  42 #ifdef'd out some unused code that was problematic for MSVC++ to compile.
  43
  44 Revision 1.2  1997/05/29 22:39:58  curt
  45 Working on incorporating the LaRCsim flight model.
  46
  47 Revision 1.1  1997/05/29 00:09:57  curt
  48 Initial Flight Gear revision.
  49
  50  * Revision 1.4  1995/03/15  12:15:23  bjax
  51  * Added ls_init_get_set() and ls_init_put_set() and ls_init_init()
  52  * routines. EBJ
  53  *
  54  * Revision 1.3  1994/01/11  19:09:44  bjax
  55  * Fixed header includes.
  56  *
  57  * Revision 1.2  1992/12/30  14:04:53  bjax
  58  * Added call to ls_step(0, 1).
  59  *
  60  * Revision 1.1  92/12/30  14:02:19  bjax
  61  * Initial revision
  62  *
  63  * Revision 1.1  92/12/30  13:21:21  bjax
  64  * Initial revision
  65  *
  66  * Revision 1.3  93/12/31  10:34:11  bjax
  67  * Added $Log marker as well.
  68  *
  69
  70 ----------------------------------------------------------------------------
  71
  72         REFERENCES:
  73
  74 ----------------------------------------------------------------------------
  75
  76         CALLED BY:
  77
  78 ----------------------------------------------------------------------------
  79
  80         CALLS TO:
  81
  82 ----------------------------------------------------------------------------
  83
  84         INPUTS:
  85
  86 ----------------------------------------------------------------------------
  87
  88         OUTPUTS:
  89
  90 --------------------------------------------------------------------------*/
  91 static char rcsid[] = "$Id$";
  92
  93 #include <string.h>
  94 #include <stdio.h>
  95 #include "ls_types.h"
  96 #include "ls_sym.h"
  97 #include "ls_step.h"
  98 #include "ls_init.h"
  99 #include "navion_init.h"
 100
 101 /* temp */
 102 #include "ls_generic.h"
 103
 104 #define MAX_NUMBER_OF_CONTINUOUS_STATES 100
 105 #define MAX_NUMBER_OF_DISCRETE_STATES  20
 106 #define HARDWIRED 13
 107 #define NIL_POINTER 0L
 108
 109 #define FACILITY_NAME_STRING "init"
 110 #define CURRENT_VERSION 10
 111
 112 typedef struct
 113 {
 114     symbol_rec  Symbol;
 115     double      value;
 116 } cont_state_rec;
 117
 118 typedef struct
 119 {
 120     symbol_rec  Symbol;
 121     long        value;
 122 } disc_state_rec;
 123
 124
 125 extern SCALAR Simtime;
 126
 127 /* static int Symbols_loaded = 0; */
 128 static int Number_of_Continuous_States = 0;
 129 static int Number_of_Discrete_States = 0;
 130 static cont_state_rec   Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ];
 131 static disc_state_rec   Discrete_States[  MAX_NUMBER_OF_DISCRETE_STATES ];
 132
 133
 134 void ls_init_init( void ) {
 135     int i;
 136     /* int error; */
 137
 138     if (Number_of_Continuous_States == 0)
 139         {
 140             Number_of_Continuous_States = HARDWIRED;
 141
 142             for (i=0;i<HARDWIRED;i++)
 143                 strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
 144
 145             strcpy( Continuous_States[ 0].Symbol.Par_Name,
 146                     "generic_.geodetic_position_v[0]");
 147             strcpy( Continuous_States[ 1].Symbol.Par_Name,
 148                     "generic_.geodetic_position_v[1]");
 149             strcpy( Continuous_States[ 2].Symbol.Par_Name,
 150                     "generic_.geodetic_position_v[2]");
 151             strcpy( Continuous_States[ 3].Symbol.Par_Name,
 152                     "generic_.v_local_v[0]");
 153             strcpy( Continuous_States[ 4].Symbol.Par_Name,
 154                     "generic_.v_local_v[1]");
 155             strcpy( Continuous_States[ 5].Symbol.Par_Name,
 156                     "generic_.v_local_v[2]");
 157             strcpy( Continuous_States[ 6].Symbol.Par_Name,
 158                     "generic_.euler_angles_v[0]");
 159             strcpy( Continuous_States[ 7].Symbol.Par_Name,
 160                     "generic_.euler_angles_v[1]");
 161             strcpy( Continuous_States[ 8].Symbol.Par_Name,
 162                     "generic_.euler_angles_v[2]");
 163             strcpy( Continuous_States[ 9].Symbol.Par_Name,
 164                     "generic_.omega_body_v[0]");
 165             strcpy( Continuous_States[10].Symbol.Par_Name,
 166                     "generic_.omega_body_v[1]");
 167             strcpy( Continuous_States[11].Symbol.Par_Name,
 168                     "generic_.omega_body_v[2]");
 169             strcpy( Continuous_States[12].Symbol.Par_Name,
 170                     "generic_.earth_position_angle");
 171         }
 172
 173     /* commented out by CLO
 174     for (i=0;i<Number_of_Continuous_States;i++)
 175         {
 176             (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
 177             if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
 178         }
 179
 180     for (i=0;i<Number_of_Discrete_States;i++)
 181         {
 182             (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
 183             if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
 184         }
 185     */
 186 }
 187
 188 void ls_init( void ) {
 189     /* int i; */
 190
 191     Simtime = 0;
 192
 193     printf("LS in init() pos = %.2f\n", Latitude);
 194
 195     ls_init_init();
 196
 197     printf("LS after init_init() pos = %.2f\n", Latitude);
 198     /* move the states to proper values */
 199
 200     /* commented out by CLO
 201     for(i=0;i<Number_of_Continuous_States;i++)
 202         if (Continuous_States[i].Symbol.Addr)
 203             ls_set_sym_val( &Continuous_States[i].Symbol,
 204                             Continuous_States[i].value );
 205
 206     for(i=0;i<Number_of_Discrete_States;i++)
 207         if (Discrete_States[i].Symbol.Addr)
 208             ls_set_sym_val( &Discrete_States[i].Symbol,
 209                             (double) Discrete_States[i].value );
 210     */
 211
 212     model_init();
 213
 214     printf("LS after model_init() pos = %.2f\n", Latitude);
 215
 216     ls_step(0.0, -1);
 217
 218     printf("LS after ls_step() pos = %.2f\n", Latitude);
 219 }
 220
 221
 222 #ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
 223
 224 char *ls_init_get_set(char *buffer, char *eob)
 225 /* This routine parses the settings file for "init" entries. */
 226 {
 227
 228     static char *fac_name = FACILITY_NAME_STRING;
 229     char *bufptr;
 230     char line[256];
 231     int n, ver, i, error, abrt;
 232
 233     enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
 234
 235     abrt = 0;
 236     looking_for = cont_states_header;
 237
 238     n = sscanf(buffer, "%s", line);
 239     if (n == 0) return 0L;
 240     if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
 241
 242     bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
 243     if (bufptr == 0) return 0L;
 244
 245     sscanf( bufptr, "%d", &ver );
 246     if (ver != CURRENT_VERSION) return 0L;
 247
 248     ls_init_init();
 249
 250     while( !abrt && (eob > bufptr))
 251       {
 252         bufptr = strtok( 0L, "\n");
 253         if (bufptr == 0) return 0L;
 254         if (strncasecmp( bufptr, "end", 3) == 0) break;
 255
 256         sscanf( bufptr, "%s", line );
 257         if (line[0] != '#') /* ignore comments */
 258             {
 259                 switch (looking_for)
 260                     {
 261                     case cont_states_header:
 262                         {
 263                             if (strncasecmp( line, "continuous_states", 17) == 0)
 264                                 {
 265                                     n = sscanf( bufptr, "%s%d", line,
 266                                                 &Number_of_Continuous_States );
 267                                     if (n != 2) abrt = 1;
 268                                     looking_for = cont_states;
 269                                     i = 0;
 270                                 }
 271                             break;
 272                         }
 273                     case cont_states:
 274                         {
 275                             n = sscanf( bufptr, "%s%s%le",
 276                                         Continuous_States[i].Symbol.Mod_Name,
 277                                         Continuous_States[i].Symbol.Par_Name,
 278                                         &Continuous_States[i].value );
 279                             if (n != 3) abrt = 1;
 280                             Continuous_States[i].Symbol.Addr = NIL_POINTER;
 281                             i++;
 282                             if (i >= Number_of_Continuous_States)
 283                                 looking_for = disc_states_header;
 284                             break;
 285                         }
 286                     case disc_states_header:
 287                         {
 288                             if (strncasecmp( line, "discrete_states", 15) == 0)
 289                                 {
 290                                     n = sscanf( bufptr, "%s%d", line,
 291                                                 &Number_of_Discrete_States );
 292                                     if (n != 2) abrt = 1;
 293                                     looking_for = disc_states;
 294                                     i = 0;
 295                                 }
 296                             break;
 297                         }
 298                     case disc_states:
 299                         {
 300                             n = sscanf( bufptr, "%s%s%ld",
 301                                         Discrete_States[i].Symbol.Mod_Name,
 302                                         Discrete_States[i].Symbol.Par_Name,
 303                                         &Discrete_States[i].value );
 304                             if (n != 3) abrt = 1;
 305                             Discrete_States[i].Symbol.Addr = NIL_POINTER;
 306                             i++;
 307                             if (i >= Number_of_Discrete_States) looking_for = done;
 308                         }
 309                     case done:
 310                         {
 311                             break;
 312                         }
 313                     }
 314
 315             }
 316       }
 317
 318     Symbols_loaded = !abrt;
 319
 320     return bufptr;
 321 }
 322 #endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
 323
 324
 325 void ls_init_put_set( FILE *fp )
 326 {
 327     int i;
 328
 329     if (fp==0) return;
 330     fprintf(fp, "\n");
 331     fprintf(fp, "#==============================  %s\n", FACILITY_NAME_STRING);
 332     fprintf(fp, "\n");
 333     fprintf(fp, FACILITY_NAME_STRING);
 334     fprintf(fp, "\n");
 335     fprintf(fp, "%04d\n", CURRENT_VERSION);
 336     fprintf(fp, "  continuous_states: %d\n", Number_of_Continuous_States);
 337     fprintf(fp, "#    module    parameter   value\n");
 338     for (i=0; i<Number_of_Continuous_States; i++)
 339         fprintf(fp, "    %s\t%s\t%E\n",
 340                 Continuous_States[i].Symbol.Mod_Name,
 341                 Continuous_States[i].Symbol.Par_Name,
 342                 Continuous_States[i].value );
 343
 344     fprintf(fp, "  discrete_states: %d\n", Number_of_Discrete_States);
 345     fprintf(fp, "#    module    parameter   value\n");
 346     for (i=0;i<Number_of_Discrete_States;i++)
 347         fprintf(fp, "    %s\t%s\t%ld\n",
 348                 Discrete_States[i].Symbol.Mod_Name,
 349                 Discrete_States[i].Symbol.Par_Name,
 350                 Discrete_States[i].value );
 351     fprintf(fp, "end\n");
 352     return;
 353 }
 354
 355 void ls_save_current_as_ic( void ) {
 356     /* Save current states as initial conditions */
 357
 358     /* int i, error; */
 359
 360     /* commented out by CLO
 361     for(i=0;i<Number_of_Continuous_States;i++)
 362         if (Continuous_States[i].Symbol.Addr)
 363             Continuous_States[i].value =
 364                 ls_get_sym_val( &Continuous_States[i].Symbol, &error );
 365
 366     for(i=0;i<Number_of_Discrete_States;i++)
 367         if (Discrete_States[i].Symbol.Addr)
 368             Discrete_States[i].value = (long)
 369                 ls_get_sym_val( &Discrete_States[i].Symbol, &error );
 370     */
 371 }